Compare commits
No commits in common. "91ada989f6abdbb5de7cc7b311c3b553bc4bb3b7" and "8648c517f65b5e01027828512c40dbb6b539f1b6" have entirely different histories.
91ada989f6
...
8648c517f6
35
README.md
35
README.md
@ -25,11 +25,11 @@ This project has example interface code for I2C and SPI used on Raspberry Pi 4,
|
||||
```c
|
||||
/* initialise the "interface" */
|
||||
int init(void);
|
||||
/* read from register `reg', `size' amount of bytes, and write them to `dst' */
|
||||
/* 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' */
|
||||
/* write `value` to register `reg` */
|
||||
int write(uint8_t reg, uint8_t value);
|
||||
/* sleep for `dur_us' microseconds */
|
||||
/* sleep for `dur_us` microseconds */
|
||||
int sleep(uint32_t dur_us);
|
||||
/* deinitalise the "interface" */
|
||||
int deinit(void);
|
||||
@ -39,30 +39,10 @@ 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
|
||||
|
||||
### STM32
|
||||
```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);
|
||||
```
|
||||
All functions return `0` on success, and any non-zero value on error.
|
||||
|
||||
## STM32
|
||||
Example i2c and SPI functions that work
|
||||
|
||||
### i2c
|
||||
```c
|
||||
#define LIS3DH_I2C_ADDR 0x18 /* can also be 0x19 */
|
||||
#define LIS3DH_I2C_ADDR 0x18
|
||||
|
||||
int i2c_write(uint8_t reg, uint8_t value) {
|
||||
uint8_t buf[2] = { reg, value };
|
||||
@ -81,8 +61,3 @@ int i2c_read(uint8_t reg, uint8_t *dst, uint32_t size) {
|
||||
}
|
||||
```
|
||||
|
||||
### SPI
|
||||
```c
|
||||
TODO
|
||||
```
|
||||
|
||||
|
@ -25,7 +25,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* reset device just in case */
|
||||
/* reset device because it sometimes corrupts itself */
|
||||
if (lis3dh_reset(&lis)) {
|
||||
/* error handling */
|
||||
}
|
||||
|
@ -25,7 +25,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* reset device just in case */
|
||||
/* reset device because it sometimes corrupts itself */
|
||||
if (lis3dh_reset(&lis)) {
|
||||
/* error handling */
|
||||
}
|
||||
@ -106,7 +106,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* only print if INT1 interrupt active (IA) = 1*/
|
||||
/* 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 .. */
|
||||
|
@ -25,7 +25,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* reset device just in case */
|
||||
/* reset device because it sometimes corrupts itself */
|
||||
if (lis3dh_reset(&lis)) {
|
||||
/* error handling */
|
||||
}
|
||||
@ -107,7 +107,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* only print if INT1 interrupt active (IA) = 1*/
|
||||
/* only print if INT1 interrupt active = 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",
|
||||
|
@ -25,7 +25,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* reset device just in case */
|
||||
/* reset device because it sometimes corrupts itself */
|
||||
if (lis3dh_reset(&lis)) {
|
||||
/* error handling */
|
||||
}
|
||||
@ -107,7 +107,7 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* only print if INT1 interrupt active (IA) = 1*/
|
||||
/* only print if INT1 interrupt active = 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",
|
||||
|
@ -4,22 +4,15 @@
|
||||
Basic example of how to use this device
|
||||
|
||||
### file: fifo.c
|
||||
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()`.
|
||||
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 LIS3DH can optionally apply a HP filter on the sampling to reduce 'static acceleration' from the data.
|
||||
|
||||
Note: it seems that the highest data rate (ODR) possible using FIFO is 200 Hz, faster than that and it does not want to restart after 1 FIFO buffer. To sample faster, such as at the advertised 5 KHz rate, you have to use `lis3dh_read()` with `LP` mode.
|
||||
Note: it seems that the highest data rate (ODR) possible using FIFO is 200 Hz, faster than that and it does not want to restart after 1 FIFO buffer. To faster faster, such as at the advertised 5 KHz rate, you have to use `lis3dh_read()`.
|
||||
|
||||
### file: interrupts.c
|
||||
This device supports two different interrupt "output pins," `INT1` and `INT2`. The appropriate flag must be set in either `cfg.pin1` or `cfg.pin2` and the interrupt source must be configured to trigger into `INT1` or `INT2`.
|
||||
|
||||
This file contains example code that listens and receives an interrupt when the FIFO overrun is reached i.e. it is full.
|
||||
|
||||
Note: `pin1.wtm` will NOT trigger if the FIFO size is 32 (default). Use `pin1.overrun` if you want an interrupt when the FIFO is full at full size (32.)
|
||||
This file contains example code that listens and receives an interrupt when the FIFO overrun is reached i.e. it is full. Note: `pin1.wtm` will NOT trigger if the FIFO size is 32 (default). Use `pin1.overrun` if you want an interrupt when the FIFO is full at full size (32.)
|
||||
|
||||
### file: single-click.c
|
||||
|
||||
|
@ -7,7 +7,7 @@
|
||||
int main() {
|
||||
|
||||
lis3dh_t lis;
|
||||
struct lis3dh_fifo_data fifo;
|
||||
struct lis3dh_fifo_data data;
|
||||
int i;
|
||||
|
||||
lis.dev.init = i2c_init;
|
||||
@ -36,15 +36,15 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* read as many [x y z] sets as specified by watermark level (size) default (max/32) */
|
||||
/* copy them to the fifo data struct `fifo' */
|
||||
if (lis3dh_read_fifo(&lis, &fifo)) {
|
||||
/* read as many [x y z] sets as specified by watermark level (size) default max/32 */
|
||||
/* copy them to the fifo data struct given below as `data' */
|
||||
if (lis3dh_read_fifo(&lis, &data)) {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* read out fifo buffer data */
|
||||
for(i=0; i<fifo.size; i++) {
|
||||
printf("x: %d mg, y: %d mg, z: %d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
|
||||
for(i=0; i<data.size; i++) {
|
||||
printf("x: %d mg, y: %d mg, z: %d mg\n", data.x[i], data.y[i], data.z[i]);
|
||||
}
|
||||
|
||||
/* deinitialise struct */
|
||||
|
@ -46,9 +46,9 @@ int main() {
|
||||
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. */
|
||||
/* 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;
|
||||
|
@ -64,12 +64,13 @@ int main() {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* read as many [x y z] sets as specified by watermark level (size) */
|
||||
/* read as many [x y z] sets as specified by watermark level (fth) */
|
||||
/* copy them to the fifo data struct given below as `fifo' */
|
||||
if (lis3dh_read_fifo(&lis, &fifo)) {
|
||||
/* error handling */
|
||||
}
|
||||
|
||||
/* above function also writes out the qty of [x y z] sets stored in `fifo' */
|
||||
for(i=0; i<fifo.size; i++) {
|
||||
printf("x: %d mg, y: %d mg, z: %d mg\n", fifo.x[i], fifo.y[i], fifo.z[i]);
|
||||
}
|
||||
|
@ -52,6 +52,8 @@ int main() {
|
||||
|
||||
printf("ADC1: %d mV\n", lis.adc.adc1);
|
||||
printf("ADC2: %d mV\n", lis.adc.adc2);
|
||||
|
||||
/* no decimals, step size is 1 celsius */
|
||||
printf("ADC3: %d oC\n", lis.adc.adc3);
|
||||
|
||||
/* deinitalise struct */
|
||||
|
54
lis3dh.c
54
lis3dh.c
@ -1,9 +1,10 @@
|
||||
#include <stddef.h> /* NULL */
|
||||
#include <string.h> /* memset() */
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
#include "lis3dh.h"
|
||||
#include "registers.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
/* initialise device struct and read WHO_AM_I register */
|
||||
int lis3dh_init(lis3dh_t *lis3dh) {
|
||||
uint8_t result;
|
||||
int err = 0;
|
||||
@ -36,8 +37,6 @@ int lis3dh_init(lis3dh_t *lis3dh) {
|
||||
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;
|
||||
@ -178,7 +177,7 @@ int lis3dh_configure(lis3dh_t *lis3dh) {
|
||||
ctrl_reg0 |= (lis3dh->cfg.sdo_pullup & 1) << 7;
|
||||
|
||||
|
||||
/* Registers have to be set in this order for SPI to function correctly, I think */
|
||||
/* Registers have to be set in this order for SPI to function correctly */
|
||||
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);
|
||||
@ -207,9 +206,9 @@ int lis3dh_configure(lis3dh_t *lis3dh) {
|
||||
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: */
|
||||
/* the real size of the int you get back from reading the acc u16
|
||||
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 */
|
||||
@ -219,8 +218,8 @@ static uint8_t acc_shift(uint8_t mode) {
|
||||
}
|
||||
}
|
||||
|
||||
/* returns a scalar that when multiplied with axis reading */
|
||||
/* turns it to a multiple of mg (1/1000 g). */
|
||||
/* returns a scalar that when multiplied with axis reading
|
||||
turns it to a multiple of mg. */
|
||||
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;
|
||||
@ -241,7 +240,6 @@ int lis3dh_read(lis3dh_t *lis3dh) {
|
||||
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);
|
||||
@ -255,17 +253,18 @@ int lis3dh_read(lis3dh_t *lis3dh) {
|
||||
return err;
|
||||
}
|
||||
|
||||
/* assume fifo has been configured */
|
||||
/* assume fifo has been configured and poll'd */
|
||||
/* 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 data[6]; /* max size */
|
||||
uint8_t sens, fifo_src;
|
||||
int err = 0;
|
||||
int idx = 0;
|
||||
|
||||
/* wait until there is at least 1 unread sample in the FIFO */
|
||||
|
||||
/* otherwise can cause problems at really fast ODRs and calling this */
|
||||
/* function in a loop without delays. */
|
||||
do {
|
||||
err |= lis3dh->dev.read(REG_FIFO_SRC_REG, &fifo_src, 1);
|
||||
lis3dh->dev.sleep(1000);
|
||||
@ -282,9 +281,6 @@ int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) {
|
||||
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;
|
||||
@ -314,8 +310,8 @@ int lis3dh_read_click(lis3dh_t *lis3dh) {
|
||||
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 */
|
||||
/* read REFERENCE reg to reset HP filter in REFERENCE mode
|
||||
it uses the --current-- acceleration as the base in the filter */
|
||||
int lis3dh_reference(lis3dh_t *lis3dh) {
|
||||
uint8_t res;
|
||||
return lis3dh->dev.read(REG_REFERENCE, &res, 1);
|
||||
@ -363,8 +359,8 @@ int lis3dh_reset(lis3dh_t *lis3dh) {
|
||||
return err;
|
||||
}
|
||||
|
||||
/* read all 3 ADCs and convert data depending on power mode */
|
||||
/* result: 1 lsb is equal to 1 millivolt */
|
||||
/* read all 3 ADCs and convert readings depending on power mode
|
||||
st 1 lsb is equal to 1 millivolt */
|
||||
int lis3dh_read_adc(lis3dh_t *lis3dh) {
|
||||
uint8_t data[6];
|
||||
uint8_t shift;
|
||||
@ -381,13 +377,13 @@ int lis3dh_read_adc(lis3dh_t *lis3dh) {
|
||||
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 */
|
||||
/* 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;
|
||||
|
Loading…
Reference in New Issue
Block a user