From adda7f84a2eaf90cf7363e588cf9caa6665d58ae Mon Sep 17 00:00:00 2001
From: Christoph Honal <christoph.honal@web.de>
Date: Mon, 13 May 2024 16:17:55 +0200
Subject: [PATCH] BMA421: Use internal FIFO for smoothing

---
 src/drivers/Bma421.cpp | 60 +++++++++++++++++++++++++++---------------
 src/drivers/Bma421.h   |  2 ++
 2 files changed, 41 insertions(+), 21 deletions(-)

diff --git a/src/drivers/Bma421.cpp b/src/drivers/Bma421.cpp
index aff62b8df0..4fd15a24d6 100644
--- a/src/drivers/Bma421.cpp
+++ b/src/drivers/Bma421.cpp
@@ -42,10 +42,11 @@ Bma421::Bma421(TwiMaster& twiMaster, uint8_t twiAddress) : twiMaster {twiMaster}
   bma.intf_ptr = this;
   bma.delay_us = user_delay;
   bma.read_write_len = 16;
+  bma.resolution = 12;
 }
 
 void Bma421::Init() {
-  if (not isResetOk)
+  if (!isResetOk)
     return; // Call SoftReset (and reset TWI device) first!
 
   auto ret = bma423_init(&bma);
@@ -68,10 +69,6 @@ void Bma421::Init() {
   if (ret != BMA4_OK)
     return;
 
-  ret = bma4_set_interrupt_mode(BMA4_LATCH_MODE, &bma);
-  if (ret != BMA4_OK)
-    return;
-
   ret = bma423_feature_enable(BMA423_STEP_CNTR, 1, &bma);
   if (ret != BMA4_OK)
     return;
@@ -84,10 +81,19 @@ void Bma421::Init() {
   if (ret != BMA4_OK)
     return;
 
-  accel_conf.odr = BMA4_OUTPUT_DATA_RATE_100HZ;
+  ret = bma4_set_fifo_config(BMA4_FIFO_ACCEL, 1, &bma);
+  if (ret != BMA4_OK)
+    return;
+  fifo_frame.data = (uint8_t*) &fifo;
+  fifo_frame.length = sizeof(fifo);
+  fifo_frame.fifo_data_enable = BMA4_FIFO_A_ENABLE;
+  fifo_frame.fifo_header_enable = 0;
+
+  struct bma4_accel_config accel_conf;
+  accel_conf.odr = BMA4_OUTPUT_DATA_RATE_200HZ;
   accel_conf.range = BMA4_ACCEL_RANGE_2G;
   accel_conf.bandwidth = BMA4_ACCEL_NORMAL_AVG4;
-  accel_conf.perf_mode = BMA4_CIC_AVG_MODE;
+  accel_conf.perf_mode = BMA4_CONTINUOUS_MODE;
   ret = bma4_set_accel_config(&accel_conf, &bma);
   if (ret != BMA4_OK)
     return;
@@ -111,30 +117,42 @@ void Bma421::Write(uint8_t registerAddress, const uint8_t* data, size_t size) {
 Bma421::Values Bma421::Process() {
   if (not isOk)
     return {};
-  struct bma4_accel rawData;
-  struct bma4_accel data;
-  bma4_read_accel_xyz(&rawData, &bma);
-
-  // Scale the measured ADC counts to units of 'binary milli-g'
-  // where 1g = 1024 'binary milli-g' units.
-  // See https://github.com/InfiniTimeOrg/InfiniTime/pull/1950 for
-  // discussion of why we opted for scaling to 1024 rather than 1000.
-  data.x = 1024 * rawData.x / accelScaleFactors[accel_conf.range];
-  data.y = 1024 * rawData.y / accelScaleFactors[accel_conf.range];
-  data.z = 1024 * rawData.z / accelScaleFactors[accel_conf.range];
+
+  bma4_read_fifo_data(&fifo_frame, &bma);
+  uint16_t length = 32; // Should be about 20 (200 Hz ODR / 10 Hz main loop)
+  bma4_extract_accel((bma4_accel*) fifo, &length, &fifo_frame, &bma);
 
   uint32_t steps = 0;
   bma423_step_counter_output(&steps, &bma);
 
-  int32_t temperature;
+  int32_t temperature = 0;
   bma4_get_temperature(&temperature, BMA4_DEG, &bma);
   temperature = temperature / 1000;
 
   uint8_t activity = 0;
   bma423_activity_output(&activity, &bma);
 
-  // X and Y axis are swapped because of the way the sensor is mounted in the PineTime
-  return {steps, data.y, data.x, data.z};
+  int16_t avgs[3] = {0};
+  for (uint8_t i = 0; i < length; i++) {
+    // X and Y axis are swapped because of the way the sensor is mounted in the PineTime
+    int16_t swap = fifo[i][0];
+    fifo[i][0] = fifo[i][1];
+    fifo[i][1] = swap;
+    for (uint8_t j = 0; j < 3; j++)
+      avgs[j] += fifo[i][j];
+  }
+
+  for (uint8_t j = 0; j < 3; j++)
+  {
+    avgs[j] /= length;
+    // Scale the measured ADC counts to units of 'binary milli-g'
+    // where 1g = 1024 'binary milli-g' units.
+    // See https://github.com/InfiniTimeOrg/InfiniTime/pull/1950 for
+    // discussion of why we opted for scaling to 1024 rather than 1000.
+    avgs[j] = 1024 * avgs[j] / accelScaleFactors[accel_conf.range];
+  }
+
+  return {steps, avgs[0], avgs[1], avgs[2]};
 }
 
 bool Bma421::IsOk() const {
diff --git a/src/drivers/Bma421.h b/src/drivers/Bma421.h
index 5269f62b79..f41cfcd922 100644
--- a/src/drivers/Bma421.h
+++ b/src/drivers/Bma421.h
@@ -42,6 +42,8 @@ namespace Pinetime {
       uint8_t deviceAddress = 0x18;
       struct bma4_dev bma;
       struct bma4_accel_config accel_conf; // Store the device configuration for later reference.
+      struct bma4_fifo_frame fifo_frame;
+      int16_t fifo[32][3] = {0};
       bool isOk = false;
       bool isResetOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;