FIFO

README.md

@@ -24,3 +24,42 @@ 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
+```

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();
 }

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

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,6 +51,16 @@ 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");