[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
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 0a6d96d..c700253 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -180,6 +180,7 @@
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 @@
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 @@
// 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 e76abcd..1cfef61 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -139,6 +139,9 @@
// 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 1fb7630..7f8e563 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -71,7 +71,10 @@
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 @@
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;
