[Bosch BMI160 Driver] Runtime sensor calibration updates.
Adds accel calibration debug printouts for logging.
Updates gyro get/set bias function arguments.
Uses parameter structures to initialize the runtime algorithms.
Bug: 75333210
Test: Built firmware and tested on a device.
Change-Id: I2d0e1e2247927430e34ae86ea1ca470d726d2d93
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 1b8fd68..70cacad 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -16,6 +16,7 @@
#include <algos/time_sync.h>
#include <atomic.h>
+#include <common/math/macros.h>
#include <cpu/cpuMath.h>
#include <errno.h>
#include <gpio.h>
@@ -61,7 +62,6 @@
#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)
@@ -108,7 +108,7 @@
#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
-#define BMI160_APP_VERSION 17
+#define BMI160_APP_VERSION 18
// fixme: to list required definitions for a slave mag
#ifdef USE_BMM150
@@ -2105,7 +2105,12 @@
x, y, z, mTask.tempCelsius);
accelCalBiasRemove(&mTask.acc, &x, &y, &z);
-#endif
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+ // Prints debug data report.
+ accelCalDebPrint(&mTask.acc, mTask.tempCelsius);
+#endif // ACCEL_CAL_DBG_ENABLED
+#endif // ACCEL_CAL_ENABLED
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Add accelerometer sample.
@@ -2206,11 +2211,13 @@
// GyroCal -- Checks for a new offset estimate update.
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
float gyro_offset_temperature_celsius = 0.0f;
+ uint64_t calibration_time_nanos = 0;
bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
if (new_gyrocal_offset_update) {
// GyroCal -- Gets the GyroCal offset estimate.
gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1],
- &gyro_offset[2], &gyro_offset_temperature_celsius);
+ &gyro_offset[2], &gyro_offset_temperature_celsius,
+ &calibration_time_nanos);
#ifdef OVERTEMPCAL_ENABLED
// OTC-Gyro Cal -- Sends a new GyroCal estimate to the OTC-Gyro.
@@ -3129,8 +3136,11 @@
bias->hardwareBias[2] & 0xFF);
#ifdef GYRO_CAL_ENABLED
- gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
- bias->softwareBias[2], sensorGetTime());
+ const float dummy_temperature_celsius = 25.0f;
+ gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0],
+ bias->softwareBias[1], bias->softwareBias[2],
+ dummy_temperature_celsius,
+ sensorGetTime());
#endif // GYRO_CAL_ENABLED
if (!saveCalibration()) {
T(pending_calibration_save) = true;
@@ -3846,81 +3856,100 @@
osEventSubscribe(mTask.tid, EVT_APP_START);
#ifdef ACCEL_CAL_ENABLED
- // Init Accel Cal
- accelCalInit(&mTask.acc,
- 800000000, /* Stillness Time in ns (0.8s) */
- 5, /* Minimum Sample Number */
- 0.00025, /* Threshold */
- 15, /* nx bucket count */
- 15, /* nxb bucket count */
- 15, /* ny bucket count */
- 15, /* nyb bucket count */
- 15, /* nz bucket count */
- 15, /* nzb bucket count */
- 15); /* nle bucket count */
+ // Initializes the accelerometer offset calibration algorithm.
+ const struct AccelCalParameters accel_cal_parameters = {
+ MSEC_TO_NANOS(800), // t0
+ 5, // n_s
+ 15, // fx
+ 15, // fxb
+ 15, // fy
+ 15, // fyb
+ 15, // fz
+ 15, // fzb
+ 15, // fle
+ 0.00025f // th
+ };
+ accelCalInit(&mTask.acc, &accel_cal_parameters);
#endif
#ifdef GYRO_CAL_ENABLED
- // Gyro Cal -- Initialization.
- 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).
+ // Initializes the gyroscope offset calibration algorithm.
+ const struct GyroCalParameters gyro_cal_parameters = {
+ SEC_TO_NANOS(5), // min_still_duration_nanos
+ SEC_TO_NANOS(5.9f), // max_still_duration_nanos [see, NOTE 1]
+ 0, // calibration_time_nanos
+ SEC_TO_NANOS(1.5f), // window_time_duration_nanos
+ 0, // bias_x
+ 0, // bias_y
+ 0, // bias_z
+ 0.95f, // stillness_threshold
+ MDEG_TO_RAD * 40.0f, // stillness_mean_delta_limit [rad/sec]
+ 7.5e-5f, // gyro_var_threshold [rad/sec]^2
+ 1.5e-5f, // gyro_confidence_delta [rad/sec]^2
+ 4.5e-3f, // accel_var_threshold [m/sec^2]^2
+ 9.0e-4f, // accel_confidence_delta [m/sec^2]^2
+ 5.0f, // mag_var_threshold [uTesla]^2
+ 1.0f, // mag_confidence_delta [uTesla]^2
+ 1.5f, // temperature_delta_limit_celsius
+ true // gyro_calibration_enable
+ };
+ // [NOTE 1]: 'max_still_duration_nanos' 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).
+ gyroCalInit(&mTask.gyro_cal, &gyro_cal_parameters);
#ifdef OVERTEMPCAL_ENABLED
- // Initialize over-temp calibration.
- 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
+ // Initializes the gyroscope over-temperature offset compensation algorithm.
+ const struct OverTempCalParameters gyro_otc_parameters = {
+ MSEC_TO_NANOS(500), // min_temp_update_period_nanos
+ DAYS_TO_NANOS(2), // age_limit_nanos
+ 0.75f, // delta_temp_per_bin
+ 40.0f * MDEG_TO_RAD, // jump_tolerance
+ 50.0f * MDEG_TO_RAD, // outlier_limit
+ 80.0f * MDEG_TO_RAD, // temp_sensitivity_limit
+ 3.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
+ 0.1f * MDEG_TO_RAD, // significant_offset_change
+ 5, // min_num_model_pts
+ true // over_temp_enable
+ };
+ overTempCalInit(&mTask.over_temp_gyro_cal, &gyro_otc_parameters);
+
#endif // OVERTEMPCAL_ENABLED
#endif // GYRO_CAL_ENABLED
#ifdef MAG_SLAVE_PRESENT
+ const struct MagCalParameters mag_cal_parameters = {
+ 3000000, // min_batch_window_in_micros
+ 0.0f, // x_bias
+ 0.0f, // y_bias
+ 0.0f, // z_bias
+ 1.0f, // c00
+ 0.0f, // c01
+ 0.0f, // c02
+ 0.0f, // c10
+ 1.0f, // c11
+ 0.0f, // c12
+ 0.0f, // c20
+ 0.0f, // c21
+ 1.0f // c22
+ };
#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
- 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
+ // Initializes the magnetometer offset calibration algorithm with diversity
+ // checker.
+ const struct DiversityCheckerParameters mag_diversity_parameters = {
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.0f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f, // max_distance_tuning_param
+ 8, // min_num_diverse_vectors
+ 1 // max_num_max_distance
+ };
+ initMagCal(&mTask.moc, &mag_cal_parameters, &mag_diversity_parameters);
#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
- 3000000); // min_batch_window_in_micros
+ // Initializes the magnetometer offset calibration algorithm.
+ initMagCal(&mTask.moc, &mag_cal_parameters);
#endif
#endif
