diff options
author | Ben Fennema <fennema@google.com> | 2018-08-15 18:11:56 -0700 |
---|---|---|
committer | Ben Fennema <fennema@google.com> | 2018-10-10 10:53:52 -0700 |
commit | 69a97582d1c8ea7794d375e20a594ea7436942c4 (patch) | |
tree | 8d6d69207ff74a9809a75872869b3b33dd8aa88a | |
parent | 4d6104be79e2438af24b58c0da02398e77927b84 (diff) | |
download | device_google_contexthub-69a97582d1c8ea7794d375e20a594ea7436942c4.tar.gz device_google_contexthub-69a97582d1c8ea7794d375e20a594ea7436942c4.tar.bz2 device_google_contexthub-69a97582d1c8ea7794d375e20a594ea7436942c4.zip |
nanohub:drivers:st_lsm6dsm: sync with imu cal changes
Test: builds with accel, gyro, overtemp and mag cal enabled
Change-Id: I053148ab64dec1cc0359e40d678b25b2c41dc729
Signed-off-by: Ben Fennema <fennema@google.com>
-rw-r--r-- | firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c | 151 | ||||
-rw-r--r-- | firmware/variant/argonkey/inc/variant/sensType.h | 1 | ||||
-rw-r--r-- | firmware/variant/argonkey/inc/variant/variant.h | 2 |
3 files changed, 90 insertions, 64 deletions
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c index 3d7522e3..d8e7e87b 100644 --- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c +++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c @@ -38,7 +38,7 @@ #include <plat/rtc.h> #include <calibration/accelerometer/accel_cal.h> #include <calibration/gyroscope/gyro_cal.h> -#include <calibration/magnetometer/mag_cal.h> +#include <calibration/magnetometer/mag_cal/mag_cal.h> #include <calibration/over_temp/over_temp_cal.h> #include <algos/time_sync.h> @@ -47,7 +47,7 @@ #include "st_lsm6dsm_ak09916_slave.h" #include "st_lsm6dsm_lps22hb_slave.h" -#define LSM6DSM_APP_VERSION 1 +#define LSM6DSM_APP_VERSION 2 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) #define LSM6DSM_I2C_MASTER_ENABLED 1 @@ -3734,9 +3734,8 @@ static bool lsm6dsm_magnCfgData(void *data, void *cookie) (int)(d->inclination * 180 / M_PI + 0.5f)); // Passing local field information to mag calibration routine -#ifdef DIVERSITY_CHECK_ENABLED diversityCheckerLocalFieldUpdate(&T(magnCal).diversity_checker, d->strength); -#endif + // TODO: pass local field information to rotation vector sensor. } else { ERROR_PRINT("lsm6dsm_magnCfgData: unknown type 0x%04x, size %d", p->type, p->size); @@ -3850,7 +3849,8 @@ static bool lsm6dsm_gyroCfgData(void *data, void *cookie) struct LSM6DSMAccelGyroCfgData *cfgData = data; #ifdef LSM6DSM_GYRO_CALIB_ENABLED - gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], sensorGetTime()); + const float dummy_temperature_celsius = 25.0f; + gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], dummy_temperature_celsius, sensorGetTime()); #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ DEBUG_PRINT("Gyro hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]); @@ -4263,8 +4263,9 @@ static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, ui if (gyroCalNewBiasAvailable(&T(gyroCal))) { float biasTemperature, gyroOffset[3] = { 0.0f, 0.0f, 0.0f }; + uint64_t calTime; - gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature); + gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature, &calTime); if (!samples->firstSample.biasCurrent) { samples->firstSample.biasCurrent = true; @@ -5089,73 +5090,95 @@ static bool lsm6dsm_startTask(uint32_t taskId) #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ #ifdef LSM6DSM_ACCEL_CALIB_ENABLED - accelCalInit(&T(accelCal), - 800000000, /* stillness period [ns] */ - 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 accelCalParameters = { + 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(&T(accelCal), &accelCalParameters); #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ #ifdef LSM6DSM_GYRO_CALIB_ENABLED - gyroCalInit(&T(gyroCal), - 5e9, /* min stillness period [ns] */ - 6e9, /* max stillness period [ns] */ - 0, 0, 0, /* initial bias offset calibration values [rad/sec] */ - 0, /* timestamp of initial bias calibration [ns] */ - 1.5e9, /* analysis window length = 1.5 seconds [ns] */ - 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] */ - 1.4f, /* 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 */ - true); /* gyro calibration enable */ + const struct GyroCalParameters gyroCalParameters = { + 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] + 5e-5f, // gyro_var_threshold [rad/sec]^2 + 1e-5f, // gyro_confidence_delta [rad/sec]^2 + 8e-3f, // accel_var_threshold [m/sec^2]^2 + 1.6e-3f, // accel_confidence_delta [m/sec^2]^2 + 1.4f, // mag_var_threshold [uTesla]^2 + 0.25f, // 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(&T(gyroCal), &gyroCalParameters); #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED - overTempCalInit(&T(overTempCal), - 5, /* min num of points to enable model update */ - 5000000000, /* min model update interval [ns] */ - 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 [ns] */ - 50.0e-3f * M_PI / 180.0f, /* limit for temperature sensitivity [(rad/sec)/C] */ - 3.0f * M_PI / 180.0f, /* limit for model intercept parameter [rad/sec] */ - true); /* over temperature compensation enable */ + // Initializes the gyroscope over-temperature offset compensation algorithm. + const struct OverTempCalParameters gyroOtcParameters = { + 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(&T(overTempCal), &gyroOtcParameters); #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ #ifdef LSM6DSM_MAGN_CALIB_ENABLED -#ifdef DIVERSITY_CHECK_ENABLED - initMagCal(&T(magnCal), - 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */ - 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */ - 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */ - 0.0f, 0.0f, 1.0f, /* magn scale matrix 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(&T(magnCal), - 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */ - 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */ - 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */ - 0.0f, 0.0f, 1.0f, /* magn scale matrix c20 - c21 - c22 */ - 3000000); /* min_batch_window_in_micros */ -#endif + const struct MagCalParameters magCalParameters = { + 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 + }; + + // Initializes the magnetometer offset calibration algorithm with diversity + // checker. + const struct DiversityCheckerParameters magDiversityParameters = { + 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(&T(magnCal), &magCalParameters, &magDiversityParameters); #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ /* Initialize index used to fill/get data from buffer */ diff --git a/firmware/variant/argonkey/inc/variant/sensType.h b/firmware/variant/argonkey/inc/variant/sensType.h index 23cd0ef4..6fca84ac 100644 --- a/firmware/variant/argonkey/inc/variant/sensType.h +++ b/firmware/variant/argonkey/inc/variant/sensType.h @@ -23,6 +23,7 @@ extern "C" { #define SENS_TYPE_GYRO_BIAS (SENS_TYPE_FIRST_USER + 0) #define SENS_TYPE_MAG_BIAS (SENS_TYPE_FIRST_USER + 1) +#define SENS_TYPE_ACCEL_BIAS (SENS_TYPE_FIRST_USER + 2) #ifdef __cplusplus } diff --git a/firmware/variant/argonkey/inc/variant/variant.h b/firmware/variant/argonkey/inc/variant/variant.h index 62fb4e84..abd92d20 100644 --- a/firmware/variant/argonkey/inc/variant/variant.h +++ b/firmware/variant/argonkey/inc/variant/variant.h @@ -84,8 +84,10 @@ extern "C" { #define LSM6DSM_ACCEL_GYRO_ROT_MATRIX -1, 0, 0, 0, -1, 0, 0, 0, 1 #define LSM6DSM_MAGN_ROT_MATRIX -1, 0, 0, 0, 1, 0, 0, 0, 1 +#define LSM6DSM_ACCEL_CALIB_ENABLED 1 #define LSM6DSM_GYRO_CALIB_ENABLED 1 #define LSM6DSM_MAGN_CALIB_ENABLED 1 +#define LSM6DSM_OVERTEMP_CALIB_ENABLED 1 /* ------------------------------------------------------------------------ */ /* sensor baro+temp lps22hb on i2c */ |