diff options
author | David Jacobs <davejacobs@google.com> | 2017-09-12 18:18:29 -0700 |
---|---|---|
committer | David Jacobs <davejacobs@google.com> | 2017-09-12 18:18:29 -0700 |
commit | a619ebe0306e220e33b169c95b7d3c8ac6b57e63 (patch) | |
tree | 5a40f3c11d214d13b0a52b26296ee2c6fc777619 | |
parent | 596363edf151b423ef0e51126733ecba36794be7 (diff) | |
download | device_google_contexthub-a619ebe0306e220e33b169c95b7d3c8ac6b57e63.tar.gz device_google_contexthub-a619ebe0306e220e33b169c95b7d3c8ac6b57e63.tar.bz2 device_google_contexthub-a619ebe0306e220e33b169c95b7d3c8ac6b57e63.zip |
[Accel/Mag Cal] Ensures sqrtf(X), only for X > 0
This CL syncs a change from Google3 runtime accelerometer
and magnetometer calibration code to ensure that sqrt() is
only called for non-negative numbers.
Tip of G3 CL: 168477539
Bug: 65559982
Test: Built and verified on hardware.
Change-Id: Idb182a84d69982a3351fe5cea444f5b77716f471
-rw-r--r-- | firmware/os/algos/calibration/accelerometer/accel_cal.c | 10 | ||||
-rw-r--r-- | firmware/os/algos/calibration/accelerometer/accel_cal.h | 3 | ||||
-rw-r--r-- | firmware/os/algos/calibration/magnetometer/mag_cal.c | 8 |
3 files changed, 17 insertions, 4 deletions
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index 0a6d96d8..c7002535 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.c +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c @@ -180,6 +180,7 @@ void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th, acc->x_bias = acc->y_bias = acc->z_bias = 0; acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0; + acc->average_temperature_celsius = 0; #ifdef IMU_TEMP_DBG_ENABLED acc->temp_time_nanos = 0; @@ -422,7 +423,11 @@ static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) { float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; - float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z); + float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; + + // Testing for negative number. + float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; + // Passing when evmin/evmax> EIGEN_RATIO. int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG); @@ -526,10 +531,11 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, // Eigen Ratio Test. if (accEigenTest(&acc->ac1[temp_gate].akf, &acc->ac1[temp_gate].agd)) { - // Storing the new offsets. + // Storing the new offsets and average temperature. acc->x_bias_new = bias.x * KSCALE2; acc->y_bias_new = bias.y * KSCALE2; acc->z_bias_new = bias.z * KSCALE2; + acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t; } #ifdef ACCEL_CAL_DBG_ENABLED //// Debug /////// diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h index e76abcd3..1cfef614 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.h +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h @@ -139,6 +139,9 @@ struct AccelCal { // to store a new offset, which gets updated during a power down event. float x_bias_new, y_bias_new, z_bias_new; + // Average temperature of the bias update. + float average_temperature_celsius; + // Offset values that get subtracted from live data float x_bias, y_bias, z_bias; diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c index 1fb76302..7f8e563f 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.c +++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c @@ -71,7 +71,10 @@ static int moc_eigen_test(struct KasaFit *kasa) { float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; - float evmag = sqrtf(eigenvals.x + eigenvals.y + eigenvals.z); + float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; + + // Testing for negative number. + float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) && (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG); @@ -119,7 +122,8 @@ int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) { initVec3(&v, out.x, out.y, out.z); vec3ScalarMul(&v, -0.5f); - float r = sqrtf(vec3Dot(&v, &v) - out.w); + float r_square = vec3Dot(&v, &v) - out.w; + float r = (r_square > 0) ? sqrtf(r_square) : 0; initVec3(bias, v.x, v.y, v.z); *radius = r; |