From 89e9d9310d6b2483556665bdac9bbf0bb9f27c98 Mon Sep 17 00:00:00 2001
From: William Clark <wrvc96@gmail.com>
Date: Thu, 4 Jan 2024 20:20:51 +0000
Subject: [PATCH] accel readings now i16 (1lsb per 1mg) instead of float

---
 lis3dh.c | 37 +++++++++++++------------------------
 lis3dh.h | 16 +++++++++-------
 main.c   |  7 ++++---
 3 files changed, 26 insertions(+), 34 deletions(-)

diff --git a/lis3dh.c b/lis3dh.c
index 61bd1c0..6aa8ef4 100644
--- a/lis3dh.c
+++ b/lis3dh.c
@@ -259,50 +259,39 @@ static uint8_t acc_sensitivity(uint8_t mode, uint8_t range) {
 
 int lis3dh_read(lis3dh_t *lis3dh) {
     uint8_t data[6];
-    int32_t x, y, z;
-    uint8_t scale, sens;
+    uint8_t shift, sens;
     int err = 0;
 
-    scale = acc_shift(lis3dh->cfg.mode);
+    shift = acc_shift(lis3dh->cfg.mode);
     sens = acc_sensitivity(lis3dh->cfg.mode, lis3dh->cfg.range);
 
     err |= lis3dh->dev.read(REG_OUT_X_L, data, 6);
 
-    x = (((int16_t)((data[0] << 8) | data[1])) >> scale) * sens;
-    y = (((int16_t)((data[2] << 8) | data[3])) >> scale) * sens;
-    z = (((int16_t)((data[4] << 8) | data[5])) >> scale) * sens;
-
-    lis3dh->acc.x = ((float)x) / 1000.0;
-    lis3dh->acc.y = ((float)y) / 1000.0;
-    lis3dh->acc.z = ((float)z) / 1000.0;
+    lis3dh->acc.x = ((int16_t)(data[1] | data[0] << 8) >> shift) * sens;
+    lis3dh->acc.y = ((int16_t)(data[3] | data[2] << 8) >> shift) * sens;
+    lis3dh->acc.z = ((int16_t)(data[5] | data[4] << 8) >> shift) * sens;
 
     return err;
 }
 
 /* assume fifo has been configured and poll'd */
 int lis3dh_read_fifo(lis3dh_t *lis3dh, struct lis3dh_fifo_data *fifo) {
-    int32_t x, y, z;
-    uint8_t scale, sens;
     uint8_t data[192]; /* max size */
+    uint8_t sens;
     int err = 0;
     int i, idx;
 
     /* FIFO is always 10-bit / normal mode */
-    scale = 6;
     sens = acc_sensitivity(LIS3DH_MODE_NORMAL, lis3dh->cfg.range);
 
     fifo->size = lis3dh->cfg.fifo.fth;
 
-    err |= lis3dh->dev.read(REG_OUT_X_L, data, 192);
+    err |= lis3dh->dev.read(REG_OUT_X_L, data, fifo->size * 6);
 
     for (i=0, idx=0; i<fifo->size * 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;
-
-        fifo->x[idx] = ((float)x) / 1000.0;
-        fifo->y[idx] = ((float)y) / 1000.0;
-        fifo->z[idx] = ((float)z) / 1000.0;
+        fifo->x[idx] = ((int16_t)(data[i+1] | data[i+0] << 8) >> 6) * sens;
+        fifo->y[idx] = ((int16_t)(data[i+3] | data[i+2] << 8) >> 6) * sens;
+        fifo->z[idx] = ((int16_t)(data[i+5] | data[i+4] << 8) >> 6) * sens;
     }
 
     return err;
@@ -383,9 +372,9 @@ int lis3dh_read_adc(lis3dh_t *lis3dh) {
 
     shift = (lis3dh->cfg.mode == LIS3DH_MODE_LP) ? 8 : 6;
 
-    lis3dh->adc.adc1 = 1200 + (400 * ((data[1] | data[0] << 8) >> shift) >> (16 - shift)); 
-    lis3dh->adc.adc2 = 1200 + (400 * ((data[3] | data[2] << 8) >> shift) >> (16 - shift)); 
-    lis3dh->adc.adc3 = 1200 + (400 * ((data[5] | data[4] << 8) >> shift) >> (16 - shift)); 
+    lis3dh->adc.adc1 = 1200 + (400 * ((int16_t)(data[1] | data[0] << 8) >> shift) >> (16 - shift)); 
+    lis3dh->adc.adc2 = 1200 + (400 * ((int16_t)(data[3] | data[2] << 8) >> shift) >> (16 - shift)); 
+    lis3dh->adc.adc3 = 1200 + (400 * ((int16_t)(data[5] | data[4] << 8) >> shift) >> (16 - shift)); 
 
     return err;
 }
diff --git a/lis3dh.h b/lis3dh.h
index e38e1c6..d0d7758 100644
--- a/lis3dh.h
+++ b/lis3dh.h
@@ -245,11 +245,12 @@ struct lis3dh_adc {
     int16_t adc3;
 };
 
-/* accel data not read from FIFO is put here */
+/* accel data read not using FIFO */
+/* 1 lsb = 1 mg */
 struct lis3dh_accel {
-    float x;
-    float y;
-    float z;
+    int16_t x;
+    int16_t y;
+    int16_t z;
 };
 
 /* stores interrupt source registers read from the device */
@@ -270,11 +271,12 @@ struct lis3dh {
 typedef struct lis3dh lis3dh_t;
 
 /* struct for containing the FIFO data */
+/* 1 lsb = 1 mg */
 struct lis3dh_fifo_data {
     uint8_t size; /* up to 32 */
-    float x[32];
-    float y[32];
-    float z[32];
+    int16_t x[32];
+    int16_t y[32];
+    int16_t z[32];
 };
 
 int lis3dh_init(lis3dh_t *lis3dh);
diff --git a/main.c b/main.c
index 1adfe92..dd12326 100644
--- a/main.c
+++ b/main.c
@@ -77,11 +77,12 @@ int main() {
 
     /* above function also writes out the qty of [x y z] sets stored in `fifo' */
     for(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]));
+        printf("x: %d, y: %d z: %d\n",
+            fifo.x[k], fifo.y[k], fifo.z[k]);
     }
 
+    printf("mag: %04.04f\n", mag(100.0, 45.0, 120.0));
+
 
     if (lis3dh_read_adc(&lis)) {
         quit("adc()", &lis);