[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
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index e8d8842..513a9c8 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 @@
}
#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.
+ 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 -- 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 -- 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 @@
#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
