From e9472ed1f00e94f395a2ddb9823e21b7b367bf2c Mon Sep 17 00:00:00 2001
From: William Clark <wrvc96@gmail.com>
Date: Sat, 23 Dec 2023 10:28:43 +0000
Subject: [PATCH] pass FIFO data

---
 Makefile |  7 ++++---
 lis3dh.c | 31 ++++++++++---------------------
 lis3dh.h | 12 +++++++++++-
 main.c   | 44 ++++++++++++++++----------------------------
 4 files changed, 41 insertions(+), 53 deletions(-)

diff --git a/Makefile b/Makefile
index faef8ce..be92e2a 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,8 @@
 CC=gcc
-CFLAGS=-O2 -std=c99 -W -Werror -Wall -Wextra -I. 
+CFLAGS=-O2 -std=gnu99 -W -Werror -Wall -Wextra -I. 
+LFLAGS=-lm
 all:
-	$(CC) $(CFLAGS) main.c i2c.c lis3dh.c -o lis3dh -lm
+	$(CC) $(CFLAGS) main.c i2c.c lis3dh.c -o lis3dh $(LFLAGS)
 
 clean:
-	rm -rf lis3dh
\ No newline at end of file
+	rm -rf lis3dh
diff --git a/lis3dh.c b/lis3dh.c
index a5193bf..a14b4d9 100644
--- a/lis3dh.c
+++ b/lis3dh.c
@@ -58,7 +58,7 @@ int lis3dh_init(lis3dh_t *lis3dh) {
     lis3dh->cfg.mode = 0;
     lis3dh->cfg.fifo.mode = 0xFF; /* in use if neq 0xFF */
     lis3dh->cfg.fifo.trig = 0;
-    lis3dh->cfg.fifo.fth = 0;
+    lis3dh->cfg.fifo.fth = 32; /* default watermark level. */
     lis3dh->cfg.filter.mode = 0xFF; /* in use if neq 0xFF */
     lis3dh->cfg.filter.cutoff = 0;
     lis3dh->cfg.filter.fds = 0;
@@ -248,45 +248,34 @@ int lis3dh_read(lis3dh_t *lis3dh) {
     return err;
 }
 
-static double mag(double x, double y, double z) {
-    return sqrt( powf(x, 2) + powf(y, 2) + powf(z, 2) );
-}
-
-int lis3dh_read_fifo(lis3dh_t *lis3dh) {
+int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) {
 
     int16_t x, y, z;
     uint8_t scale, sens;
-    uint8_t data[192];
+    uint8_t data[192]; /* max size */
     int err = 0;
+    int i, idx;
 
     scale = acc_shift(lis3dh);
     sens = acc_sensitivity(lis3dh);
 
+    fifo->size = lis3dh->cfg.fifo.fth;
+
     /* 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) {
+    for(i=0, idx=0; i<lis3dh->cfg.fifo.fth * 6; i+=6, idx++) {
         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;
 
-        double dx = ((double)x) / 1000.0;
-        double dy = ((double)y) / 1000.0;
-        double dz = ((double)z) / 1000.0;
-
-        printf("x: %04.04f, y: %04.04f, z: %04.04f mag:%f\n", 
-            dx,
-            dy,
-            dz,
-            mag(dx, dy, dz));
+        fifo->x[idx] = ((float)x) / 1000.0;
+        fifo->y[idx] = ((float)y) / 1000.0;
+        fifo->z[idx] = ((float)z) / 1000.0;
     }
 
-    puts("<= FIFO");
-
     return err;
 }
 
diff --git a/lis3dh.h b/lis3dh.h
index 00c3ea7..1016681 100644
--- a/lis3dh.h
+++ b/lis3dh.h
@@ -100,12 +100,22 @@ struct lis3dh {
 
 typedef struct lis3dh lis3dh_t;
 
+/* struct for containing the FIFO data */
+struct lis3dh_fifo_data {
+    uint8_t size; /* up to 32 */
+    float x[32];
+    float y[32];
+    float z[32];
+};
+
 int lis3dh_init(lis3dh_t *lis3dh);
 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);
+int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo);
+
+
 
 #endif
\ No newline at end of file
diff --git a/main.c b/main.c
index 035c387..d898680 100644
--- a/main.c
+++ b/main.c
@@ -5,16 +5,14 @@
 #include "lis3dh.h"
 #include "i2c.h"
 
-double accel_mag(lis3dh_t *lis) {
-    double d = 0.0;
-
-    d = sqrt( powf(lis->acc.x, 2) + powf(lis->acc.y, 2) + powf(lis->acc.z, 2));
-    return d;
+float mag(float x, float y, float z) {
+    return sqrt( powf(x, 2) + powf(y, 2) + powf(z, 2) );
 }
 
 int main() {
 
     lis3dh_t lis;
+    struct lis3dh_fifo_data fifo;
 
     lis.dev.init = i2c_init;
     lis.dev.read = i2c_read;
@@ -29,40 +27,30 @@ int main() {
     }
 
     lis.cfg.mode = LIS3DH_MODE_HR;
-    lis.cfg.range = LIS3DH_FS_4G;
-    lis.cfg.rate = LIS3DH_ODR_25_HZ;
+    lis.cfg.range = LIS3DH_FS_2G;
+    lis.cfg.rate = LIS3DH_ODR_100_HZ;
     lis.cfg.fifo.mode = LIS3DH_FIFO_MODE_STREAM;
- //lis.cfg.filter.mode = LIS3DH_FILTER_MODE_AUTORESET;
-    lis.cfg.filter.cutoff = LIS3DH_FILTER_CUTOFF_8;
-    lis.cfg.filter.fds = 1;
 
     if (lis3dh_configure(&lis)) {
         puts("configure ERR");
     }
 
-/*
     for(int i=0; i<100; i++) {
-        if (lis3dh_poll(&lis)) {
-            puts("poll ERR");
+    
+        if (lis3dh_poll_fifo(&lis)) {
+            puts("poll_fifo ERR");
         }
 
-        if (lis3dh_read(&lis)) {
-            puts("read ERR");
+        if (lis3dh_read_fifo(&lis, &fifo)) {
+            puts("read_fifo ERR");
         }
 
-        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");
-    }
+        for(int k=0; k<fifo.size; k++) {
+            printf("x: %04.04f, y: %04.04f, z: %04.04f mag: %04.04f\n",
+                fifo.x[k], fifo.y[k], fifo.z[k],
+                mag(fifo.x[k], fifo.y[k], fifo.z[k]));
+        }
+   }
     
     if (lis3dh_deinit(&lis)) {
         puts("deinit ERR");