release-request-f0fc1aa1-730c-4deb-b1ff-e8ebeeccc4b5-for-git_oc-mr1-release-4335659 snap-temp-L85900000102427279

Change-Id: I5e49bb012d6b074032f3a59a74799a66f820af4d

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;