[Nanohub - Gyro Calibration] BMI160 Driver Update

Syncs changes made for O-DR back to legacy devices running on
the Nanohub CHRE. Provides some parameter adjustments for the
GyroCal and OTC-Gyro runtime calibration algorithms.

Bug: 64755461
Test: Built and verified on hardware.
Change-Id: Ie2c97ba9a699536c4b4911d3581ab2e8a079f5c3

1 files changed, 43 insertions, 49 deletions

diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index e8d88422..513a9c8c 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -61,6 +61,7 @@
 
 #ifdef GYRO_CAL_ENABLED
 #include <calibration/gyroscope/gyro_cal.h>
+#include <common/math/macros.h>
 #endif  // GYRO_CAL_ENABLED
 
 #if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
@@ -107,7 +108,7 @@
 #define DBG_WM_CALC               0
 #define TIMESTAMP_DBG             0
 
-#define BMI160_APP_VERSION 16
+#define BMI160_APP_VERSION 17
 
 // fixme: to list required definitions for a slave mag
 #ifdef USE_BMM150
@@ -2207,25 +2208,16 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal
       }
 
 #ifdef OVERTEMPCAL_ENABLED
-      // OTC-Gyro Cal -- A timer is used to limit the frequency of the offset
-      // update checks.
-      static uint64_t imu_new_otc_offset_timer = 0;  // nanoseconds
-      bool new_otc_offset_update = false;
-      bool new_otc_model_update = false;
-      if ((rtc_time - imu_new_otc_offset_timer) >= 500000000) {
-        imu_new_otc_offset_timer = rtc_time;
-
-        // OTC-Gyro Cal --  Gets the latest OTC-Gyro temperature compensated
-        // offset estimate.
-        new_otc_offset_update =
-            overTempCalNewOffsetAvailable(&mTask.over_temp_gyro_cal);
-        overTempCalGetOffset(&mTask.over_temp_gyro_cal,
-                             &gyro_offset_temperature_celsius, gyro_offset);
-
-        // OTC-Gyro Cal --  Checks for a model update.
-        new_otc_model_update =
-            overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal);
-      }
+      // OTC-Gyro Cal --  Gets the latest OTC-Gyro temperature compensated
+      // offset estimate.
+      bool new_otc_offset_update =
+          overTempCalNewOffsetAvailable(&mTask.over_temp_gyro_cal);
+      overTempCalGetOffset(&mTask.over_temp_gyro_cal,
+                           &gyro_offset_temperature_celsius, gyro_offset);
+
+      // OTC-Gyro Cal --  Checks for a model update.
+      bool new_otc_model_update =
+          overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal);
 
       if (new_otc_offset_update) {
 #else   // OVERTEMPCAL_ENABLED
@@ -3857,38 +3849,40 @@ static bool startTask(uint32_t task_id)
 
 #ifdef GYRO_CAL_ENABLED
     // Gyro Cal -- Initialization.
-    gyroCalInit(
-        &mTask.gyro_cal,
-        5e9,                       // min stillness period = 5 seconds
-        6e9,                       // max stillness period = 6 seconds
-        0, 0, 0,                   // initial bias offset calibration
-        0,                         // time stamp of initial bias calibration
-        1.5e9,                     // analysis window length = 1.5 seconds
-        7.5e-5f,                   // gyroscope variance threshold [rad/sec]^2
-        1.5e-5f,                   // gyroscope confidence delta [rad/sec]^2
-        4.5e-3f,                   // accelerometer variance threshold [m/sec^2]^2
-        9.0e-4f,                   // accelerometer confidence delta [m/sec^2]^2
-        5.0f,                      // magnetometer variance threshold [uT]^2
-        1.0f,                      // magnetometer confidence delta [uT]^2
-        0.95f,                     // stillness threshold [0,1]
-        40.0e-3f * M_PI / 180.0f,  // stillness mean variation limit [rad/sec]
-        1.5f,                      // maximum temperature deviation during stillness [C]
-        true);                     // gyro calibration enable
+    gyroCalInit(&mTask.gyro_cal,
+                SEC_TO_NANOS(5.0f),   // Min stillness period = 5.0 seconds
+                SEC_TO_NANOS(5.9f),   // Max stillness period = 6.0 seconds (NOTE 1)
+                0, 0, 0,              // Initial bias offset calibration
+                0,                    // Time stamp of initial bias calibration
+                SEC_TO_NANOS(1.5f),   // Analysis window length = 1.5 seconds
+                7.5e-5f,              // Gyroscope variance threshold [rad/sec]^2
+                1.5e-5f,              // Gyroscope confidence delta [rad/sec]^2
+                4.5e-3f,              // Accelerometer variance threshold [m/sec^2]^2
+                9.0e-4f,              // Accelerometer confidence delta [m/sec^2]^2
+                5.0f,                 // Magnetometer variance threshold [uT]^2
+                1.0f,                 // Magnetometer confidence delta [uT]^2
+                0.95f,                // Stillness threshold [0,1]
+                40.0f * MDEG_TO_RAD,  // Stillness mean variation limit [rad/sec]
+                1.5f,                 // Max temperature delta during stillness [C]
+                true);                // Gyro calibration enable
+    // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
+    // period of 6.0 seconds and avoid buffer boundary conditions that could push
+    // the max stillness to the next multiple of the analysis window length
+    // (i.e., 7.5 seconds).
 
 #ifdef OVERTEMPCAL_ENABLED
     // Initialize over-temp calibration.
-    overTempCalInit(
-        &mTask.over_temp_gyro_cal,
-        5,                         // Min num of points to enable model update
-        5000000000,                // Min model update interval [nsec]
-        0.75f,                     // Temperature span of bin method [C]
-        50.0e-3f * M_PI / 180.0f,  // Model fit tolerance [rad/sec]
-        50.0e-3f * M_PI / 180.0f,  // Outlier rejection tolerance [rad/sec]
-        172800000000000,           // Model data point age limit [nsec]
-        50.0e-3f * M_PI / 180.0f,  // Limit for temp. sensitivity [rad/sec/C]
-        3.0f * M_PI / 180.0f,      // Limit for model intercept [rad/sec]
-        3.0e-3f * M_PI / 180.0f,   // Significant offset change [rad/sec]
-        true);                     // Over-temp compensation enable
+    overTempCalInit(&mTask.over_temp_gyro_cal,
+                    5,                     // Min num of points to enable model update
+                    SEC_TO_NANOS(0.5f),    // Min temperature update interval [nsec]
+                    0.75f,                 // Temperature span of bin method [C]
+                    40.0f * MDEG_TO_RAD,   // Jump tolerance [rad/sec]
+                    50.0f * MDEG_TO_RAD,   // Outlier rejection tolerance [rad/sec]
+                    DAYS_TO_NANOS(2),      // Model data point age limit [nsec]
+                    80.0f * MDEG_TO_RAD,   // Limit for temp. sensitivity [rad/sec/C]
+                    3.0e3f * MDEG_TO_RAD,  // Limit for model intercept parameter [rad/sec]
+                    0.1f * MDEG_TO_RAD,    // Significant offset change [rad/sec]
+                    true);                 // Over-temp compensation enable
 #endif  // OVERTEMPCAL_ENABLED
 #endif  // GYRO_CAL_ENABLED