BMI160 driver updates for runtime gyroscope calibration
Bug: 37544952
Test: Firmware built and tested on hardware.
Change-Id: I6726ac0f200193ca71a73b69e8bc62f0adf5b755
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 9bb2209..3511dae 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -63,9 +63,9 @@
#include <calibration/gyroscope/gyro_cal.h>
#endif // GYRO_CAL_ENABLED
-#ifdef GYRO_CAL_DBG_ENABLED
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
#include <calibration/util/cal_log.h>
-#endif // GYRO_CAL_DBG_ENABLED
+#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
#ifdef OVERTEMPCAL_ENABLED
#include <calibration/over_temp/over_temp_cal.h>
@@ -103,7 +103,7 @@
#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
-#define BMI160_APP_VERSION 14
+#define BMI160_APP_VERSION 15
// fixme: to list required definitions for a slave mag
#ifdef USE_BMM150
@@ -2207,18 +2207,24 @@
// 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 =
- overTempCalGetOffset(&mTask.over_temp_gyro_cal, rtc_time,
- &gyro_offset_temperature_celsius, gyro_offset);
+ 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);
}
if (new_otc_offset_update) {
-#else // OVERTEMPCAL_ENABLED
+#else // OVERTEMPCAL_ENABLED
if (new_gyrocal_offset_update) {
#endif // OVERTEMPCAL_ENABLED
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
@@ -2258,8 +2264,7 @@
}
}
#ifdef OVERTEMPCAL_ENABLED
- if (overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal)
- || new_otc_offset_update) {
+ if (new_otc_model_update || new_otc_offset_update) {
// Notify HAL to store new gyro OTC-Gyro data.
T(otcGyroUpdateBuffer).sendToHostRequest = true;
}
@@ -3848,56 +3853,62 @@
#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
- 1e-5f, // gyroscope confidence delta [rad/sec]^2
- 8e-3f, // accelerometer variance threshold [m/sec^2]^2
- 1.6e-3f, // accelerometer confidence delta [m/sec^2]^2
- 5.0f, // magnetometer variance threshold [uT]^2
- 0.25, // 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,
+ 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
#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].
- 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 parameter [rad/sec].
- true); // Over-temp compensation enable.
+ 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
#endif // OVERTEMPCAL_ENABLED
#endif // GYRO_CAL_ENABLED
#ifdef MAG_SLAVE_PRESENT
#ifdef DIVERSITY_CHECK_ENABLED
- initMagCal(&mTask.moc, 0.0f, 0.0f, 0.0f, // bias x, y, z
- 1.0f, 0.0f, 0.0f, // c00, c01, c02
- 0.0f, 1.0f, 0.0f, // c10, c11, c12
- 0.0f, 0.0f, 1.0f, // c20, c21, c22
- 8, // min_num_diverse_vectors
- 1, // max_num_max_distance
- 6.0f, // var_threshold
- 10.0f, // max_min_threshold
- 48.f, // local_field
- 0.5f, // threshold_tuning_param
- 2.552); // max_distance_tuning_param
+ initMagCal(&mTask.moc,
+ 0.0f, 0.0f, 0.0f, // bias x, y, z
+ 1.0f, 0.0f, 0.0f, // c00, c01, c02
+ 0.0f, 1.0f, 0.0f, // c10, c11, c12
+ 0.0f, 0.0f, 1.0f, // c20, c21, c22
+ 3000000, // min_batch_window_in_micros
+ 8, // min_num_diverse_vectors
+ 1, // max_num_max_distance
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f); // max_distance_tuning_param
#else
- initMagCal(&mTask.moc, 0.0f, 0.0f, 0.0f, // bias x, y, z
- 1.0f, 0.0f, 0.0f, // c00, c01, c02
- 0.0f, 1.0f, 0.0f, // c10, c11, c12
- 0.0f, 0.0f, 1.0f); // c20, c21, c22
+ initMagCal(&mTask.moc,
+ 0.0f, 0.0f, 0.0f, // bias x, y, z
+ 1.0f, 0.0f, 0.0f, // c00, c01, c02
+ 0.0f, 1.0f, 0.0f, // c10, c11, c12
+ 0.0f, 0.0f, 1.0f); // c20, c21, c22
#endif
#endif
