From 0dff0b92c3ea93e43bc43a949fd01eab8265f7f8 Mon Sep 17 00:00:00 2001 From: William Clark Date: Fri, 22 Dec 2023 10:22:43 +0000 Subject: [PATCH] FIFO --- README.md | 39 +++++++++++++++++++++++++++ lis3dh.c | 79 ++++++++++++++++++++++++++++++++++++++++++++++++++++--- lis3dh.h | 22 +++++++++++++++- main.c | 14 ++++++++++ 4 files changed, 149 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 1d7cb0e..af77172 100644 --- a/README.md +++ b/README.md @@ -23,4 +23,43 @@ x: 0.5760 g, y: -0.8800 g, z: 0.0320 g, mag: 1.0522 x: 0.5560 g, y: -0.8840 g, z: -0.0240 g, mag: 1.0446 x: 0.5120 g, y: -0.9040 g, z: -0.0360 g, mag: 1.0395 x: 0.6000 g, y: -0.8720 g, z: 0.0320 g, mag: 1.0590 +``` + +FIFO demo: +``` +$ ./lis3dh +FIFO => +x: 0.5280, y: -0.8760, z: -0.0080 +x: 0.5320, y: -0.8920, z: -0.0120 +x: 0.5280, y: -0.8680, z: -0.0040 +x: 0.5280, y: -0.8800, z: 0.0080 +x: 0.5400, y: -0.8920, z: 0.0120 +x: 0.5400, y: -0.8960, z: 0.0120 +x: 0.5400, y: -0.8960, z: 0.0000 +x: 0.5560, y: -0.8840, z: -0.0040 +x: 0.5360, y: -0.8840, z: -0.0160 +x: 0.5560, y: -0.8840, z: -0.0040 +x: 0.5320, y: -0.8960, z: 0.0080 +x: 0.5400, y: -0.8880, z: -0.0040 +x: 0.5320, y: -0.8800, z: 0.0000 +x: 0.5560, y: -0.8920, z: 0.0120 +x: 0.5280, y: -0.8680, z: -0.0120 +x: 0.5440, y: -0.8760, z: 0.0000 +x: 0.5520, y: -0.8880, z: -0.0040 +x: 0.5400, y: -0.8840, z: -0.0160 +x: 0.5560, y: -0.8840, z: -0.0040 +x: 0.5360, y: -0.8840, z: -0.0040 +x: 0.5360, y: -0.8880, z: -0.0160 +x: 0.5560, y: -0.8920, z: 0.0080 +x: 0.5400, y: -0.8880, z: -0.0120 +x: 0.5560, y: -0.8840, z: -0.0160 +x: 0.5480, y: -0.8800, z: 0.0040 +x: 0.5520, y: -0.8880, z: -0.0040 +x: 0.5280, y: -0.8680, z: -0.0160 +x: 0.5560, y: -0.8920, z: -0.0040 +x: 0.5400, y: -0.8840, z: -0.0040 +x: 0.5360, y: -0.8880, z: -0.0080 +x: 0.5280, y: -0.8680, z: -0.0120 +x: 0.5480, y: -0.8880, z: -0.0120 +<= FIFO ``` \ No newline at end of file diff --git a/lis3dh.c b/lis3dh.c index 29ad357..94c7f10 100644 --- a/lis3dh.c +++ b/lis3dh.c @@ -16,14 +16,26 @@ int lis3dh_init(lis3dh_t *lis3dh) { int err = 0; if (lis3dh->dev.init() != 0) { - err |= 1; + return 1; } err |= lis3dh->dev.read(REG_WHO_AM_I, &result, 1); if (result != 0x33) { - err |= 1; + return 1; } + /* zero struct */ + lis3dh->acc.x = 0.0; + lis3dh->acc.y = 0.0; + lis3dh->acc.z = 0.0; + lis3dh->cfg.rate = 0; + lis3dh->cfg.range = 0; + lis3dh->cfg.mode = 0; + lis3dh->cfg.fifo.mode = 0; + lis3dh->cfg.fifo.trig = 0; + lis3dh->cfg.fifo.fth = 0; + lis3dh->cfg.fifo.enable = 0; + err |= lis3dh_reboot(lis3dh); return err; @@ -31,15 +43,26 @@ int lis3dh_init(lis3dh_t *lis3dh) { int lis3dh_configure(lis3dh_t *lis3dh) { - uint8_t ctrl_reg1, ctrl_reg4; + uint8_t ctrl_reg1, ctrl_reg4, ctrl_reg5; + uint8_t fifo_ctrl_reg; uint8_t ref; int err = 0; /* last 0b111 enables Z, Y and X axis */ ctrl_reg1 = 0 | (lis3dh->cfg.rate << 4) | 0b111; ctrl_reg4 = 0 | (lis3dh->cfg.range << 4); + ctrl_reg5 = 0; + fifo_ctrl_reg = 0; - /* set block update */ + /* set enable FIFO */ + if (lis3dh->cfg.fifo.enable) { + ctrl_reg5 |= 0x40; + fifo_ctrl_reg |= (lis3dh->cfg.fifo.fth & 0x1F); + fifo_ctrl_reg |= (lis3dh->cfg.fifo.mode << 6); + fifo_ctrl_reg |= ((lis3dh->cfg.fifo.trig & 1) << 5); + } + + /* always set block update */ ctrl_reg4 |= 0x80; /* set high resolution */ @@ -54,6 +77,8 @@ int lis3dh_configure(lis3dh_t *lis3dh) { err |= lis3dh->dev.write(REG_CTRL_REG1, ctrl_reg1); 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); /* read REFERENCE to clear internal filter struct */ err |= lis3dh->dev.read(REG_REFERENCE, &ref, 1); @@ -69,12 +94,25 @@ int lis3dh_poll(lis3dh_t *lis3dh) { int err = 0; do { + lis3dh->dev.sleep(1000); /* 1 ms */ err |= lis3dh->dev.read(REG_STATUS_REG, &status, 1); } while (!err && !((status >> 3) & 1)); return err; } +int lis3dh_poll_fifo(lis3dh_t *lis3dh) { + uint8_t src; + int err = 0; + + do { + lis3dh->dev.sleep(1000); /* 1 ms */ + err |= lis3dh->dev.read(REG_FIFO_SRC_REG, &src, 1); + } while (!err && !(src >> 6)); + + return err; +} + /* 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: */ @@ -168,6 +206,39 @@ int lis3dh_read(lis3dh_t *lis3dh) { return err; } +int lis3dh_read_fifo(lis3dh_t *lis3dh) { + + int16_t x, y, z; + uint8_t scale, sens; + uint8_t data[192]; + int err = 0; + + scale = acc_shift(lis3dh); + sens = acc_sensitivity(lis3dh); + + /* must set MSbit of the address to multi-read and + have the device auto-increment the address. + see 5.1.5 in datasheet. */ + err |= lis3dh->dev.read(REG_OUT_X_L | 0x80, data, 192); + + puts("FIFO =>"); + + for(int i=0; i<192; i+=6) { + x = (((int16_t)((data[i + 0] << 8) | data[i + 1])) >> scale) * sens; + y = (((int16_t)((data[i + 2] << 8) | data[i + 3])) >> scale) * sens; + z = (((int16_t)((data[i + 4] << 8) | data[i + 5])) >> scale) * sens; + + printf("x: %04.04f, y: %04.04f, z: %04.04f\n", + (double)x / 1000.0, + (double)y / 1000.0, + (double)z / 1000.0); + } + + puts("<= FIFO"); + + return err; +} + int lis3dh_deinit(lis3dh_t *lis3dh) { return lis3dh->dev.deinit(); } \ No newline at end of file diff --git a/lis3dh.h b/lis3dh.h index ea6cd3e..54ac50a 100644 --- a/lis3dh.h +++ b/lis3dh.h @@ -22,11 +22,20 @@ #define LIS3DH_FS_8G 0b10 #define LIS3DH_FS_16G 0b11 -/* modes */ +/* operating modes */ #define LIS3DH_MODE_HR 0b00 #define LIS3DH_MODE_LP 0b01 #define LIS3DH_MODE_NORMAL 0b10 +/* FIFO modes */ +#define LIS3DH_FIFO_MODE_BYPASS 0b00 +#define LIS3DH_FIFO_MODE_NORMAL 0b01 /* "FIFO" */ +#define LIS3DH_FIFO_MODE_STREAM 0b10 +#define LIS3DH_FIFO_MODE_STREAM_TO_FIFO 0b11 + +/* FIFO trigger pin selection */ +#define LIS3DH_FIFO_TRIG_INT1 0b0 +#define LIS3DH_FIFO_TRIG_INT2 0b1 struct lis3dh_device { int (*init)(void); @@ -36,10 +45,18 @@ struct lis3dh_device { int (*deinit)(void); }; +struct lis3dh_fifo_config { + uint8_t fth; + uint8_t trig; + uint8_t mode; + uint8_t enable; +}; + struct lis3dh_config { uint8_t rate; /* ODR */ uint8_t range; /* FS */ uint8_t mode; /* LPen and HR */ + struct lis3dh_fifo_config fifo; }; struct lis3dh_acceleration { @@ -61,4 +78,7 @@ int lis3dh_deinit(lis3dh_t *lis3dh); int lis3dh_configure(lis3dh_t *lis3dh); int lis3dh_poll(lis3dh_t *lis3dh); int lis3dh_read(lis3dh_t *lis3dh); +int lis3dh_poll_fifo(lis3dh_t *lis3dh); +int lis3dh_read_fifo(lis3dh_t *lis3dh); + #endif \ No newline at end of file diff --git a/main.c b/main.c index e5ef0b6..896677f 100644 --- a/main.c +++ b/main.c @@ -23,6 +23,7 @@ int main() { lis.dev.deinit = i2c_deinit; + /* write config after init() */ if (lis3dh_init(&lis)) { puts("init ERR"); } @@ -30,11 +31,14 @@ int main() { lis.cfg.mode = LIS3DH_MODE_NORMAL; lis.cfg.range = LIS3DH_FS_2G; lis.cfg.rate = LIS3DH_ODR_100_HZ; + lis.cfg.fifo.enable = 1; + lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_NORMAL; if (lis3dh_configure(&lis)) { puts("configure ERR"); } +/* for(int i=0; i<100; i++) { if (lis3dh_poll(&lis)) { puts("poll ERR"); @@ -47,7 +51,17 @@ int main() { printf("x: % 04.04f g, y: % 04.04f g, z: % 04.04f g, mag:% 04.04f\n", lis.acc.x, lis.acc.y, lis.acc.z, accel_mag(&lis)); } + */ + /* FIFO test */ + if (lis3dh_poll_fifo(&lis)) { + puts("poll_fifo ERR"); + } + + if (lis3dh_read_fifo(&lis)) { + puts("read_fifo ERR"); + } + if (lis3dh_deinit(&lis)) { puts("deinit ERR"); }