Compare commits

..

94 Commits

Author SHA1 Message Date
2c4b1dbbe5 cleanup 2024-08-19 01:27:16 +01:00
9a486ad037 makefile change 2024-04-25 22:34:17 +01:00
a53de34b3c whitespace 2024-01-12 20:50:48 +00:00
13a281b2e2 stm32 interface example 2024-01-12 20:45:02 +00:00
9c9f152e87 README 2024-01-08 19:44:36 +00:00
7466b91fe3 self test 2024-01-08 06:16:29 +00:00
898a0f7ee2 Filter example + README 2024-01-07 18:11:24 +00:00
f5e6e346cf update filter mode 2024-01-07 17:54:56 +00:00
33c6e45db5 README 2024-01-07 17:31:20 +00:00
4a001cf54a More information, sleep-to-wake, and re-work FIFO examples 2024-01-07 17:19:46 +00:00
79001b810d non-fifo reading examples 2024-01-07 08:13:15 +00:00
ba64cc64ae fix comments 2024-01-07 07:57:46 +00:00
5446d28c13 FIFO example 2024-01-07 07:55:06 +00:00
14a051b84b assert 2024-01-07 01:23:33 +00:00
f8092b7405 cleanup 2024-01-06 23:53:38 +00:00
ce05aa49e5 FIFO README 2024-01-06 23:44:38 +00:00
d0aa9e1137 FIFO modes being worked on 2024-01-06 23:28:58 +00:00
06ae577bc1 wrong const in lis3dh_read_adc() 2024-01-06 20:44:11 +00:00
91ada989f6 README 2024-01-06 17:53:39 +00:00
4dcc5b109d cleanup comments 2024-01-06 17:49:36 +00:00
54a0bdb7d8 README 2024-01-06 17:25:44 +00:00
c484947cdf change tab to 4s 2024-01-06 17:19:24 +00:00
8648c517f6 rework poll/read and amend examples 2024-01-06 16:51:22 +00:00
0a0cf2a7aa fix reg order and longer sleep after configure(). Amend notes on usage 2024-01-06 01:36:35 +00:00
485922d898 SPI working! when using SPI, the order of writing out ctrl_reg's matters....but seemingly not when using i2c 2024-01-06 00:52:37 +00:00
f6020e9d15 fth -> size; make user config more consistent 2024-01-05 20:32:03 +00:00
cc448c54b0 amend examples to use i16 accel data 2024-01-04 21:13:49 +00:00
89e9d9310d accel readings now i16 (1lsb per 1mg) instead of float 2024-01-04 20:20:51 +00:00
bd76380a65 adc/temp integer 2024-01-04 19:54:13 +00:00
3b3b00a9f8 working on SPI 2024-01-04 14:37:44 +00:00
112f59510b 4D/6D detection 2024-01-03 16:48:22 +00:00
574dd1a0fc remove sleep from examples 2024-01-03 16:02:42 +00:00
da27083859 README 2024-01-03 15:41:58 +00:00
bce3d7f8f0 free-fall 2024-01-02 16:17:09 +00:00
b0e8428519 examples: possibly helpful comments 2024-01-02 15:35:35 +00:00
b86e97662a revised examples 2024-01-02 14:57:30 +00:00
1a29e35097 rewrite macros and user configurable REFERENCE 2024-01-02 14:42:49 +00:00
d25f5ab98e README 2024-01-02 11:08:48 +00:00
e9c37cfca7 I am stupid 2024-01-02 11:04:37 +00:00
96a2a8336a fixed old examples 2024-01-02 11:00:49 +00:00
b0528ff456 inertial wakeup 2024-01-02 10:56:58 +00:00
eb81b5225d README 2024-01-01 13:34:11 +00:00
804c415a13 adc and temp sense example 2024-01-01 13:27:55 +00:00
50a491a1be double click 2024-01-01 13:08:06 +00:00
78cde50f13 single-click detection & fixed examples 2024-01-01 12:05:53 +00:00
a96c0a9466 give linux some time to remove old sysfs gpio mapping 2024-01-01 12:05:17 +00:00
1a54ce4e5e single click detection 2024-01-01 11:05:45 +00:00
d1ab5a8d99 more src reg interaction 2024-01-01 10:15:57 +00:00
6352a65fec INT dur table 2023-12-31 22:31:22 +00:00
415dc2fe1c cleanup unused or unnecessary code 2023-12-31 15:47:22 +00:00
432bf4d421 temperature 2023-12-31 15:41:24 +00:00
70dd2c9a1b ADC 2023-12-31 15:24:40 +00:00
f64bf4a72d all registers 2023-12-31 14:14:12 +00:00
16b40de1e1 lis3dh_reset() and bit fiddle to prevent overwriting regs in case of user issue 2023-12-30 20:22:29 +00:00
cf48f7db54 INT1 and INT2 config 2023-12-30 16:23:25 +00:00
87dd035d9e int to int_pin rename to avoid confusion with int regs 2023-12-30 15:36:35 +00:00
5bde365c65 examples and more debug flags 2023-12-30 13:10:40 +00:00
365bb369bb memset() and comments 2023-12-29 23:42:15 +00:00
30508a8378 tidying and testing new functions 2023-12-29 23:24:15 +00:00
9c3f912d1a read REFERENCE 2023-12-29 18:19:11 +00:00
84e6fa4caa latch interrupts + fn to clear on device 2023-12-29 17:26:54 +00:00
c96f77127d README: shorten output example 2023-12-29 00:58:49 +00:00
4c4f858774 remove gpio unregister from quit() 2023-12-29 00:56:07 +00:00
7e8652c3b1 README: interrupt 2023-12-28 18:40:09 +00:00
b0907115c2 rename {pin1,pin2} => {int1,int2} 2023-12-28 18:15:25 +00:00
7ec153d397 interrupt generation 2023-12-28 18:10:34 +00:00
e1f2040e6c interface 2023-12-27 20:59:47 +00:00
a31f67d4a9 remove tabs 2023-12-27 19:59:29 +00:00
fd91ea1c73 cleanup and code examples in README 2023-12-27 19:54:47 +00:00
b513cbd5d0 graphs 2023-12-26 22:57:02 +00:00
d3b6c912dd data for graphs 2023-12-26 07:43:30 +00:00
ecf00e3584 FIFO is always 10-bit 2023-12-26 07:42:55 +00:00
b27722f466 README and removed weActTest.zip; it will be its own repo 2023-12-25 00:06:59 +00:00
robin
950852c151 weACT PCB
Using ic2 dev2 (C4:SDA & A8 SCL)
acelerometer being read.
2023-12-24 20:53:06 +00:00
c979a449cf Pin config for IRQ 2023-12-23 18:38:28 +00:00
eac1eaa041 ansi 2023-12-23 15:37:33 +00:00
48ee2a6932 filter default and fifo size check 2023-12-23 15:34:20 +00:00
5d3d1437f9 c99 + comments 2023-12-23 13:31:59 +00:00
11e6e65c6f change to float and remove unused h 2023-12-23 10:33:28 +00:00
e9472ed1f0 pass FIFO data 2023-12-23 10:28:43 +00:00
57740b2e92 graph noise 2023-12-23 10:05:49 +00:00
cc4e64016c test 2023-12-22 21:59:31 +00:00
d32394d724 change spec 2023-12-22 17:01:33 +00:00
a5b704fbfe filter: decently filter out DC/constant acceleration 2023-12-22 17:00:53 +00:00
9ae2d4ce01 fix: device would sometimes corrupt its own registers. 2023-12-22 16:25:25 +00:00
0dff0b92c3 FIFO 2023-12-22 10:22:43 +00:00
31bcd4d12f tidying 2023-12-22 08:26:10 +00:00
509bae290b working simple example 2023-12-21 23:29:22 +00:00
f3c07a5c8b i2c 2023-12-21 20:52:17 +00:00
1726159cb8 . 2023-12-21 18:17:20 +00:00
e25f035863 work on C implementation 2023-12-21 17:31:12 +00:00
db6afa4aaf reading accel data 2023-12-21 13:59:31 +00:00
5e2e85695c it finds device on i2c 2023-12-20 16:40:22 +00:00
effa278a05 useful documents 2023-12-19 11:06:03 +00:00
47 changed files with 3765 additions and 1 deletions

3
.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
lis3dh
.vscode/
*.o

29
Makefile Normal file
View File

@ -0,0 +1,29 @@
CC=gcc
CFLAGS = -g -O0 -Wall -Wextra -Wpedantic -Wshadow -Wformat=2 -Wfloat-equal \
-Wconversion -Wcast-align -Wpointer-arith -Wstrict-overflow=5 \
-Wwrite-strings -Wcast-qual -Wswitch-default -Wswitch-enum \
-Wunreachable-code -Wstrict-prototypes -Wmissing-prototypes \
-Wmissing-declarations -Wredundant-decls -Wold-style-definition \
-Winline -Wlogical-op -Wjump-misses-init -Wdouble-promotion \
-Wformat-overflow=2 -Wformat-signedness -Wmissing-include-dirs \
-Wnull-dereference -Wstack-usage=1024 -Wpacked -Woverlength-strings \
-Wvla -Walloc-zero -Wduplicated-cond -Wduplicated-branches -Wrestrict \
-fanalyzer \
-I. -std=c89
CFILES = $(wildcard *.c)
OBJECTS = $(CFILES:.c=.o)
BIN = lis3dh
all: $(BIN)
$(BIN): $(OBJECTS)
@echo ">>> $@"
@$(CC) $(CFLAGS) $^ -o $@
%.o:%.c
@echo "cc $<"
@$(CC) $(CFLAGS) -c $< -o $@
clean:
@rm -rf $(OBJECTS)

143
README.md
View File

@ -1,3 +1,144 @@
# LIS3DH
3-axis accelerometer
A C linux driver for the 3-axis accelerometer LIS3DH. Supports both I2C and SPI.
### Features
> - FIFO
> - HP filter
> - 2G, 4G, 8G and 16G
> - Low-power mode, normal mode and high-resolution mode
> - ADC and temperature sensing
> - Interrupt generation
> - Free-fall detection
> - Single-click detection
> - Double-click detection
> - 4D/6D orientation detection
> - Sleep-to-Wake/Return-to-Sleep
> - Self test
## Examples
See the `example/` dir for complete code examples and explanations of LIS3DH terminology
## Implementation
This driver requires the user to implement the following interface functions:
This project has example interface code for I2C and SPI used on Raspberry Pi 4 and STM32
```c
/* initialise the "interface" */
int init(void);
/* read from register `reg', `size' amount of bytes, and write them to `dst' */
int read(uint8_t reg, uint8_t *dst, uint32_t size);
/* write `value' to register `reg' */
int write(uint8_t reg, uint8_t value);
/* sleep for `dur_us' microseconds */
int sleep(uint32_t dur_us);
/* deinitalise the "interface" */
int deinit(void);
```
All above functions return `0` on success, and any non-zero value on error.
If `init` and/or `deinit` are set to `NULL`, they will be ignored. Useful on microcontrollers.
---
## Functions
```c
int lis3dh_init(lis3dh_t *lis3dh);
int lis3dh_deinit(lis3dh_t *lis3dh);
int lis3dh_configure(lis3dh_t *lis3dh);
int lis3dh_read(lis3dh_t *lis3dh);
int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo);
int lis3dh_read_int1(lis3dh_t *lis3dh);
int lis3dh_read_int2(lis3dh_t *lis3dh);
int lis3dh_read_click(lis3dh_t *lis3dh);
int lis3dh_reference(lis3dh_t *lis3dh);
int lis3dh_reset(lis3dh_t *lis3dh);
int lis3dh_read_adc(lis3dh_t *lis3dh);
int lis3dh_read_temp(lis3dh_t *lis3dh);
int lis3dh_fifo_reset(lis3dh_t *lis3dh);
```
All functions return `0` on success, and any non-zero value on error.
## Raspberry Pi
First run `$ sudo raspi-config` and enable SPI and/or I2C.
Edit `main.c` to use I2C or SPI.
See `i2c.c` and `spi.c` for which pins/device to use.
```sh
$ git clone https://github.com/wrclark/lis3dh.git
$ cd lis3dh
$ make
$ ./lis3dh
```
## STM32
### I2C
```c
#define LIS3DH_I2C_ADDR 0x18 /* can also be 0x19 */
int i2c_write(uint8_t reg, uint8_t value) {
uint8_t buf[2] = { reg, value };
HAL_I2C_Master_Transmit(&hi2c2, LIS3DH_I2C_ADDR << 1, buf, 2, HAL_MAX_DELAY);
return 0;
}
int i2c_read(uint8_t reg, uint8_t *dst, uint32_t size) {
if (size > 1) {
reg |= 0x80; /* auto-increment bit */
}
uint8_t send[2] = { reg, 0x00 };
HAL_I2C_Master_Transmit(&hi2c2, LIS3DH_I2C_ADDR << 1, send, 2, HAL_MAX_DELAY);
HAL_I2C_Master_Receive(&hi2c2, LIS3DH_I2C_ADDR << 1, dst, size, HAL_MAX_DELAY);
return 0;
}
```
### SPI
CPHA=0 CPOL=0 8 bits motorola
```c
/* Every sleep call duration is a multiple of 1000 us (1 ms) */
/* So dividing this value by 1000 is perfectly fine. */
int sleep_us(uint32_t dur_us) {``
HAL_Delay(dur_us / 1000);
return 0;
}
int spi_write(uint8_t reg, uint8_t value) {
uint8_t send[2];
reg &= 0x3F; /* clear 2 msbit */
send[0] = reg;
send[1] = value;
/* CS LOW */
HAL_GPIO_WritePin(SPI1_GPIO_CS_GPIO_Port, SPI1_GPIO_CS_Pin, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, send, 2, HAL_MAX_DELAY);
/* CS HIGH */
HAL_GPIO_WritePin(SPI1_GPIO_CS_GPIO_Port, SPI1_GPIO_CS_Pin, GPIO_PIN_SET);
return 0;
}
int spi_read(uint8_t reg, uint8_t *dst, uint32_t size) {
uint8_t send[2];
reg |= 0x80; /* read bit = 1 */
if (size > 1) {
reg |= 0x40; /* auto increment for rx > 1 */
}
send[0] = reg;
send[1] = 0x00;
/* CS LOW */
HAL_GPIO_WritePin(SPI1_GPIO_CS_GPIO_Port, SPI1_GPIO_CS_Pin, GPIO_PIN_RESET);
HAL_SPI_Transmit(&hspi1, send, 2, 1000);
HAL_SPI_Receive(&hspi1, dst, size, 1000);
/* CS HIGH */
HAL_GPIO_WritePin(SPI1_GPIO_CS_GPIO_Port, SPI1_GPIO_CS_Pin, GPIO_PIN_SET);
return 0;
}
```

95
doc/lis3dh.py Normal file
View File

@ -0,0 +1,95 @@
# LIS3DH
from time import sleep
from machine import I2C
import struct
LIS3DH_I2C_ADDR = 0x18
# ODR data rate
ODR_POWER_OFF = 0b0000
# all operating modes
ODR_1_HZ = 0b0001
ODR_10_HZ = 0b0010
ODR_25_HZ = 0b0011
ODR_50_HZ = 0b0100
ODR_100_HZ = 0b0101
ODR_200_HZ = 0b0110
ODR_400_HZ = 0b0111
# specific operating modes only
ODR_LP_1600_HZ = 0b1000
ODR_NORM_1344_HZ = 0b1001
ODR_LP_5376_HZ = 0b1001
# acc. scale
FS_2G = 0b00
FS_4G = 0b01
FS_8G = 0b10
FS_16G = 0b11
sda_pin = machine.Pin(16)
scl_pin = machine.Pin(17)
i2c = I2C(0, scl=scl_pin, sda=sda_pin, freq=400000)
def read(reg, n):
return i2c.readfrom_mem(LIS3DH_I2C_ADDR, reg, n)
def write(reg, val):
i2c.writeto_mem(LIS3DH_I2C_ADDR, reg, val)
# WHO_AM_I
if read(0x0F, 1)[0] == 0x33:
print("found LIS3DH")
# reboot
write(0x24, struct.pack("B", 0x80))
sleep(0.1)
Z_en = 1
Y_en = 1
X_en = 1
ctrl_reg1 = 0 | (ODR_10_HZ << 4) | (Z_en << 2) | (Y_en << 1) | X_en
write(0x20, struct.pack("B", ctrl_reg1))
ctrl_reg4 = 0 | (FS_16G << 4)
ctrl_reg4 |= 0x80 # set block update
ctrl_reg4 |= 0x08 # set HR mode
write(0x23, struct.pack("B", ctrl_reg4))
sleep(0.2) # 200 ms
while True:
while ((read(0x27, 1)[0] >> 3) & 1) == 0:
sleep(0.02)
data = read(0x28 | 0x80, 6)
accX, accY, accZ = struct.unpack("<hhh", data)
accX >>= 2
accY >>= 2
accZ >>= 2
# divide depending on FS to get units of mg
# this is only for HR mode (maybe)
# see table 4 in datasheet
# scale output to g
#scale = 1/(4 * 1000) # FS_2G HR
#scale = 1/(2 * 1000) # FS_4G HR
#scale = 1/(1 * 1000) # FS_8G HR
scale = 1/(0.3333333 * 1000) # FS_16G HR
accX *= scale
accY *= scale
accZ *= scale
print(f"x: {accX:.3f}, y: {accY:.3f}, z: {accZ:.3f}")
sleep(0.05)

135
example/4d-movement.c Normal file
View File

@ -0,0 +1,135 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.pin1.ia1 = 1; /* allow INT1 through INT_PIN1 */
/* 1 LSb = 16 mg @ FS_2G
* 0.1g threshold = 100/16 = 6.25
* add read error, +40mg => 140/16 ~= 9
*/
lis.cfg.int1_ths = 9;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* 10 ms => 10/2.5 = 5
* lis.cfg.int1_dur = 5; <== 10 ms minimum duration to trig
*/
lis.cfg.int1_dur = 0; /* instantaneous */
/* enable Y-axis and X-axis, + and - */
lis.cfg.int1.yh = 1;
lis.cfg.int1.xh = 1;
lis.cfg.int1.yl = 1;
lis.cfg.int1.xl = 1;
/* 4D movement recognition */
lis.cfg.int1.aoi = 0;
lis.cfg.int1.en_6d = 1;
lis.cfg.int1.en_4d = 1;
/* latch interrupt */
lis.cfg.int1.latch = 1;
/* set up a HP filter to ignore constant earth acceleration */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.ia1 = 1; /* enable filter for INT1 generator */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear old interrupt if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for( ;; ) {
/* wait for INT1 to go active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC and by doing so clear IA on device */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* only print if INT1 interrupt active = 1*/
/* ZH and ZL are always 0 in 4D mode */
if (LIS3DH_INT_SRC_IA(lis.src.int1)) {
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

135
example/4d-position.c Normal file
View File

@ -0,0 +1,135 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.pin1.ia1 = 1; /* allow INT1 through INT_PIN1 */
/* 1 LSb = 16 mg @ FS_2G
* 0.1g threshold = 100/16 = 6.25
* add read error, +40mg => 140/16 ~= 9
*/
lis.cfg.int1_ths = 9;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* 10 ms => 10/2.5 = 5
* lis.cfg.int1_dur = 5; <== 10 ms minimum duration to trig
*/
lis.cfg.int1_dur = 0; /* instantaneous */
/* enable Y-axis and X-axis, + and - */
lis.cfg.int1.yh = 1;
lis.cfg.int1.xh = 1;
lis.cfg.int1.yl = 1;
lis.cfg.int1.xl = 1;
/* 4D position recognition */
lis.cfg.int1.aoi = 1;
lis.cfg.int1.en_6d = 1;
lis.cfg.int1.en_4d = 1;
/* latch interrupt */
lis.cfg.int1.latch = 1;
/* set up a HP filter to ignore constant earth acceleration */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.ia1 = 1; /* enable filter for INT1 generator */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear old interrupt if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for( ;; ) {
/* wait for INT1 to go active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC and by doing so clear IA on device */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* only print if INT1 interrupt active (IA) = 1*/
/* ZH and ZL are always 0 in 4D mode */
if (LIS3DH_INT_SRC_IA(lis.src.int1)) {
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

135
example/6d-movement.c Normal file
View File

@ -0,0 +1,135 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.pin1.ia1 = 1; /* allow INT1 through INT_PIN1 */
/* 1 LSb = 16 mg @ FS_2G
* 0.1g threshold = 100/16 = 6.25
* add read error, +40mg => 140/16 ~= 9
*/
lis.cfg.int1_ths = 9;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* 10 ms => 10/2.5 = 5
* lis.cfg.int1_dur = 5; <== 10 ms minimum duration to trig
*/
lis.cfg.int1_dur = 0; /* instantaneous */
/* enable all axes */
lis.cfg.int1.yh = 1;
lis.cfg.int1.zh = 1;
lis.cfg.int1.xh = 1;
lis.cfg.int1.yl = 1;
lis.cfg.int1.zl = 1;
lis.cfg.int1.xl = 1;
/* 6D movement recognition */
lis.cfg.int1.aoi = 0;
lis.cfg.int1.en_6d = 1;
/* latch interrupt */
lis.cfg.int1.latch = 1;
/* set up a HP filter to ignore constant earth acceleration */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.ia1 = 1; /* enable filter for INT1 generator */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear old interrupt if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for( ;; ) {
/* wait for INT1 to go active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC and by doing so clear IA on device */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* only print if INT1 interrupt active (IA) = 1*/
if (LIS3DH_INT_SRC_IA(lis.src.int1)) {
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

135
example/6d-position.c Normal file
View File

@ -0,0 +1,135 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.pin1.ia1 = 1; /* allow INT1 through INT_PIN1 */
/* 1 LSb = 16 mg @ FS_2G
* 0.1g threshold = 100/16 = 6.25
* add read error, +40mg => 140/16 ~= 9
*/
lis.cfg.int1_ths = 9;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* 10 ms => 10/2.5 = 5
* lis.cfg.int1_dur = 5; <== 10 ms minimum duration to trig
*/
lis.cfg.int1_dur = 0; /* instantaneous */
/* enable all axes */
lis.cfg.int1.yh = 1;
lis.cfg.int1.zh = 1;
lis.cfg.int1.xh = 1;
lis.cfg.int1.yl = 1;
lis.cfg.int1.zl = 1;
lis.cfg.int1.xl = 1;
/* 6D position recognition */
lis.cfg.int1.aoi = 1;
lis.cfg.int1.en_6d = 1;
/* latch interrupt */
lis.cfg.int1.latch = 1;
/* set up a HP filter to ignore constant earth acceleration */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.ia1 = 1; /* enable filter for INT1 generator */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear old interrupt if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for( ;; ) {
/* wait for INT1 to go active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC and by doing so clear IA on device */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* only print if INT1 interrupt active (IA) = 1*/
if (LIS3DH_INT_SRC_IA(lis.src.int1)) {
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

153
example/README.md Normal file
View File

@ -0,0 +1,153 @@
# lis3dh/example
## Operating mode
The LIS3DH has 3 operating modes.
| mode | symbol | description |
|------|---------|------------|
| LP | `LIS3DH_MODE_LP` | Low-power mode. 8-bit acc. reading resolution. |
| Normal | `LIS3DH_MODE_NORMAL` | "Normal" mode. 10-bit acc. reading resolution. |
| HR | `LIS3DH_MODE_HR` | High-resolution mode. 12-bit acc. reading resolution. |
## ODR
There are serveral `ODR` (internal data/sample rate) options, but some may only be used in a specific operating mode.
| ODR | mode | symbol |
|-----|------|--------|
| 1 Hz | any | `LIS3DH_ODR_1_HZ` |
| 10 Hz | any | `LIS3DH_ODR_10_HZ` |
| 25 Hz | any | `LIS3DH_ODR_25_HZ` |
| 50 Hz | any | `LIS3DH_ODR_50_HZ` |
| 100 Hz | any | `LIS3DH_ODR_100_HZ` |
| 200 Hz | any | `LIS3DH_ODR_200_HZ` |
| 400 Hz | any | `LIS3DH_ODR_400_HZ` |
| 1344 Hz | Normal | `LIS3DH_ODR_NORM_1344_HZ` |
| 1600 Hz | LP | `LIS3DH_ODR_LP_1600_HZ` |
| 5376 Hz | LP | `LIS3DH_ODR_LP_5376_HZ` |
## Filter
The LIS3DH can apply its built-in high-pass filter to samples [regular reading, FIFO reading] and some specific user-functions. It has 3 different modes.
| mode | symbol | description |
|------|--------|-------------|
| Normal | `LIS3DH_FILTER_MODE_NORMAL` | Use `lis3dh_reference()` to set the filter to the current accel field, without having to wait for it to settle at/near it. |
| Autoreset | `LIS3DH_FILTER_MODE_AUTORESET` | Same as `normal` but this mode also automatically resets itself upon an interrupt(*). |
| REFERENCE | `LIS3DH_FILTER_MODE_REFERENCE` | Output data is calculated as the difference between `cfg.reference` and the measured acceleration. |
\* INT by the generator which the filter is programmed to apply itself to.
See files: `filter-normal.c`, and `filter-reference.c`.
# FIFO
Instead of polling for every single [x y z] set, a FIFO with programmable capacity ("watermark") can be used, and then dumped into memory once full.
All FIFO readings use 10-bit resolution regardless of the mode set in `cfg.mode`.
The watermark level can be adjusted to a value [1-32] (0 disables FIFO) by modifying the `cfg.fifo.size` property before calling `lis3dh_configure()`.
The FIFO "engine" samples/appends another set of [x y z] values at 1/ODR. The maximum ODR supported by the FIFO "engine" is 200 Hz.
| FIFO mode | symbol | description |
|------------------|-----------------------|----------------------------|
| Bypass | `LIS3DH_FIFO_MODE_BYPASS` | FIFO is inoperational |
| FIFO | `LIS3DH_FIFO_MODE_FIFO` | FIFO can be read/emptied at any time but once overrun has to be reset. See files: `fifo-int-ovrn.c`, `fifo-int-wtm.c`, `fifo.c` |
| Stream | `LIS3DH_FIFO_MODE_STREAM` | FIFO continously writes new data at 1/ODR and will overwrite old data until it is read/emptied. See files: `stream-int-ovrn.c`, `stream-int-wtm.c`, `stream.c` |
| Stream_to_FIFO | `LIS3DH_FIFO_STREAM_TO_FIFO` | FIFO behaves like Stream mode until a set interrupt is activated, then changes to a mode FIFO. |
Note: FIFO will not trigger a watermark interrupt (`pin1.wtm`) if the FIFO size is default (32; maximum size). To use the watermark interrupt, the FIFO size has to be between [1-31]. An overrun interrupt (`pin1.overrun`) will always trigger when the FIFO is full, regardless of programmed capacity.
Note: to sample data faster than 200 Hz, it is necessary to use the regular data polling functionality using `lis3dh_read()` with the appropriate configuration. See files `simple.c` and `fast.c` for examples.
### file: self-test.c
Run a device self-test to see if the device is within spec. Mine apparently isn't. (must be at-rest during test).
### file: single-click.c
Set up single-click detection (no latching interrupt possible)
### file: double-click.c
Set up double-click detection (no latching interrupt possible)
### file: adc.c
Enable and read built-in ADCs.
> - Input range: 800 mV to 1600 mV
> - Resolution: 8-bit in LP mode, 10-bit in normal and in HR mode.
> - Sampling frequency: same as ODR
### file: temp.c
Enable and read built-in temperature sensor
> - Operating range: -40 to 85°C
> - Step size: ±1°C
### Inertial interrupts
There are two interrupt registers, `int1` and `int2` that can be configured for inertial interrupts. The config structs are identical and contain the fields: `zh`, `zl`, `yh`, `yl`, `xh`, `xl`, and more. `zh` stands for `Z_axis_high` and `zl` stands for `Z_axis_low`. If both are enabled, the device will generate an interrupt upon Z-axis acceleration exceeding `threshold`, or upon Z-axis acceleration reading at or below `-threshold` (in OR mode. Not possible in AND mode).
| aoi | en_6d | interrupt mode |
|-----|-------|-------------------------|
| 0 | 0 | OR combination |
| 0 | 1 | 6d MOVEMENT recognition |
| 1 | 0 | AND combination |
| 1 | 1 | 6d POSITION recognition |
#### OR combination
An interrupt is generated when at least one of the configured axes is at or above the threshold level.
#### 6D MOVEMENT recognition
An interrupt is generated when the device moves from a direction (known or unknown) to a different known direction. The interrupt is only active for 1/ODR.
#### AND combination
An interrupt is generated when all of the configures axes are at or above the threshold level.
#### 6D POSITION recognition
An interrupt is generated when the device is "stable" in a known direction. The interrupt is active as long as the direction is maintained.
### file: inertial-wakeup.c
Inertial interrupt example in OR mode (easily changed to AND mode) with configurable axes, threshold and minimum acceleration duration.
### file: free-fall.c
Inertial interrupt example activating upon free-fall. It works by using an AND mode interrupt of all the negative axes and comparing them to a threshold value (in the case of negative axis the threshold is multiplied by -1), recommended to be at 350mg (for >30 ms) and activating when the experienced negative acceleration is greater (abs. sense) than the negative threshold.
### file: 6d-movement.c
Inertial interrupt example, generates an interrupt when some acceleration, `threshold` is experienced on any configured axis for `duration` time. Supposedly the device knows what a "known" direction is.
### file: 6d-position.c
Inertial interrupt example, the interrupt line is kept active so long as the device is stable (ie acceleration on configured axes does not exceed `threshold` for `duration` time).
---
### 4D detection
4D detection is a subset of 6D detection meant for detecting portrait/landscape screen rotations on mobile phones, etc. It functionally works the same as the 6D modes, except that detection along the Z-axis is disabled.
### file: 4d-movement.c
Inertial interrupt example, generates an interrupt when some acceleration, `threshold` is experienced on any configured axis for `duration` time. Supposedly the device knows what a "known" direction is.
### file: 4d-position.c
Inertial interrupt example, the interrupt line is kept active so long as the device is stable (ie acceleration on configured axes does not exceed `threshold` for `duration` time).
# "Sleep to Wake" and "Return to Sleep"
The LIS3DH can be programmed to automatically enter a slow, low-power mode until it detects a specific event (acceleration exceeding `threshold`). Then, it will enter the mode set in `cfg.{mode,rate}` and behave as normal, until the `duration` since the beginning of sensing event has elapsed.
The device, if configured with any non-zero values in `cfg.act_ths` and `cfg.act_dur` immediately enters low-power mode and will remain so until it experiences an acceleration [OR combination of all axes] that exceeds `threshold`. Upon experiencing such an acceleration, the device will activate `INT2` (configurable) and for a period of time (specified in `cfg.act_dur`) behave as normal, i.e. use the mode set in `cfg`.
When the time period (specified in `cfg.act_dur`) has elapsed, the device will trigger on `INT2` (configurable) again, and enter low-power mode. This cycle continues indefinitely.
See file: `sleep-to-wake.c`.

57
example/adc.c Normal file
View File

@ -0,0 +1,57 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.en_adc = 1; /* enable ADC */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* Read all 3 ADCs */
if (lis3dh_read_adc(&lis)) {
/* error handling */
}
/* print measured mV */
printf("ADC1: %d mV\n", lis.adc.adc1);
printf("ADC2: %d mV\n", lis.adc.adc2);
printf("ADC3: %d mV\n", lis.adc.adc3);
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

139
example/double-click.c Normal file
View File

@ -0,0 +1,139 @@
/*
* SCLICK SCLICK
* __________ ___________
* | | | | |
* | | | | |
* ----- -------------------- | ------
* |
* TIME_LIMIT TIME_LIMIT |
* >---------< >--------< | ==> DCLICK_INT
* LATENCY WINDOW >----|---<
* >----------<>----------< LATENCY
* */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ; /* minimum recommended ODR */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.click = 1; /* enable filtering for CLICK function */
lis.cfg.click.xd = 1; /* enable X axis double click */
lis.cfg.click.yd = 1; /* enable Y axis double click */
lis.cfg.click.zd = 1; /* enable Z axis double click */
lis.cfg.pin1.click = 1; /* enable CLICK INT through pin1 */
/* 1 LSb = 16 mg @ FS_2G
* so a 0.3g 'shock' is 300/16 = 18.75
* However, the device can have up to +- 40mg read error, so add 40mg
* 0.34g => 340/16 ~= 21
*/
lis.cfg.click_ths = 21;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* time_limit of 75 ms = 75/2.5 = 30
* time_latency of 40 ms = 40/2.5 = 16
* time_window of 500 ms = 500/2.5 = 200
*
*/
lis.cfg.time_limit = 30; /* range: 0-127 */
lis.cfg.time_latency = 16; /* range: 0-255 */
lis.cfg.time_window = 200; /* range: 0-255 */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read CLICK_SRC to clear previous interrupts, if any */
if (lis3dh_read_click(&lis)) {
/* error handling */
}
for(;;) {
/* poll interrupt on INT1 pin */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read CLICK_SRC when interrupt has fired */
if (lis3dh_read_click(&lis)) {
/* error handling */
}
/* only print if DCLICK=1 */
if (LIS3DH_CLICK_DCLICK(lis.src.click)) {
/* print data gathered from CLICK_SRC */
printf("Click: X=%d, Y=%d, Z=%d, Sign=%d, S_CLICK=%d, D_CLICK=%d\n",
LIS3DH_CLICK_SRC_X(lis.src.click),
LIS3DH_CLICK_SRC_Y(lis.src.click),
LIS3DH_CLICK_SRC_Z(lis.src.click),
LIS3DH_CLICK_SIGN(lis.src.click),
LIS3DH_CLICK_SCLICK(lis.src.click),
LIS3DH_CLICK_DCLICK(lis.src.click));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

53
example/fast.c Normal file
View File

@ -0,0 +1,53 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_LP; /* Low-power mode */
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_LP_5376_HZ; /* 5 KHz ODR, only available in low-power mode */
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* read accelerometer and store results at struct lis.acc */
if (lis3dh_read(&lis)) {
/* error handling */
}
/* print data .. */
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", lis.acc.x, lis.acc.y, lis.acc.z);
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

82
example/fifo-int-ovrn.c Normal file
View File

@ -0,0 +1,82 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
#include "interrupt.h"
/* Raspberry Pi GPIO pin connected to LIS3DH INT1 pin */
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.pin1.overrun = 1;
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_FIFO;
lis.cfg.fifo.trig = LIS3DH_FIFO_TRIG_INT1;
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* wait until FIFO overrun interrupt is active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
printf("fifo.size=%d\n", fifo.size);
/* in FIFO mode, the FIFO engine must be restarted after it has been overrun */
/* otherwise, it will not fill up again. */
if (lis3dh_fifo_reset(&lis)) {
/* error handling */
}
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

84
example/fifo-int-wtm.c Normal file
View File

@ -0,0 +1,84 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
#include "interrupt.h"
/* Raspberry Pi GPIO pin connected to LIS3DH INT1 pin */
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.pin1.wtm = 1; /* watermark interrupt */
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_FIFO;
lis.cfg.fifo.trig = LIS3DH_FIFO_TRIG_INT1;
lis.cfg.fifo.size = 15; /* size must be < 32 to use the watermark interrupt */
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* wait until FIFO watermark interrupt is active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
printf("fifo.size=%d\n", fifo.size);
/* Since the WTM is less than the full size, there is no need to reset the device after reading */
/* However, if the reading of the device is somehow delayed and the FIFO fills up between */
/* receiving the wtm interrupt, and calling lis3dh_read_fifo(), the FIFO must be reset. */
/* this can easily be simulated by calling a sleep function. */
/* Therefore, it is a good idea to reset the FIFO anyway. */
lis3dh_fifo_reset(&lis);
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

71
example/fifo.c Normal file
View File

@ -0,0 +1,71 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_FIFO;
/* modifying the size of the FIFO buffer is not useful when just polling without interrupts */
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
/* FIFO size will rarely go above 2 in this example, because lis3dh_read_fifo() */
/* does not wait for the FIFO buffer to be full, just non-empty. */
printf("fifo.size=%d\n", fifo.size);
/* Since the WTM is less than the full size, there is no need to reset the device after reading */
/* However, if the reading of the device is somehow delayed and the FIFO fills up between */
/* calls to lis3dh_read_fifo(), then the device is overrun, and must be reset. */
/* this can easily be simulated by calling a sleep function. */
/* Therefore, it is a good idea to reset the FIFO anyway. */
lis3dh_fifo_reset(&lis);
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

63
example/filter-normal.c Normal file
View File

@ -0,0 +1,63 @@
#define _GNU_SOURCE
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
/* normal mode */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
/* 3dB cutoff freq (~ ODR) */
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
/* write config to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
for ( ;; ) {
/* read an [x y z] accel set. Filter is applied */
if (lis3dh_read(&lis)) {
/* error handling */
}
printf("x: %4.d, y: %4.d, z: %4.d\n", lis.acc.x, lis.acc.y, lis.acc.z);
usleep(5000); /* 5 ms */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

View File

@ -0,0 +1,73 @@
#define _GNU_SOURCE
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
/* normal mode */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_REFERENCE;
/* 3dB cutoff freq (~ ODR) */
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
/* Output [x y z] data is calculated as the difference between the
* measured acceleration and the value stored in REFERENCE.
* signed 7-bit int, 1 LSb value depends on FS.
* FS_2G: ~ 16mg per 1 LSb
* FS_4G: ~ 31mg per 1 LSb
* FS_8G: ~ 63mg per 1 LSb
* FS_16G: ~127mg per 1 LSb */
/* @ 2G,
* To have the output data be referenced to 0.8g:
* => 800 mg / 16 mg = 50 */
/* to reference -0.8g simply put -50 */
lis.cfg.reference = 50;
/* write config to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* Do not read REFERENCE */
for ( ;; ) {
/* read an [x y z] accel set. Filter is applied */
if (lis3dh_read(&lis)) {
/* error handling */
}
printf("x: %4.d, y: %4.d, z: %4.d\n", lis.acc.x, lis.acc.y, lis.acc.z);
usleep(5000); /* 5 ms */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

129
example/free-fall.c Normal file
View File

@ -0,0 +1,129 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
/* no filter because we want the 'absolute' experienced acceleration */
lis.cfg.pin1.ia1 = 1; /* enable INT1 through pin1 */
lis.cfg.int1.latch = 1; /* latch interrupt until INT1_SRC is read */
/* enable all the low directions */
/* this works because the low directions are compared to -threshold, */
/* and are below it at rest, and greater than it at free-fall, */
/* while the opposite is true for the high directions. */
lis.cfg.int1.xl = 1;
lis.cfg.int1.yl = 1;
lis.cfg.int1.zl = 1;
/* AND interrupt mode */
lis.cfg.int1.aoi = 1;
lis.cfg.int1.en_6d = 0;
/* 1 LSb = 16 mg @ FS_2G
* Due to inherent problems with accuracy and timing, you should use a "free-fall window",
* rather than setting the threshold to 0. AN3308 pg 27 recommends at or below 350mg for all axes,
* to be considered as being in a state of free-fall, but the gradual return to experiencing
* 1g in free-fall is not discussed.
*
* 350/16 ~= 20
*/
lis.cfg.int1_ths = 20;
/*
* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* At 400 ODR,
* 30 ms = 30/2.5 = 12
*/
lis.cfg.int1_dur = 12;
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear previous interrupts, if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for ( ;; ) {
/* poll interrupt on INT1 pin */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC when interrupt has fired */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* only print if IA=1 */
if (LIS3DH_INT_SRC_IA(lis.src.int1)) {
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

132
example/inertial-wakeup.c Normal file
View File

@ -0,0 +1,132 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.pin1.ia1 = 1; /* allow INT1 through INT_PIN1 */
/* 1 LSb = 16 mg @ FS_2G
* 0.3g threshold = 300/16 = 18.75
* add read error, +40mg => 340/16 = 21.25 ~= 21
* if you for some reason don't want to use the HP filter,
* just add 1g to the threshold calculation.
*/
lis.cfg.int1_ths = 21;
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* For ODR=400:
* 10 ms => 10/2.5 = 5
* lis.cfg.int1_dur = 5; <== 10 ms minimum duration to wake up
*/
lis.cfg.int1_dur = 0; /* instantaneous */
/* enable X_high, Y_high and Z_high */
lis.cfg.int1.yh = 1;
lis.cfg.int1.zh = 1;
lis.cfg.int1.xh = 1;
/* OR mode. Think about the axis combinations for AND mode */
lis.cfg.int1.aoi = 0; /* set to 1 for AND mode */
lis.cfg.int1.en_6d = 0;
/* latch interrupt */
lis.cfg.int1.latch = 1;
/* set up a HP filter to ignore constant earth acceleration */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.ia1 = 1; /* enable filter for INT1 generator */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read INT1_SRC to clear old interrupt if any */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
for( ;; ) {
/* wait for INT1 to go active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read INT1_SRC */
if (lis3dh_read_int1(&lis)) {
/* error handling */
}
/* print received interrupt .. */
printf("IA=%d ZH=%d ZL=%d YH=%d YL=%d XH=%d XL=%d\n",
LIS3DH_INT_SRC_IA(lis.src.int1),
LIS3DH_INT_SRC_Z_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Z_LOW(lis.src.int1),
LIS3DH_INT_SRC_Y_HIGH(lis.src.int1),
LIS3DH_INT_SRC_Y_LOW(lis.src.int1),
LIS3DH_INT_SRC_X_HIGH(lis.src.int1),
LIS3DH_INT_SRC_X_LOW(lis.src.int1));
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

103
example/self-test.c Normal file
View File

@ -0,0 +1,103 @@
#define _GNU_SOURCE
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include <stdlib.h>
#include "lis3dh.h"
#include "i2c.h"
/* table 4 of datasheet */
#define ST_MIN 17
#define ST_MAX 360
/* AN3308 suggests 5 */
#define ITERATIONS 5
int main() {
lis3dh_t lis;
int32_t x_nost, y_nost, z_nost; /* store avg of measurements before self-test */
int32_t x_st, y_st, z_st; /* store avg of self-test measurements */
int32_t xd, yd, zd; /* differences */
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
x_nost = y_nost = z_nost = 0;
x_st = y_st = z_st = 0;
xd = yd = zd = 0;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_NORMAL;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_50_HZ;
lis3dh_configure(&lis);
/* discard first couple of samples as datasheet suggests they are wrong/noisy */
for(i=0; i<15; i++) lis3dh_read(&lis);
for(i=0; i<ITERATIONS; i++) {
lis3dh_read(&lis);
x_nost += lis.acc.x;
y_nost += lis.acc.y;
z_nost += lis.acc.z;
}
x_nost /= ITERATIONS;
y_nost /= ITERATIONS;
z_nost /= ITERATIONS;
lis.cfg.self_test = LIS3DH_SELF_TEST_0;
lis3dh_configure(&lis);
/* discard first couple of samples as datasheet suggests they are wrong/noisy */
for(i=0; i<15; i++) lis3dh_read(&lis);
for(i=0; i<ITERATIONS; i++) {
lis3dh_read(&lis);
x_st += lis.acc.x;
y_st += lis.acc.y;
z_st += lis.acc.z;
}
x_st /= ITERATIONS;
y_st /= ITERATIONS;
z_st /= ITERATIONS;
xd = abs(x_st - x_nost);
yd = abs(y_st - y_nost);
zd = abs(z_st - z_nost);
printf("variance min=%d, max=%d\n", ST_MIN, ST_MAX);
printf("xd=%d yd=%d zd=%d\n", xd, yd, zd);
if ((ST_MIN <= xd && xd <= ST_MAX) && (ST_MIN <= yd && yd <= ST_MAX) && (ST_MIN <= zd && zd <= ST_MAX)) {
puts("Pass");
} else {
puts("Fail");
}
lis3dh_reset(&lis);
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

49
example/simple.c Normal file
View File

@ -0,0 +1,49 @@
#define _GNU_SOURCE
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_4G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read latest [x y z] data, store in the `lis' struct's `acc' field */
if (lis3dh_read(&lis)) {
/* error handling */
}
printf("x: %d mg, y: %d mg, z: %d mg\n", lis.acc.x, lis.acc.y, lis.acc.z);
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

123
example/single-click.c Normal file
View File

@ -0,0 +1,123 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ; /* minimum recommended ODR */
lis.cfg.filter.mode = LIS3DH_FILTER_MODE_NORMAL;
lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
lis.cfg.filter.click = 1; /* enable filtering for CLICK function */
lis.cfg.click.xs = 1; /* enable X axis single click */
lis.cfg.click.ys = 1; /* enable Y axis single click */
lis.cfg.click.zs = 1; /* enable Z axis single click */
lis.cfg.pin1.click = 1; /* enable CLICK INT through pin1 */
/* 1 LSb = 16 mg @ FS_2G
* so a 0.3g 'shock' is 300/16 = 18.75
* However, the device can have up to +- 40mg read error, add 40 mg
* 0.34g => 340/16 ~= 21
* (Note: 0.34g and not 1.34g because of HP filter)
*/
lis.cfg.click_ths = 21;
/*
* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 400 2.5
*
* At 400 ODR,
* 20 ms = 20/2.5 = 8
*/
lis.cfg.time_limit = 8;
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* read REFERENCE to set filter to current accel field */
if (lis3dh_reference(&lis)) {
/* error handling */
}
/* read CLICK_SRC to clear previous interrupts, if any */
if (lis3dh_read_click(&lis)) {
/* error handling */
}
for(;;) {
/* poll interrupt on INT1 pin */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read CLICK_SRC when interrupt has fired */
if (lis3dh_read_click(&lis)) {
/* error handling */
}
/* only print if SCLICK=1 */
if (LIS3DH_CLICK_SCLICK(lis.src.click)) {
/* print data gathered from CLICK_SRC */
printf("Click: X=%d, Y=%d, Z=%d, Sign=%d, S_CLICK=%d, D_CLICK=%d\n",
LIS3DH_CLICK_SRC_X(lis.src.click),
LIS3DH_CLICK_SRC_Y(lis.src.click),
LIS3DH_CLICK_SRC_Z(lis.src.click),
LIS3DH_CLICK_SIGN(lis.src.click),
LIS3DH_CLICK_SCLICK(lis.src.click),
LIS3DH_CLICK_DCLICK(lis.src.click));
}
}
/* unregister interrupt */
if (int_unregister(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

119
example/sleep-to-wake.c Normal file
View File

@ -0,0 +1,119 @@
#define _GNU_SOURCE
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "interrupt.h"
#include "i2c.h"
/* LIS3DH INT2 connected to Raspberry Pi GPIO 16 (Pin 36) */
#define GPIO_INTERRUPT_PIN_INT2 16
int main() {
lis3dh_t lis;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt on specified pin */
if (int_register(GPIO_INTERRUPT_PIN_INT2)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
/* enable ACT function through INT PIN 2 */
/* interrupts must be used, there is no reg to poll */
lis.cfg.pin2.act = 1;
/* This functionality cannot be filtered internally !! */
/* 1 LSb = 16 mg @ FS_2G
* 1 LSb = 32 mg @ FS_4G
* 1 LSb = 62 mg @ FS_8G
* 1 LSb = 186 mg @ FS_16G
*
* @ FS_2G
* Wake up when acceleration goes above 1.3g .. (any axis)
* 1.3g / 16mg = 1300/16 = N LSb = 81.25 ~ 81
*/
/* This controls activity recognition */
lis.cfg.act_ths = 81; /* 7-bit ; must be > 1*/
/* Sleep-to-wake and return-to-sleep duration
* 1 LSb = (8 * 1[LSb] + 1) / ODR
* Confusing formulation !
*
* [ODR] [1 LSb in milliseconds]
* 1 1000
* 10 100
* 25 40
* 50 20
* 100 10
* 200 5
* 400 2.5
*
* Full table in lis3dh.h
*
* 1 LSb = (8 * N[2.5ms] + 1) / ODR ( @ ODR=400Hz )
* 1 LSb = (8 * N[LSb] + 1) / ODR ( @ ODR=400Hz )
*
* So, 5000 ms => (8 * N[2.5] + 1) / ODR ; solve N=time(ms)/2.5
* => (8 * 2000[LSb] + 1) / ODR
* => (8 * 2000 + 1) / ODR
* => 16001 / 400
* => 40
*
* After `act_ths' worth of acceleration has been felt;
* Stay in Wake mode for 5000 ms
*/
/* This controls return-to-sleep */
lis.cfg.act_dur = 40; /* 8-bit ; range 0-255 */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* wait until INT2 goes active (Wake mode) */
if (int_poll(GPIO_INTERRUPT_PIN_INT2)) {
/* error handling */
}
/* Do "work" here at high ODR, HR mode, etc. */
puts("I am awake!");
/* wait until INT2 goes inactive (Sleep mode) */
if (int_poll(GPIO_INTERRUPT_PIN_INT2)) {
/* error handling */
}
puts("Sleeping...");
}
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

78
example/stream-int-ovrn.c Normal file
View File

@ -0,0 +1,78 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
#include "interrupt.h"
/* Raspberry Pi GPIO pin connected to LIS3DH INT1 pin */
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.pin1.overrun = 1;
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_STREAM;
lis.cfg.fifo.trig = LIS3DH_FIFO_TRIG_INT1;
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* wait until FIFO overrun interrupt is active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
printf("fifo.size=%d\n", fifo.size);
/* No need to reset the FIFO engine in stream mode */
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

79
example/stream-int-wtm.c Normal file
View File

@ -0,0 +1,79 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
#include "interrupt.h"
/* Raspberry Pi GPIO pin connected to LIS3DH INT1 pin */
#define GPIO_INTERRUPT_PIN_INT1 12
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* register interrupt */
if (int_register(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.pin1.wtm = 1; /* watermark interrupt */
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_STREAM;
lis.cfg.fifo.trig = LIS3DH_FIFO_TRIG_INT1;
lis.cfg.fifo.size = 15; /* size must be < 32 to use the watermark interrupt */
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* wait until FIFO watermark interrupt is active */
if (int_poll(GPIO_INTERRUPT_PIN_INT1)) {
/* error handling */
}
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
printf("fifo.size=%d\n", fifo.size);
/* No need to reset the FIFO engine in stream mode */
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

66
example/stream.c Normal file
View File

@ -0,0 +1,66 @@
#define _GNU_SOURCE /* usleep() */
#include <unistd.h> /* usleep() */
#include <stdio.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise struct and check device id */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset just in case*/
if (lis3dh_reset(&lis)) {
/* error handling */
}
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_STREAM;
/* modifying the size of the FIFO buffer is not useful when just polling without interrupts */
/* write cfg to device */
if (lis3dh_configure(&lis)) {
/* error handling */
}
for ( ;; ) {
/* read FIFO into `fifo_data' struct `fifo' */
if (lis3dh_read_fifo(&lis, &fifo)) {
/* error handling */
}
/* print data .. */
for(i=0; i<fifo.size; i++) {
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
}
/* FIFO size will rarely go above 2 in this example, because lis3dh_read_fifo() */
/* does not wait for the FIFO buffer to be full, just non-empty. */
printf("fifo.size=%d\n", fifo.size);
/* No need to reset the FIFO engine in stream mode */
}
/* deinitalise device */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

63
example/temp.c Normal file
View File

@ -0,0 +1,63 @@
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include "lis3dh.h"
#include "i2c.h"
int main() {
lis3dh_t lis;
/* set fn ptrs to rw on bus (i2c or SPI) */
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/* initialise LIS3DH struct */
if (lis3dh_init(&lis)) {
/* error handling */
}
/* reset device just in case */
if (lis3dh_reset(&lis)) {
/* error handling */
}
/* set up config */
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_400_HZ;
lis.cfg.en_adc = 1; /* enable ADC */
lis.cfg.en_temp = 1; /* enable temp sensing */
/* write device config */
if (lis3dh_configure(&lis)) {
/* error handling */
}
/* Read all 3 ADCs */
if (lis3dh_read_adc(&lis)) {
/* error handling */
}
/* ADC3 now reports the temperature instead of reading from its input */
/* the temperature data is overwritten to lis.adc.adc3 */
if (lis3dh_read_temp(&lis)) {
/* error handling */
}
printf("ADC1: %d mV\n", lis.adc.adc1);
printf("ADC2: %d mV\n", lis.adc.adc2);
printf("ADC3: %d oC\n", lis.adc.adc3);
/* deinitalise struct */
if (lis3dh_deinit(&lis)) {
/* error handling */
}
return 0;
}

91
i2c.c Normal file
View File

@ -0,0 +1,91 @@
/*
Example I2C use on linux/raspberry pi
*/
#include <stdio.h>
#include <fcntl.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <stdlib.h>
#include "i2c.h"
/*
* Pinout config for this example
*
* LIS3DH SDA => Raspberry Pi GPIO 2 (Physical pin 3)
* LIS3DH SCL => Raspberry Pi GPIO 3 (Physical pin 5)
*
*/
#define I2C_DEVICE "/dev/i2c-1"
#define I2C_LIS3DH_ADDRESS 0x18 /* Can also be 0x19 */
static int fd;
int i2c_init(void) {
fd = open(I2C_DEVICE, O_RDWR);
if (fd < 0) {
fprintf(stderr, "i2c_init(): could not open device: %s\n", I2C_DEVICE);
return 1;
}
if (ioctl(fd, I2C_SLAVE, I2C_LIS3DH_ADDRESS) < 0) {
fprintf(stderr, "i2c_init(): failed to acquire bus/talk to slave\n");
close(fd);
return 1;
}
return 0;
}
int i2c_read(uint8_t reg, uint8_t *dst, uint32_t size) {
uint8_t cmd[2];
if (size > 1) {
reg |= 0x80; /* AUTO INC */
}
cmd[0] = reg;
cmd[1] = 0x00;
if (write(fd, cmd, 2) != 2) {
fprintf(stderr, "i2c_read(): error write()\n");
return 1;
}
if (read(fd, dst, size) != (int)size) {
fprintf(stderr, "i2c_read(): error read()\n");
return 1;
}
return 0;
}
int i2c_write(uint8_t reg, uint8_t value) {
uint8_t cmd[2];
cmd[0] = reg;
cmd[1] = value;
if (write(fd, cmd, 2) != 2) {
fprintf(stderr, "i2c_write(): error write()\n");
return 1;
}
return 0;
}
int i2c_deinit(void) {
if (fd) {
close(fd);
}
return 0;
}

11
i2c.h Normal file
View File

@ -0,0 +1,11 @@
#ifndef I2C_H
#define I2C_H
#include <stdint.h>
int i2c_init (void);
int i2c_read (uint8_t reg, uint8_t *dst, uint32_t size);
int i2c_write (uint8_t reg, uint8_t value);
int i2c_deinit (void);
#endif

121
interrupt.c Normal file
View File

@ -0,0 +1,121 @@
/*
SYSFS gpio interrupt example for Raspberry Pi
*/
#define _GNU_SOURCE
#include <stddef.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <poll.h>
#include "interrupt.h"
static int write_file(const char *path, const char *str) {
int fd = 0;
if ((fd = open(path, O_WRONLY)) == 0) {
fprintf(stderr, "unable to open %s\n", path);
return 1;
}
if (write(fd, str, strlen(str)) < 1) {
fprintf(stderr, "unable to write to %s\n", path);
close(fd);
return 1;
}
close(fd);
return 0;
}
/* Register a sysfs GPIO pin to be interruptable
similar to:
$ echo 12 > /sys/class/gpio/export
$ echo both > /sys/class/gpio12/edge
$ echo in > /sys/class/gpio12/direction
*/
int int_register(int pin) {
char path[256];
char buffer[64];
/* begin by unregistering supplied pin, just in case */
(void) int_unregister(pin);
memset(buffer, 0, 64);
sprintf(buffer, "%d", pin);
if (write_file("/sys/class/gpio/export", buffer) != 0) {
return 1;
}
/* sleep 500 ms to let linux set up the dir .. */
usleep(500000);
memset(path, 0, 256);
sprintf(path, "%s%d/%s", "/sys/class/gpio/gpio", pin, "edge");
if (write_file(path, "both") != 0) {
return 1;
}
memset(path, 0, 256);
sprintf(path, "%s%d/%s", "/sys/class/gpio/gpio", pin, "direction");
if (write_file(path, "in") != 0) {
return 1;
}
return 0;
}
/* unregister an interrupt gpio config
similar to:
$ echo 12 > /sys/class/gpio/unexport
*/
int int_unregister(int pin) {
char buffer[64];
memset(buffer, 0, 64);
sprintf(buffer, "%d", pin);
if (write_file("/sys/class/gpio/unexport", buffer) != 0) {
return 1;
}
usleep(50000); /* sleep 50 ms */
return 0;
}
/* poll a gpio pin for interrupt event
blocking
*/
int int_poll(int pin) {
int fd;
struct pollfd pfd;
char path[256];
char buf[16];
memset(path, 0, 256);
sprintf(path, "%s%d/%s", "/sys/class/gpio/gpio", pin, "value");
if ((fd = open(path, O_RDONLY)) == 0) {
fprintf(stderr, "unable to open %s\n", path);
return 1;
}
pfd.fd = fd;
pfd.events = POLLPRI | POLLERR;
lseek(fd, 0, SEEK_SET);
read(fd, buf, sizeof buf);
poll(&pfd, 1, -1);
lseek(fd, 0, SEEK_SET);
read(fd, buf, sizeof buf);
return 0;
}

11
interrupt.h Normal file
View File

@ -0,0 +1,11 @@
#ifndef INTERRUPT_H
#define INTERRUPT_H
/*
Note: `pin' is GPIO No.
*/
int int_register(int pin);
int int_unregister(int pin);
int int_poll(int pin);
#endif

477
lis3dh.c Normal file
View File

@ -0,0 +1,477 @@
#include <stddef.h> /* NULL */
#include <string.h> /* memset() */
#include "lis3dh.h"
#include "registers.h"
/* initialise device struct and read WHO_AM_I register */
int lis3dh_init(lis3dh_t *lis3dh) {
uint8_t result;
int err = 0;
if (!lis3dh) {
return 1;
}
/* if init has been given, check it */
if (lis3dh->dev.init != NULL) {
if (lis3dh->dev.init() != 0) {
return 1;
}
}
/* check WHO_AM_I equal to 0x33 */
err |= lis3dh->dev.read(REG_WHO_AM_I, &result, 1);
if (result != 0x33) {
return 1;
}
/* zero device struct */
memset(&lis3dh->acc, 0, sizeof lis3dh->acc);
memset(&lis3dh->cfg, 0, sizeof lis3dh->cfg);
memset(&lis3dh->adc, 0, sizeof lis3dh->adc);
memset(&lis3dh->src, 0, sizeof lis3dh->src);
lis3dh->cfg.fifo.mode = 0xFF; /* in use if neq 0xFF */
lis3dh->cfg.fifo.size = 32; /* default fifo size */
lis3dh->cfg.filter.mode = 0xFF; /* in use if neq 0xFF */
lis3dh->cfg.filter.fds = 1; /* bypass OFF by default */
return err;
}
/* write configuration options to the device */
/* then sleep 100 ms to let it set itself up properly */
int lis3dh_configure(lis3dh_t *lis3dh) {
uint8_t ctrl_reg1, ctrl_reg2, ctrl_reg3;
uint8_t ctrl_reg4, ctrl_reg5, ctrl_reg6;
uint8_t ctrl_reg0, temp_cfg_reg;
uint8_t fifo_ctrl_reg, int1_cfg, int2_cfg;
uint8_t int1_ths, int2_ths, int1_dur, int2_dur;
uint8_t click_cfg, click_ths;
uint8_t time_limit, act_ths;
int err = 0;
if (!lis3dh) {
return 1;
}
/* the 0x07 enables Z, Y and X axis in that order */
ctrl_reg1 = (lis3dh->cfg.rate << 4) | 0x07;
ctrl_reg2 = 0;
ctrl_reg3 = 0;
ctrl_reg4 = (lis3dh->cfg.range << 4) | (lis3dh->cfg.spi3w & 1);
ctrl_reg5 = 0;
ctrl_reg6 = 0;
fifo_ctrl_reg = 0;
int1_cfg = 0;
int2_cfg = 0;
int1_ths = 0;
int2_ths = 0;
int1_dur = 0;
int2_dur = 0;
click_cfg = 0;
click_ths = 0;
time_limit = 0;
act_ths = 0;
ctrl_reg0 = 0;
temp_cfg_reg = 0;
/* set self-test bits */
ctrl_reg4 |= (lis3dh->cfg.self_test & 0x03) << 1;
/* determine whether to enable ADC or TEMP sensor */
temp_cfg_reg |= (lis3dh->cfg.en_adc & 1) << 7;
temp_cfg_reg |= (lis3dh->cfg.en_temp & 1) << 6;
/* set time limit */
time_limit |= (lis3dh->cfg.time_limit & 0x7F);
act_ths |= (lis3dh->cfg.act_ths & 0x7F);
/* set click config register */
click_cfg |= (lis3dh->cfg.click.xs & 1);
click_cfg |= (lis3dh->cfg.click.xd & 1) << 1;
click_cfg |= (lis3dh->cfg.click.ys & 1) << 2;
click_cfg |= (lis3dh->cfg.click.yd & 1) << 3;
click_cfg |= (lis3dh->cfg.click.zs & 1) << 4;
click_cfg |= (lis3dh->cfg.click.zd & 1) << 5;
/* CLICK threshold */
click_ths |= (lis3dh->cfg.click_ths & 0x7F);
click_ths |= (lis3dh->cfg.click.latch & 1) << 7;
/* set interrupt registers */
/* INT PIN 1 */
ctrl_reg3 |= (lis3dh->cfg.pin1.click & 1) << 7;
ctrl_reg3 |= (lis3dh->cfg.pin1.ia1 & 1) << 6;
ctrl_reg3 |= (lis3dh->cfg.pin1.ia2 & 1) << 5;
ctrl_reg3 |= (lis3dh->cfg.pin1.drdy_zyxda & 1) << 4;
ctrl_reg3 |= (lis3dh->cfg.pin1.drdy_321 & 1) << 3;
ctrl_reg3 |= (lis3dh->cfg.pin1.wtm & 1) << 2;
ctrl_reg3 |= (lis3dh->cfg.pin1.overrun & 1) << 1;
/* INT PIN 2 */
ctrl_reg6 |= (lis3dh->cfg.pin2.click & 1) << 7;
ctrl_reg6 |= (lis3dh->cfg.pin2.ia1 & 1) << 6;
ctrl_reg6 |= (lis3dh->cfg.pin2.ia2 & 1) << 5;
ctrl_reg6 |= (lis3dh->cfg.pin2.boot & 1) << 4;
ctrl_reg6 |= (lis3dh->cfg.pin2.act & 1) << 3;
ctrl_reg6 |= (lis3dh->cfg.pin2.polarity & 1) << 1;
/* set some of CTRL_REG5 */
ctrl_reg5 |= (lis3dh->cfg.int2.en_4d & 1);
ctrl_reg5 |= (lis3dh->cfg.int2.latch & 1) << 1;
ctrl_reg5 |= (lis3dh->cfg.int1.en_4d & 1) << 2;
ctrl_reg5 |= (lis3dh->cfg.int1.latch & 1) << 3;
/* set INT1_CFG and INT2_CFG */
int1_cfg |= (lis3dh->cfg.int1.xl & 1);
int1_cfg |= (lis3dh->cfg.int1.xh & 1) << 1;
int1_cfg |= (lis3dh->cfg.int1.yl & 1) << 2;
int1_cfg |= (lis3dh->cfg.int1.yh & 1) << 3;
int1_cfg |= (lis3dh->cfg.int1.zl & 1) << 4;
int1_cfg |= (lis3dh->cfg.int1.zh & 1) << 5;
int1_cfg |= (lis3dh->cfg.int1.en_6d & 1) << 6;
int1_cfg |= (lis3dh->cfg.int1.aoi & 1) << 7;
int2_cfg |= (lis3dh->cfg.int2.xl & 1);
int2_cfg |= (lis3dh->cfg.int2.xh & 1) << 1;
int2_cfg |= (lis3dh->cfg.int2.yl & 1) << 2;
int2_cfg |= (lis3dh->cfg.int2.yh & 1) << 3;
int2_cfg |= (lis3dh->cfg.int2.zl & 1) << 4;
int2_cfg |= (lis3dh->cfg.int2.zh & 1) << 5;
int2_cfg |= (lis3dh->cfg.int2.en_6d & 1) << 6;
int2_cfg |= (lis3dh->cfg.int2.aoi & 1) << 7;
/* duration values */
int1_dur = lis3dh->cfg.int1_dur & 0x7F;
int2_dur = lis3dh->cfg.int2_dur & 0x7F;
/* threshold values */
int1_ths = lis3dh->cfg.int1_ths & 0x7F;
int2_ths = lis3dh->cfg.int2_ths & 0x7F;
/* set enable FIFO if a mode is set and size (watermark) is not 0 */
if (lis3dh->cfg.fifo.mode != 0xFF && lis3dh->cfg.fifo.size != 0) {
ctrl_reg5 |= 0x40; /* bit FIFO_EN */
fifo_ctrl_reg |= ((lis3dh->cfg.fifo.size - 1) & 0x1F);
fifo_ctrl_reg |= (lis3dh->cfg.fifo.mode << 6);
fifo_ctrl_reg |= ((lis3dh->cfg.fifo.trig & 1) << 5);
}
/* set enable filter if mode is set */
if (lis3dh->cfg.filter.mode != 0xFF) {
ctrl_reg2 |= ((lis3dh->cfg.filter.mode & 0x03) << 6);
ctrl_reg2 |= ((lis3dh->cfg.filter.cutoff & 0x03) << 4);
ctrl_reg2 |= ((lis3dh->cfg.filter.fds & 1) << 3);
ctrl_reg2 |= ((lis3dh->cfg.filter.click & 1) << 2);
ctrl_reg2 |= ((lis3dh->cfg.filter.ia2 & 1) << 1);
ctrl_reg2 |= (lis3dh->cfg.filter.ia1 & 1);
}
/* always set block update (BDU) */
/* guarantees all bytes for one [x y z] set were sampled at the same time */
ctrl_reg4 |= 0x80;
/* set high resolution */
if (lis3dh->cfg.mode == LIS3DH_MODE_HR) {
ctrl_reg4 |= 0x08;
}
/* set LPen */
if (lis3dh->cfg.mode == LIS3DH_MODE_LP) {
ctrl_reg1 |= 0x08;
}
/* ctrl_reg0 is 0x10 | (SDO << 7) */
ctrl_reg0 |= 0x10;
ctrl_reg0 |= (lis3dh->cfg.sdo_pullup & 1) << 7;
/* Registers have to be set in this order for SPI to function correctly, I think */
err |= lis3dh->dev.write(REG_CTRL_REG4, ctrl_reg4);
err |= lis3dh->dev.write(REG_CTRL_REG5, ctrl_reg5);
err |= lis3dh->dev.write(REG_FIFO_CTRL_REG, fifo_ctrl_reg);
err |= lis3dh->dev.write(REG_INT1_CFG, int1_cfg);
err |= lis3dh->dev.write(REG_INT1_THS, int1_ths);
err |= lis3dh->dev.write(REG_INT1_DURATION, int1_dur);
err |= lis3dh->dev.write(REG_INT2_CFG, int2_cfg);
err |= lis3dh->dev.write(REG_INT2_THS, int2_ths);
err |= lis3dh->dev.write(REG_INT2_DURATION, int2_dur);
err |= lis3dh->dev.write(REG_CLICK_CFG, click_cfg);
err |= lis3dh->dev.write(REG_CLICK_THS, click_ths);
err |= lis3dh->dev.write(REG_TIME_LIMIT, time_limit);
err |= lis3dh->dev.write(REG_TIME_LATENCY, lis3dh->cfg.time_latency);
err |= lis3dh->dev.write(REG_TIME_WINDOW, lis3dh->cfg.time_window);
err |= lis3dh->dev.write(REG_ACT_THS, act_ths);
err |= lis3dh->dev.write(REG_ACT_DUR, lis3dh->cfg.act_dur);
err |= lis3dh->dev.write(REG_TEMP_CFG_REG, temp_cfg_reg);
err |= lis3dh->dev.write(REG_REFERENCE, lis3dh->cfg.reference);
err |= lis3dh->dev.write(REG_CTRL_REG6, ctrl_reg6);
err |= lis3dh->dev.write(REG_CTRL_REG0, ctrl_reg0);
err |= lis3dh->dev.write(REG_CTRL_REG2, ctrl_reg2);
err |= lis3dh->dev.write(REG_CTRL_REG3, ctrl_reg3);
err |= lis3dh->dev.write(REG_CTRL_REG1, ctrl_reg1);
lis3dh->dev.sleep(100000); /* 100 ms */
return err;
}
/* The "real size" of the i16 accelerometer axis reading depends on the power mode. */
/* shift down the 16 bit word by this amount: */
static uint8_t acc_shift(uint8_t mode) {
switch (mode) {
case LIS3DH_MODE_HR: return 4; /* i12 */
case LIS3DH_MODE_NORMAL: return 6; /* i10 */
case LIS3DH_MODE_LP: return 8; /* i8 */
default: return 0;
}
}
/* returns a scalar that when multiplied with axis reading */
/* turns it to a multiple of mg (1/1000 g). */
static uint8_t acc_sensitivity(uint8_t mode, uint8_t range) {
switch (range) {
case LIS3DH_FS_2G: return (mode == LIS3DH_MODE_LP) ? 16 : (mode == LIS3DH_MODE_NORMAL) ? 4 : 1;
case LIS3DH_FS_4G: return (mode == LIS3DH_MODE_LP) ? 32 : (mode == LIS3DH_MODE_NORMAL) ? 8 : 2;
case LIS3DH_FS_8G: return (mode == LIS3DH_MODE_LP) ? 64 : (mode == LIS3DH_MODE_NORMAL) ? 16 : 4;
case LIS3DH_FS_16G: return (mode == LIS3DH_MODE_LP) ? 192 : (mode == LIS3DH_MODE_NORMAL) ? 48 : 12;
default: return 0;
}
}
/* read a single [x y z] set. */
int lis3dh_read(lis3dh_t *lis3dh) {
uint8_t data[6];
uint8_t shift, sens, status;
int err = 0;
if (!lis3dh) {
return 1;
}
shift = acc_shift(lis3dh->cfg.mode);
sens = acc_sensitivity(lis3dh->cfg.mode, lis3dh->cfg.range);
/* poll STATUS_REG until new data is available */
do {
err |= lis3dh->dev.read(REG_STATUS_REG, &status, 1);
lis3dh->dev.sleep(1000);
} while ((!LIS3DH_STATUS_ZYXDA(status) || !LIS3DH_STATUS_ZYXOR(status)) && !err);
err |= lis3dh->dev.read(REG_OUT_X_L, data, 6);
lis3dh->acc.x = (int16_t)((int16_t)(data[1] | data[0] << 8) >> shift) * sens;
lis3dh->acc.y = (int16_t)((int16_t)(data[3] | data[2] << 8) >> shift) * sens;
lis3dh->acc.z = (int16_t)((int16_t)(data[5] | data[4] << 8) >> shift) * sens;
return err;
}
/* assume fifo has been configured */
/* wait until FIFO is NOT empty, then */
/* read groups of the 6 OUT bytes until EMPTY flag in FIFO_SRC is set */
int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) {
uint8_t data[6];
uint8_t sens, fifo_src, idx = 0;
int err = 0;
if (!lis3dh || !fifo) {
return 1;
}
/* wait until there is at least 1 unread sample in the FIFO */
do {
err |= lis3dh->dev.read(REG_FIFO_SRC_REG, &fifo_src, 1);
lis3dh->dev.sleep(1000);
} while (!LIS3DH_FIFO_SRC_UNREAD(fifo_src) && !err);
/* FIFO is always 10-bit / normal mode */
sens = acc_sensitivity(LIS3DH_MODE_NORMAL, lis3dh->cfg.range);
do {
err |= lis3dh->dev.read(REG_OUT_X_L, data, 6);
err |= lis3dh->dev.read(REG_FIFO_SRC_REG, &fifo_src, 1);
fifo->x[idx] = ((int16_t)(data[1] | data[0] << 8) >> 6) * sens;
fifo->y[idx] = ((int16_t)(data[3] | data[2] << 8) >> 6) * sens;
fifo->z[idx] = ((int16_t)(data[5] | data[4] << 8) >> 6) * sens;
} while (idx++ < lis3dh->cfg.fifo.size - 1 && !LIS3DH_FIFO_SRC_EMPTY(fifo_src) && !err);
/* the device stores FIFO offsets rather than `size' so a size-32 FIFO */
/* has an offset of 31 ^ */
fifo->size = idx;
return err;
}
/* if NULL, this function doesn't have to be called */
int lis3dh_deinit(lis3dh_t *lis3dh) {
if (!lis3dh) {
return 1;
}
if (lis3dh->dev.deinit != NULL) {
return lis3dh->dev.deinit();
}
return 0;
}
/* read INT1_SRC to clear interrupt active flag and get relevant data */
int lis3dh_read_int1(lis3dh_t *lis3dh) {
if (!lis3dh) {
return 1;
}
return lis3dh->dev.read(REG_INT1_SRC, &lis3dh->src.int1, 1);
}
/* read INT2_SRC to clear interrupt active flag and get relevant data */
int lis3dh_read_int2(lis3dh_t *lis3dh) {
if (!lis3dh) {
return 1;
}
return lis3dh->dev.read(REG_INT2_SRC, &lis3dh->src.int2, 1);
}
/* read CLICK_SRC to clear interrupt active flag and get relevant data */
int lis3dh_read_click(lis3dh_t *lis3dh) {
if (!lis3dh) {
return 1;
}
return lis3dh->dev.read(REG_CLICK_SRC, &lis3dh->src.click, 1);
}
/* read REFERENCE reg to reset HP filter in REFERENCE mode */
/* it then uses the --current-- acceleration as the base in the filter */
int lis3dh_reference(lis3dh_t *lis3dh) {
uint8_t res;
if (!lis3dh) {
return 1;
}
return lis3dh->dev.read(REG_REFERENCE, &res, 1);
}
/* reset user regs and reload trim params */
int lis3dh_reset(lis3dh_t *lis3dh) {
int err = 0;
if (!lis3dh) {
return 1;
}
/* set BOOT bit so device reloads internal trim parameters */
err |= lis3dh->dev.write(REG_CTRL_REG5, 0x80);
/* wait 30 ms */
lis3dh->dev.sleep(30000);
/* write default values to rw regs */
err |= lis3dh->dev.write(REG_CTRL_REG0, 0x10);
err |= lis3dh->dev.write(REG_CTRL_REG1, 0x07);
err |= lis3dh->dev.write(REG_CTRL_REG2, 0x00);
err |= lis3dh->dev.write(REG_CTRL_REG3, 0x00);
err |= lis3dh->dev.write(REG_CTRL_REG4, 0x00);
err |= lis3dh->dev.write(REG_CTRL_REG5, 0x00);
err |= lis3dh->dev.write(REG_CTRL_REG6, 0x00);
err |= lis3dh->dev.write(REG_FIFO_CTRL_REG, 0x00);
err |= lis3dh->dev.write(REG_INT1_CFG, 0x00);
err |= lis3dh->dev.write(REG_INT1_THS, 0x00);
err |= lis3dh->dev.write(REG_INT1_DURATION, 0x00);
err |= lis3dh->dev.write(REG_INT2_CFG, 0x00);
err |= lis3dh->dev.write(REG_INT2_THS, 0x00);
err |= lis3dh->dev.write(REG_INT2_DURATION, 0x00);
err |= lis3dh->dev.write(REG_CLICK_CFG, 0x00);
err |= lis3dh->dev.write(REG_CLICK_THS, 0x00);
err |= lis3dh->dev.write(REG_TIME_LIMIT, 0x00);
err |= lis3dh->dev.write(REG_TIME_LATENCY, 0x00);
err |= lis3dh->dev.write(REG_TIME_WINDOW, 0x00);
err |= lis3dh->dev.write(REG_ACT_THS, 0x00);
err |= lis3dh->dev.write(REG_ACT_DUR, 0x00);
/* set BOOT bit again so device reloads internal trim parameters */
err |= lis3dh->dev.write(REG_CTRL_REG5, 0x80);
/* wait 30 ms */
lis3dh->dev.sleep(30000);
return err;
}
/* read all 3 ADCs and convert data depending on power mode */
/* result: 1 lsb is equal to 1 millivolt */
int lis3dh_read_adc(lis3dh_t *lis3dh) {
uint8_t data[6];
uint8_t shift;
int err = 0;
if (!lis3dh) {
return 1;
}
err |= lis3dh->dev.read(REG_OUT_ADC1_L, data, 6);
shift = (lis3dh->cfg.mode == LIS3DH_MODE_LP) ? 8 : 6;
lis3dh->adc.adc1 = (int16_t)(1200 + ((800 * ((int16_t)(data[1] | data[0] << 8) >> shift)) >> (16 - shift)));
lis3dh->adc.adc2 = (int16_t)(1200 + ((800 * ((int16_t)(data[3] | data[2] << 8) >> shift)) >> (16 - shift)));
lis3dh->adc.adc3 = (int16_t)(1200 + ((800 * ((int16_t)(data[5] | data[4] << 8) >> shift)) >> (16 - shift)));
return err;
}
/* the temperature sensor only reports the difference between its current temp, */
/* and the factory calibrated temperature, 25 celsius. */
/* in increments of plus or negative 1 unit celsius. */
/* the reported temperature is stored inplace of adc3 */
/* temp sensing is always in 8-bit mode */
/* operating range: -40 to 85 celsius */
/* 1 lsb = 1 deg C */
int lis3dh_read_temp(lis3dh_t *lis3dh) {
uint8_t data;
int err = 0;
if (!lis3dh) {
return 1;
}
err |= lis3dh->dev.read(REG_OUT_ADC3_H, &data, 1);
lis3dh->adc.adc3 = (int8_t)data + 25;
return err;
}
/* This function is meant to be used to reset the FIFO in `FIFO' mode. */
int lis3dh_fifo_reset(lis3dh_t *lis3dh) {
int err = 0;
uint8_t fifo_ctrl_reg = 0;
if (!lis3dh) {
return 1;
}
/* create a FIFO_MODE_BYPASS config */
fifo_ctrl_reg |= ((lis3dh->cfg.fifo.size - 1) & 0x1F);
fifo_ctrl_reg |= (LIS3DH_FIFO_MODE_BYPASS << 6);
fifo_ctrl_reg |= ((lis3dh->cfg.fifo.trig & 1) << 5);
/* write it to the device */
err |= lis3dh->dev.write(REG_FIFO_CTRL_REG, fifo_ctrl_reg);
lis3dh->dev.sleep(1000);
/* re-create original FIFO mode config */
fifo_ctrl_reg |= (lis3dh->cfg.fifo.mode << 6);
/* write to device to immediately start FIFO sampling process */
err |= lis3dh->dev.write(REG_FIFO_CTRL_REG, fifo_ctrl_reg);
return err;
}

324
lis3dh.h Normal file
View File

@ -0,0 +1,324 @@
#ifndef LIS3DH_H
#define LIS3DH_H
#include <stdint.h>
/* macros for checking INT_SRC registers upon interrupt */
#define LIS3DH_INT_SRC_X_LOW(c) (!!(c & 0x01)) /* X axis low */
#define LIS3DH_INT_SRC_X_HIGH(c) (!!(c & 0x02)) /* X axis high */
#define LIS3DH_INT_SRC_Y_LOW(c) (!!(c & 0x04)) /* Y axis low */
#define LIS3DH_INT_SRC_Y_HIGH(c) (!!(c & 0x08)) /* Y axis high */
#define LIS3DH_INT_SRC_Z_LOW(c) (!!(c & 0x10)) /* Z axis low */
#define LIS3DH_INT_SRC_Z_HIGH(c) (!!(c & 0x20)) /* Z axis high */
#define LIS3DH_INT_SRC_IA(c) (!!(c & 0x40)) /* Interrupt active */
/* macros for checking CLICK_SRC register upon interrupt */
#define LIS3DH_CLICK_SRC_X(c) (!!(c & 0x01)) /* X high event */
#define LIS3DH_CLICK_SRC_Y(c) (!!(c & 0x02)) /* Y high event */
#define LIS3DH_CLICK_SRC_Z(c) (!!(c & 0x04)) /* Z high event */
#define LIS3DH_CLICK_SIGN(c) (!!(c & 0x08)) /* Click sign, 1 = positive */
#define LIS3DH_CLICK_SCLICK(c) (!!(c & 0x10)) /* Single-click det. enabled */
#define LIS3DH_CLICK_DCLICK(c) (!!(c & 0x20)) /* Double-click det. enabled */
/* macros for checking FIFO_SRC register */
#define LIS3DH_FIFO_SRC_UNREAD(c) (c & 0x1F) /* FSS / current unread samples in FIFO */
#define LIS3DH_FIFO_SRC_EMPTY(c) (!!(c & 0x20)) /* FIFO is empty */
#define LIS3DH_FIFO_SRC_OVRN(c) (!!(c & 0x40)) /* FIFO has overrun (full 32) */
#define LIS3DH_FIFO_SRC_WTM(c) (!!(c & 0x80)) /* FIFO watermark hit (not full 32) */
/* macros for checking STATUS register */
#define LIS3DH_STATUS_XDA(c) (!!(c & 0x01)) /* X-axis data available */
#define LIS3DH_STATUS_YDA(c) (!!(c & 0x02)) /* Y-axis data available */
#define LIS3DH_STATUS_ZDA(c) (!!(c & 0x04)) /* Z-axis data available */
#define LIS3DH_STATUS_ZYXDA(c) (!!(c & 0x08)) /* {Z,Y,X}-axis data available */
#define LIS3DH_STATUS_XOR(c) (!!(c & 0x10)) /* X-axis data has overrun (been overwritten) */
#define LIS3DH_STATUS_YOR(c) (!!(c & 0x20)) /* Y-axis data has overrun (been overwritten) */
#define LIS3DH_STATUS_ZOR(c) (!!(c & 0x40)) /* Z-axis data has overrun (been overwritten) */
#define LIS3DH_STATUS_ZYXOR(c) (!!(c & 0x80)) /* {Z,Y,X}-axis data has overrun (been overwritten) */
/* rates */
/* all power modes */
#define LIS3DH_ODR_POWEROFF 0x00
#define LIS3DH_ODR_1_HZ 0x01
#define LIS3DH_ODR_10_HZ 0x02
#define LIS3DH_ODR_25_HZ 0x03
#define LIS3DH_ODR_50_HZ 0x04
#define LIS3DH_ODR_100_HZ 0x05
#define LIS3DH_ODR_200_HZ 0x06
#define LIS3DH_ODR_400_HZ 0x07
/* only normal mode */
#define LIS3DH_ODR_NORM_1344_HZ 0x09
/* only low-power mode */
#define LIS3DH_ODR_LP_1600_HZ 0x08
#define LIS3DH_ODR_LP_5376_HZ 0x09
/* range/sens */
#define LIS3DH_FS_2G 0x00 /* Full-scale sensing range: ± 2G */
#define LIS3DH_FS_4G 0x01 /* Full-scale sensing range: ± 4G */
#define LIS3DH_FS_8G 0x02 /* Full-scale sensing range: ± 8G */
#define LIS3DH_FS_16G 0x03 /* Full-scale sensing range: ± 16G */
/* operating modes */
#define LIS3DH_MODE_HR 0x00 /* High resolution: 12-bit */
#define LIS3DH_MODE_LP 0x01 /* Low-power: 8-bit */
#define LIS3DH_MODE_NORMAL 0x02 /* Normal: 10-bit */
/* FIFO modes */
/* FIFO is not operational, and the buffer is reset. Must be used for switching FIFO modes */
#define LIS3DH_FIFO_MODE_BYPASS 0x00
/* Once FIFO fills up completely, it must be reset to be used again */
#define LIS3DH_FIFO_MODE_FIFO 0x01
/* Continously (over)writes buffer until it is read */
#define LIS3DH_FIFO_MODE_STREAM 0x02
/* Stream mode, but automatically switches to FIFO mode once a set interrupt has occurred */
#define LIS3DH_FIFO_MODE_STREAM_TO_FIFO 0x03
/* FIFO trigger pin selection */
#define LIS3DH_FIFO_TRIG_INT1 0x00
#define LIS3DH_FIFO_TRIG_INT2 0x01
/* Reference mode
* Output [x y z] data is calculated as the difference between the
* measured acceleration and the value stored in REFERENCE.
* signed 7-bit int, 1 LSb value depends on FS.
* FS_2G: ~ 16mg per 1 LSb
* FS_4G: ~ 31mg per 1 LSb
* FS_8G: ~ 63mg per 1 LSb
* FS_16G: ~127mg per 1 LSb */
#define LIS3DH_FILTER_MODE_REFERENCE 0x01
/* Normal mode
* Probably the same as 0x00 */
#define LIS3DH_FILTER_MODE_NORMAL 0x02
/* Autoreset mode
* The filter is automatically reset upon configured interrupt event.
* It can be reset at any time by reading REFERENCE. */
#define LIS3DH_FILTER_MODE_AUTORESET 0x03
/* filter cutoff */
/* unfortunately, there is only a table for low-power mode,
* and the actual cutoff-frequency depends on the ODR.
* Naming scheme after ODR@400Hz
* AN3308 > section 4.3.1.1 */
#define LIS3DH_FILTER_CUTOFF_8 0x00 /* highest freq */
#define LIS3DH_FILTER_CUTOFF_4 0x01
#define LIS3DH_FILTER_CUTOFF_2 0x02
#define LIS3DH_FILTER_CUTOFF_1 0x03 /* lowest freq */
/* Positive and negative axis self-test */
#define LIS3DH_SELF_TEST_0 0x01
#define LIS3DH_SELF_TEST_1 0x02
/* Note: IA{1,2} is interrupt activity {1,2} or interrupt generators */
/* user provided functions, init and deinit can be set to NULL and won't be used */
struct lis3dh_device {
int (*init)(void);
int (*read)(uint8_t reg, uint8_t *dst, uint32_t size);
int (*write)(uint8_t reg, uint8_t value);
int (*sleep)(uint32_t dur_us);
int (*deinit)(void);
};
struct lis3dh_click_config {
uint8_t zd; /* double click interrupt on Z-axis */
uint8_t zs; /* single click interrupt on Z-axis */
uint8_t yd; /* double click interrupt on Y-axis */
uint8_t ys; /* single click interrupt on Y-axis */
uint8_t xd; /* double click interrupt on X-axis */
uint8_t xs; /* single click interrupt on X-axis */
uint8_t latch; /* active until CLICK_SRC is read */
};
/* INT1_CFG and INT2_CFG have identical struct */
struct lis3dh_int_config {
uint8_t aoi; /* AND/OR combination of int events */
uint8_t en_6d; /* 6 direction detection */
uint8_t en_4d; /* both en_6d and en_4d must = 1 for 4D to work ! */
uint8_t zh; /* interrupt generation on Z high event / Dir. recog. */
uint8_t zl; /* interrupt generation on Z low event / Dir. recog. */
uint8_t yh; /* interrupt generation on Y high event / Dir. recog. */
uint8_t yl; /* interrupt generation on Y low event / Dir. recog. */
uint8_t xh; /* interrupt generation on X high event / Dir. recog. */
uint8_t xl; /* interrupt generation on X low event / Dir. recog. */
uint8_t latch; /* active until INT1_SRC/INT2_SRC is read */
};
/* config for interrupt output pin #2 */
struct lis3dh_int_pin2_config {
uint8_t click; /* CLICK interrupt */
uint8_t ia1; /* IA1 interrupt */
uint8_t ia2; /* IA2 interrupt */
uint8_t boot; /* enable BOOT on pin 2 */
uint8_t act; /* interrupt on activity */
uint8_t polarity; /* INT1 & INT2 polarity. 0 active high, 1 active low */
};
/* config for interrupt output pin #1 */
struct lis3dh_int_pin1_config {
uint8_t click; /* CLICK interrupt */
uint8_t ia1; /* IA1 interrupt */
uint8_t ia2; /* IA2 interrupt */
uint8_t drdy_zyxda; /* new [xyz] data ready (not via FIFO) */
uint8_t drdy_321; /* not sure */
uint8_t wtm; /* FIFO reached watermark level */
uint8_t overrun; /* FIFO has overrun */
};
/* config for high-pass filter */
struct lis3dh_filter_config {
uint8_t mode; /* filter mode, reset behaviour */
uint8_t cutoff; /* high-pass filter cutoff freq (~ ODR) */
uint8_t fds; /* ¬(bypass filter) */
uint8_t click; /* enable filter for CLICK function */
uint8_t ia2; /* enable filter for AOI func on INT 2 */
uint8_t ia1; /* enable filter for AOI func on INT 1 */
};
/* config for FIFO */
struct lis3dh_fifo_config {
uint8_t size; /* size of fifo; 0-32; 0=don't use. fth=[1-32]-1 (index) */
uint8_t trig; /* pin to trigger when watermark/overrun occurs */
uint8_t mode; /* FIFO mode */
};
struct lis3dh_config {
uint8_t rate; /* ODR */
uint8_t range; /* FS */
uint8_t mode; /* LPen and HR */
struct lis3dh_fifo_config fifo;
struct lis3dh_filter_config filter;
struct lis3dh_int_pin1_config pin1;
struct lis3dh_int_pin2_config pin2;
struct lis3dh_int_config int1;
struct lis3dh_int_config int2;
struct lis3dh_click_config click;
/* 1 LSb = 16 mg @ FS_2G
* 1 LSb = 32 mg @ FS_4G
* 1 LSb = 62 mg @ FS_8G
* 1 LSb = 186 mg @ FS_16G
*/
uint8_t int1_ths; /* 7-bit INT 1 threshold value */
uint8_t int2_ths; /* 7-bit INT 2 threshold value */
uint8_t click_ths; /* 7-bit CLICK threshold value */
uint8_t act_ths; /* 7-bit ACT threshold value */
/* Duration time is measured in N/ODR where:
* --- N = The content of the intX_dur integer
* --- ODR = the data rate, eg 100, 400...
* [ODR] [1 LSb in milliseconds]
* 1 1000
* 10 100
* 25 40
* 50 20
* 100 10
* 200 5
* 400 2.5
* 1600 0.6
* 1344 0.744
* 5376 0.186
* (Note: this calc also applies to:
* - time_limit
* - time_window
* - time_latency
* used in CLICK)
*
* this is the "1[LSb]" in act_dur calc
*/
uint8_t int1_dur; /* 7-bit INT 1 duration value */
uint8_t int2_dur; /* 7-bit INT 2 duration value */
/* Sleep-to-wake and return-to-sleep duration
* 1 LSb = (8 * 1[LSb] + 1) / ODR
*/
uint8_t act_dur; /* 8-bit ACT duration value */
uint8_t time_limit; /* 7-bit time limit ~ CLICK */
uint8_t time_latency; /* 8-bit time latency ~ CLICK */
uint8_t time_window; /* 8-bit time window ~ CLICK */
uint8_t sdo_pullup; /* Use pull-up on SDO. default 0 use */
uint8_t en_adc; /* enable ADC */
uint8_t en_temp; /* enable temp sensor on ADC 3 */
uint8_t spi3w; /* set to 1 to enable 3-way SPI, default 0 */
uint8_t self_test; /* set self-test mode */
uint8_t reference; /* HP filter reference */
};
/* reads from internal ADCs.
* Input range: 800 mV to 1600 mV
* Resolution:
* 8-bit in LP mode
* 10-bit in normal and in HR mode.
* Sampling frequency:
* same as ODR
* Output:
* actual value in mV
* 1 lsb = 1 mv
* Temperature:
* 1 lsb = 1 deg C
*/
struct lis3dh_adc {
int16_t adc1;
int16_t adc2;
int16_t adc3;
};
/* accel data read not using FIFO */
/* 1 lsb = 1 mg */
struct lis3dh_accel {
int16_t x;
int16_t y;
int16_t z;
};
/* stores interrupt source registers read from the device */
struct lis3dh_int_src {
uint8_t int1;
uint8_t int2;
uint8_t click;
};
struct lis3dh {
struct lis3dh_device dev; /* fn ptrs to interface w/ device */
struct lis3dh_config cfg; /* config variables to write to device */
struct lis3dh_accel acc; /* accel data read from device (not FIFO) */
struct lis3dh_adc adc; /* adc and optionally temp read from device */
struct lis3dh_int_src src; /* INT_SRC registers read from device */
};
typedef struct lis3dh lis3dh_t;
/* struct for containing the FIFO data */
/* 1 lsb = 1 mg */
struct lis3dh_fifo_data {
uint8_t size; /* up to 31 */
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
int lis3dh_init(lis3dh_t *lis3dh);
int lis3dh_deinit(lis3dh_t *lis3dh);
int lis3dh_configure(lis3dh_t *lis3dh);
int lis3dh_read(lis3dh_t *lis3dh);
int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo);
int lis3dh_read_int1(lis3dh_t *lis3dh);
int lis3dh_read_int2(lis3dh_t *lis3dh);
int lis3dh_read_click(lis3dh_t *lis3dh);
int lis3dh_reference(lis3dh_t *lis3dh);
int lis3dh_reset(lis3dh_t *lis3dh);
int lis3dh_read_adc(lis3dh_t *lis3dh);
int lis3dh_read_temp(lis3dh_t *lis3dh);
int lis3dh_fifo_reset(lis3dh_t *lis3dh);
#endif

53
main.c Normal file
View File

@ -0,0 +1,53 @@
#define _GNU_SOURCE
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include "lis3dh.h"
#include "i2c.h"
/* #include "spi.h" */
int main(void) {
lis3dh_t lis;
struct lis3dh_fifo_data fifo;
int i;
lis.dev.init = i2c_init;
lis.dev.read = i2c_read;
lis.dev.write = i2c_write;
lis.dev.sleep = usleep;
lis.dev.deinit = i2c_deinit;
/*
lis.dev.init = spi_init;
lis.dev.read = spi_read;
lis.dev.write = spi_write;
lis.dev.sleep = usleep;
lis.dev.deinit = spi_deinit;
*/
lis3dh_init(&lis);
lis3dh_reset(&lis);
lis.cfg.mode = LIS3DH_MODE_HR;
lis.cfg.range = LIS3DH_FS_2G;
lis.cfg.rate = LIS3DH_ODR_100_HZ;
lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_FIFO;
lis3dh_configure(&lis);
for ( ;; ) {
usleep(100000); /* 100 ms */
lis3dh_read_fifo(&lis, &fifo);
for(i=0; i<fifo.size; i++)
printf("x: %4.4d mg, y: %4.4d mg, z: %4.4d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
printf("fifo.size=%d\n", fifo.size);
lis3dh_fifo_reset(&lis);
}
lis3dh_deinit(&lis);
return 0;
}

141
spi.c Normal file
View File

@ -0,0 +1,141 @@
/*
Example SPI use on linux/raspberry pi
*/
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>
#include <string.h>
#include "spi.h"
#define SPI_DEVICE "/dev/spidev0.0"
#define SPI_SPEED 1000 * 1000 /* 1 MHz */
/*
* Pinout config for this example
*
* MOSI - GPIO 10 (physical pin 19) => LIS3DH "SDA" or "SDI"
* MISO - GPIO 9 (physical pin 21) => LIS3DH "SDO"
* SCLK - GPIO 11 (physical pin 23) => LIS3DH "SCL"
* CE0 - GPIO 8 (physical pin 24) => LIS3DH "!CS"
*
*/
static int fd;
int spi_init(void) {
uint8_t mode = SPI_MODE_0;
uint8_t bits = 8;
uint32_t speed = SPI_SPEED;
if ((fd = open(SPI_DEVICE, O_RDWR)) < 0) {
fprintf(stderr, "spi_init(): open(%s)\n", SPI_DEVICE);
return 1;
}
if (ioctl(fd, SPI_IOC_RD_MODE, &mode) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_RD_MODE\n");
return 1;
}
if (ioctl(fd, SPI_IOC_WR_MODE, &mode) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_WR_MODE\n");
return 1;
}
if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_WR_BITS_PER_WORD\n");
return 1;
}
if (ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_RD_BITS_PER_WORD\n");
return 1;
}
if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_WR_MAX_SPEED_HZ\n");
return 1;
}
if (ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed) == -1) {
fprintf(stderr, "spi_init(): SPI_IOC_RD_MAX_SPEED_HZ\n");
return 1;
}
return 0;
}
int spi_read(uint8_t reg, uint8_t *dst, uint32_t size) {
uint8_t send[2];
struct spi_ioc_transfer tr[2] = {0};
/* clear 2 MSbits */
reg &= 0x3F;
/* set READ bit */
reg |= 0x80;
if (size > 1) {
/* set AUTO INC bit */
reg |= 0x40;
}
send[0] = reg;
send[1] = 0x00;
tr[0].tx_buf = (unsigned long) send;
tr[0].rx_buf = (unsigned long) 0;
tr[0].len = 2;
tr[1].tx_buf = (unsigned long) 0;
tr[1].rx_buf = (unsigned long) dst;
tr[1].len = size;
if (ioctl(fd, SPI_IOC_MESSAGE(2), tr) < 0) {
fprintf(stderr, "spi_read(): error ioctl()\n");
return 1;
}
return 0;
}
int spi_write(uint8_t reg, uint8_t value) {
struct spi_ioc_transfer tr[2] = {0};
/* clear 2 MSbits */
reg &= 0x3F;
tr[0].tx_buf = (unsigned long) &reg;
tr[0].rx_buf = (unsigned long) 0;
tr[0].len = 1;
tr[1].tx_buf = (unsigned long) &value;
tr[1].rx_buf = (unsigned long) 0;
tr[1].len = 1;
if (ioctl(fd, SPI_IOC_MESSAGE(2), tr) < 0) {
fprintf(stderr, "spi_write(): error ioctl()\n");
return 1;
}
return 0;
}
int spi_deinit(void) {
if (fd) {
close(fd);
}
return 0;
}

11
spi.h Normal file
View File

@ -0,0 +1,11 @@
#ifndef SPI_H
#define SPI_H
#include <stdint.h>
int spi_init(void);
int spi_read(uint8_t reg, uint8_t *dst, uint32_t size);
int spi_write(uint8_t reg, uint8_t value);
int spi_deinit(void);
#endif