release-request-5ee66a34-3e58-4de0-ba73-1f49074ee497-for-git_oc-mr1-release-4169335 snap-temp-L20600000081126177

Change-Id: Ic419f6db018b3e470b4377dfbb7e5221de6f0d0e

3 files changed, 181 insertions, 163 deletions

diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 83c5487b..2dd4c9a4 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -27,11 +27,15 @@
 
 // Maximum gyro bias correction (should be set based on expected max bias
 // of the given sensor).
-#define MAX_GYRO_BIAS (0.1f)  // [rad/sec]
+#define MAX_GYRO_BIAS (0.2f)  // [rad/sec]
 
 // Converts units of radians to milli-degrees.
 #define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
 
+// Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and
+// resets when exceeded.
+#define GYRO_WATCHDOG_TIMEOUT_NANOS (5000000000)
+
 #ifdef GYRO_CAL_DBG_ENABLED
 // The time value used to throttle debug messaging.
 #define GYROCAL_WAIT_TIME_NANOS (100000000)
@@ -40,7 +44,7 @@
 #define NANOS_TO_SEC (1.0e-9f)
 
 // A debug version label to help with tracking results.
-#define GYROCAL_DEBUG_VERSION_STRING "[May 15, 2017]"
+#define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
 
 // Debug log tag string used to identify debug report output data.
 #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
@@ -194,8 +198,7 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   gyro_cal->window_time_duration_nanos = window_time_duration_nanos;
 
   // Set the watchdog timeout duration.
-  gyro_cal->gyro_watchdog_timeout_duration_nanos =
-      2 * window_time_duration_nanos;
+  gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS;
 
   // Load the last valid cal from system memory.
   gyro_cal->bias_x = bias_x;  // [rad/sec]
@@ -225,9 +228,6 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
 
 #ifdef GYRO_CAL_DBG_ENABLED
-  CAL_DEBUG_LOG("[GYRO_CAL:MEMORY]", "sizeof(struct GyroCal): %lu",
-                (unsigned long int)sizeof(struct GyroCal));
-
   if (gyro_cal->gyro_calibration_enable) {
     CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED.");
   } else {
@@ -263,11 +263,11 @@ void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
   gyro_cal->calibration_time_nanos = calibration_time_nanos;
 
 #ifdef GYRO_CAL_DBG_ENABLED
-  CAL_DEBUG_LOG("[GYRO_CAL:RECALL]",
-                "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
-                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
-                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+  CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
+                "Gyro Bias Calibration [mDPS]: %s%d.%03d, %s%d.%03d, %s%d.%03d",
+                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 3));
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -518,18 +518,18 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
         gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
 #ifdef GYRO_CAL_DBG_ENABLED
     CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
-                  "Offset|Temp|Time [mdps|C|nsec]: %s%d.%06d, %s%d.%06d, "
-                  "%s%d.%06d, %s%d.%06d, %llu",
+                  "Offset|Temp|Time [mDPS|C|nsec]: %s%d.%03d, %s%d.%03d, "
+                  "%s%d.%03d, %s%d.%03d, %llu",
                   CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
                                        RAD_TO_MILLI_DEGREES,
-                                   6),
+                                   3),
                   CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
                                        RAD_TO_MILLI_DEGREES,
-                                   6),
+                                   3),
                   CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
                                        RAD_TO_MILLI_DEGREES,
-                                   6),
-                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 6),
+                                   3),
+                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
                   (unsigned long long int)calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
@@ -956,18 +956,18 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
   switch (print_data) {
     case OFFSET:
       CAL_DEBUG_LOG(debug_tag,
-                    "Cal#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, "
-                    "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+                    "Cal#|Offset|Temp|Time [mDPS|C|nsec]: %lu, %s%d.%03d, "
+                    "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
                     (unsigned long int)gyro_cal->debug_calibration_count,
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] *
                                          RAD_TO_MILLI_DEGREES,
-                                     6),
+                                     3),
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
                                          RAD_TO_MILLI_DEGREES,
-                                     6),
+                                     3),
                     CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
                                          RAD_TO_MILLI_DEGREES,
-                                     6),
+                                     3),
                     CAL_ENCODE_FLOAT(
                         gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
                     (unsigned long long int)
@@ -980,12 +980,13 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
                      : -1.0f;  // Signals that magnetometer was not used.
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Start|End|Confidence [nsec]: %lu, %llu, %llu, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "Cal#|Stillness|Confidence [nsec]: %lu, %llu, %s%d.%03d, %s%d.%03d, "
+          "%s%d.%03d",
           (unsigned long int)gyro_cal->debug_calibration_count,
-          (unsigned long long int)
-              gyro_cal->debug_gyro_cal.start_still_time_nanos,
-          (unsigned long long int)gyro_cal->debug_gyro_cal.end_still_time_nanos,
+          (unsigned long long int)(gyro_cal->debug_gyro_cal
+                                       .end_still_time_nanos -
+                                   gyro_cal->debug_gyro_cal
+                                       .start_still_time_nanos),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
           CAL_ENCODE_FLOAT(mag_data, 3));
@@ -1013,32 +1014,32 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case GYRO_MINMAX_STILLNESS_MEAN:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Peak Stillness Variation [mdps]: %lu, %s%d.%06d, "
-          "%s%d.%06d, %s%d.%06d",
+          "Cal#|Gyro Peak Stillness Variation [mDPS]: %lu, %s%d.%03d, "
+          "%s%d.%03d, %s%d.%03d",
           (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
                                RAD_TO_MILLI_DEGREES,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
                                RAD_TO_MILLI_DEGREES,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
                             gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
                                RAD_TO_MILLI_DEGREES,
-                           6));
+                           3));
       break;
 
     case ACCEL_STATS:
       CAL_DEBUG_LOG(
           debug_tag,
           "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
-          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
           (unsigned long int)gyro_cal->debug_calibration_count,
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 8),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 8),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 8));
@@ -1047,32 +1048,38 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case GYRO_STATS:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Mean|Var [mdps|(rad/sec)^2]: %lu, %s%d.%06d, "
-          "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, %s%d.%03d, "
+          "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
           (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 6),
+              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
           CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 6),
+              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 3),
           CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
+              gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0] *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+                           6));
       break;
 
     case MAG_STATS:
       if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
         CAL_DEBUG_LOG(debug_tag,
-                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%06d, "
-                      "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+                      "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%03d, "
+                      "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
                       (unsigned long int)gyro_cal->debug_calibration_count,
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 8),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 8),
-                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 8));
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
+                      CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
       } else {
         CAL_DEBUG_LOG(debug_tag,
                       "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
@@ -1085,47 +1092,53 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case ACCEL_STATS_TUNING:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%06d, "
-          "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 6),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8),
-          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8));
+          "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%03d, "
+          "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 6),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 6),
+          CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 6));
       break;
 
     case GYRO_STATS_TUNING:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Gyro Mean|Var [mdps|(rad/sec)^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
-          "%s%d.%08d, %s%d.%08d, %s%d.%08d",
+          "Gyro Mean|Var [mDPS|mDPS^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
+          "%s%d.%06d, %s%d.%06d, %s%d.%06d",
           CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
                                RAD_TO_MILLI_DEGREES,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
                                RAD_TO_MILLI_DEGREES,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
                                RAD_TO_MILLI_DEGREES,
+                           3),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
                            6),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8));
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z *
+                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+                           6));
       break;
 
     case MAG_STATS_TUNING:
       if (gyro_cal->using_mag_sensor) {
         CAL_DEBUG_LOG(
             debug_tag,
-            "Mag Mean|Var [uT|uT^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
-            "%s%d.%08d, %s%d.%08d, %s%d.%08d",
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 6),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8),
-            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8));
+            "Mag Mean|Var [uT|uT^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
+            "%s%d.%06d, %s%d.%06d, %s%d.%06d",
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 3),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 3),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 3),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
+            CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
       } else {
         CAL_DEBUG_LOG(GYROCAL_TUNE_TAG,
                       "Mag Mean|Var [uT|uT^2]: 0, 0, 0, -1.0, -1.0, -1.0");
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index abcb7e32..35e4d589 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -39,7 +39,7 @@
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // A debug version label to help with tracking results.
-#define OTC_DEBUG_VERSION_STRING "[May 15, 2017]"
+#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
 
 // The time value used to throttle debug messaging.
 #define OTC_WAIT_TIME_NANOS (100000000)
@@ -281,9 +281,6 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
                               1e3f * 180.0f / NANO_PI);
 
   createDebugTag(over_temp_cal, ":INIT]");
-  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "sizeof(struct OverTempCal): %lu",
-                (unsigned long int)sizeof(struct OverTempCal));
-
   if (over_temp_cal->over_temp_enable) {
     CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                   "Over-temperature compensation ENABLED.");
@@ -359,20 +356,33 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Prints the recalled model data.
-  createDebugTag(over_temp_cal, ":RECALL]");
-  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                "Temperature|Offset|Sensitivity|Intercept [C|units/C|units]: "
-                "%s%d.%06d, | %s%d.%06d, %s%d.%06d, %s%d.%06d | %s%d.%06d, "
-                "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d",
-                CAL_ENCODE_FLOAT(offset_temp_celsius, 6),
-                CAL_ENCODE_FLOAT(offset[0], 6), CAL_ENCODE_FLOAT(offset[1], 6),
-                CAL_ENCODE_FLOAT(offset[2], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[2], 6));
+  createDebugTag(over_temp_cal, ":SET MODEL]");
+  CAL_DEBUG_LOG(
+      over_temp_cal->otc_debug_tag,
+      "Offset|Temp [%s|C]: %s%d.%03d, %s%d.%03d, %s%d.%03d | %s%d.%03d",
+      over_temp_cal->otc_unit_tag,
+      CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
+      CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
+      CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3),
+      CAL_ENCODE_FLOAT(offset_temp_celsius, 3));
+
+  CAL_DEBUG_LOG(
+      over_temp_cal->otc_debug_tag,
+      "Sensitivity|Intercept [%s/C|%s]: %s%d.%03d, %s%d.%03d, %s%d.%03d | "
+      "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+      over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
+      CAL_ENCODE_FLOAT(temp_sensitivity[0] * over_temp_cal->otc_unit_conversion,
+                       3),
+      CAL_ENCODE_FLOAT(temp_sensitivity[1] * over_temp_cal->otc_unit_conversion,
+                       3),
+      CAL_ENCODE_FLOAT(temp_sensitivity[2] * over_temp_cal->otc_unit_conversion,
+                       3),
+      CAL_ENCODE_FLOAT(sensor_intercept[0] * over_temp_cal->otc_unit_conversion,
+                       3),
+      CAL_ENCODE_FLOAT(sensor_intercept[1] * over_temp_cal->otc_unit_conversion,
+                       3),
+      CAL_ENCODE_FLOAT(sensor_intercept[2] * over_temp_cal->otc_unit_conversion,
+                       3));
 
   // Resets the debug print machine to ensure that updateDebugData() can
   // produce a debug report and interupt any ongoing report.
@@ -400,25 +410,6 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
 
   // Gets the latest temperature compensated offset estimate.
   overTempCalGetOffset(over_temp_cal, offset_temp_celsius, offset);
-
-#ifdef OVERTEMPCAL_DBG_ENABLED
-  // Prints the updated model data.
-  createDebugTag(over_temp_cal, ":STORED]");
-  CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                "Temperature|Offset|Sensitivity|Intercept [C|units/C|units]: "
-                "%s%d.%06d, | %s%d.%06d, %s%d.%06d, %s%d.%06d | %s%d.%06d, "
-                "%s%d.%06d, %s%d.%06d | %s%d.%06d, %s%d.%06d, %s%d.%06d",
-                CAL_ENCODE_FLOAT(*offset_temp_celsius, 6),
-                CAL_ENCODE_FLOAT(offset[0], 6),
-                CAL_ENCODE_FLOAT(offset[1], 6),
-                CAL_ENCODE_FLOAT(offset[2], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[0], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[1], 6),
-                CAL_ENCODE_FLOAT(temp_sensitivity[2], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[0], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[1], 6),
-                CAL_ENCODE_FLOAT(sensor_intercept[2], 6));
-#endif  // OVERTEMPCAL_DBG_ENABLED
 }
 
 void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
@@ -464,7 +455,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Prints the updated model data.
-  createDebugTag(over_temp_cal, ":RECALL]");
+  createDebugTag(over_temp_cal, ":SET MODEL DATA SET]");
   CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
                 "Over-temperature full model data set recalled.");
 
@@ -572,11 +563,11 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "Offset|Temperature|Time [%s|C|nsec]: "
-          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
           over_temp_cal->otc_unit_tag,
-          CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 6),
-          CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 6),
-          CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 6),
+          CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
+          CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
+          CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(temperature_celsius, 3),
           (unsigned long long int)timestamp_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
@@ -695,8 +686,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
     // Prints out temperature and the current timestamp.
     createDebugTag(over_temp_cal, ":TEMP]");
     CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                  "Temperature|Time [C|nsec] = %s%d.%06d, %llu",
-                  CAL_ENCODE_FLOAT(temperature_celsius, 6),
+                  "Temperature|Time [C|nsec] = %s%d.%03d, %llu",
+                  CAL_ENCODE_FLOAT(temperature_celsius, 3),
                   (unsigned long long int)timestamp_nanos);
   }
 #endif  // OVERTEMPCAL_DBG_LOG_TEMP
@@ -926,15 +917,15 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "%c-Axis Parameters|Max Error|Time [%s/C|%s|%s|nsec]: "
-          "%s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
+          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
           kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
           over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(
-              temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 6),
+              temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(
-              sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 6),
+              sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3),
           CAL_ENCODE_FLOAT(max_error[i] * over_temp_cal->otc_unit_conversion,
-                           6),
+                           3),
           (unsigned long long int)timestamp_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
     }
@@ -1024,18 +1015,18 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
   createDebugTag(over_temp_cal, ":REMOVE]");
   CAL_DEBUG_LOG(
       over_temp_cal->otc_debug_tag,
-      "Offset|Temp|Time [%s|C|nsec]: %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+      "Offset|Temp|Time [%s|C|nsec]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
       "%s%d.%03d, %llu",
       over_temp_cal->otc_unit_tag,
       CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
                            over_temp_cal->otc_unit_conversion,
-                       6),
+                       3),
       CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
                            over_temp_cal->otc_unit_conversion,
-                       6),
+                       3),
       CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
                            over_temp_cal->otc_unit_conversion,
-                       6),
+                       3),
       CAL_ENCODE_FLOAT(
           over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
       (unsigned long long int)over_temp_cal->model_data[model_index]
@@ -1272,25 +1263,25 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       // Prints out the latest offset estimate (input data).
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, %s%d.%06d, "
-          "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, %s%d.%03d, "
+          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
               over_temp_cal->debug_overtempcal.latest_offset.offset[0] *
                   over_temp_cal->otc_unit_conversion,
-              6),
+              3),
           CAL_ENCODE_FLOAT(
               over_temp_cal->debug_overtempcal.latest_offset.offset[1] *
                   over_temp_cal->otc_unit_conversion,
-              6),
+              3),
           CAL_ENCODE_FLOAT(
               over_temp_cal->debug_overtempcal.latest_offset.offset[2] *
                   over_temp_cal->otc_unit_conversion,
-              6),
+              3),
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset
                                .offset_temp_celsius,
-                           6),
+                           3),
           (unsigned long long int)
               over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);
 
@@ -1301,35 +1292,40 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
 
     case OTC_PRINT_MODEL_PARAMETERS:
       // Prints out the model parameters.
+      CAL_DEBUG_LOG(
+          over_temp_cal->otc_debug_tag,
+          "Cal#|Sensitivity [%s/C]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          over_temp_cal->otc_unit_tag,
+          (unsigned long int)over_temp_cal->debug_num_estimates,
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+                  over_temp_cal->otc_unit_conversion,
+              3),
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+                  over_temp_cal->otc_unit_conversion,
+              3),
+          CAL_ENCODE_FLOAT(
+              over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+                  over_temp_cal->otc_unit_conversion,
+              3));
+
       CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                    "Cal#|Sensitivity|Intercept [%s/C|%s]: %lu, %s%d.%06d, "
-                    "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
-                    over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
+                    "Cal#|Intercept [%s]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+                    over_temp_cal->otc_unit_tag,
                     (unsigned long int)over_temp_cal->debug_num_estimates,
                     CAL_ENCODE_FLOAT(
-                        over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
-                            over_temp_cal->otc_unit_conversion,
-                        6),
-                    CAL_ENCODE_FLOAT(
-                        over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
-                            over_temp_cal->otc_unit_conversion,
-                        6),
-                    CAL_ENCODE_FLOAT(
-                        over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
-                            over_temp_cal->otc_unit_conversion,
-                        6),
-                    CAL_ENCODE_FLOAT(
                         over_temp_cal->debug_overtempcal.sensor_intercept[0] *
                             over_temp_cal->otc_unit_conversion,
-                        6),
+                        3),
                     CAL_ENCODE_FLOAT(
                         over_temp_cal->debug_overtempcal.sensor_intercept[1] *
                             over_temp_cal->otc_unit_conversion,
-                        6),
+                        3),
                     CAL_ENCODE_FLOAT(
                         over_temp_cal->debug_overtempcal.sensor_intercept[2] *
                             over_temp_cal->otc_unit_conversion,
-                        6));
+                        3));
 
       wait_timer = timestamp_nanos;                 // Starts the wait timer.
       next_state = OTC_PRINT_MODEL_ERROR;           // Sets the next state.
@@ -1340,23 +1336,21 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       // Computes the maximum error over all of the model data.
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "Cal#|#Updates|#ModelPts|Model Error|Update Time [%s|nsec]: %lu, "
-          "%lu, %lu, %s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
+          "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
+          "%lu, %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           (unsigned long int)over_temp_cal->debug_num_model_updates,
           (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
                                over_temp_cal->otc_unit_conversion,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[1] *
                                over_temp_cal->otc_unit_conversion,
-                           6),
+                           3),
           CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[2] *
                                over_temp_cal->otc_unit_conversion,
-                           6),
-          (unsigned long long int)
-              over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos);
+                           3));
 
       i = 0;                          // Resets the model data printer counter.
       wait_timer = timestamp_nanos;       // Starts the wait timer.
@@ -1369,18 +1363,18 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       if (i < over_temp_cal->num_model_pts) {
         CAL_DEBUG_LOG(
             over_temp_cal->otc_debug_tag,
-            "  Model[%lu] [%s|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+            "  Model[%lu] [%s|C|nsec] = %s%d.%03d, %s%d.%03d, %s%d.%03d, "
             "%s%d.%03d, %llu",
             (unsigned long int)i, over_temp_cal->otc_unit_tag,
             CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[0] *
                                  over_temp_cal->otc_unit_conversion,
-                             6),
+                             3),
             CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[1] *
                                  over_temp_cal->otc_unit_conversion,
-                             6),
+                             3),
             CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[2] *
                                  over_temp_cal->otc_unit_conversion,
-                             6),
+                             3),
             CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp_celsius,
                              3),
             (unsigned long long int)over_temp_cal->model_data[i]
diff --git a/firmware/os/drivers/st_mag40/st_mag40.c b/firmware/os/drivers/st_mag40/st_mag40.c
index c96e3400..bdc874aa 100644
--- a/firmware/os/drivers/st_mag40/st_mag40.c
+++ b/firmware/os/drivers/st_mag40/st_mag40.c
@@ -215,6 +215,7 @@ struct st_mag40_Task {
     uint8_t subState;
 
     /* sensor flags */
+    uint8_t samplesToDiscard;
     uint32_t rate;
     uint64_t latency;
     bool magOn;
@@ -484,6 +485,7 @@ static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
     mTask.currentODR = st_mag40_regVal[num];
     mTask.rate = rate;
     mTask.latency = latency;
+    mTask.samplesToDiscard = 2;
 
     if (trySwitchState(SENSOR_MAG_CONFIGURATION)) {
         mTask.subState = CONFIG_SET_RATE;
@@ -699,6 +701,12 @@ static void parseRawData(uint8_t *raw)
     float xi, yi, zi;
 #endif
 
+	/* Discard samples generated during sensor turn-on time */
+    if (mTask.samplesToDiscard > 0) {
+        mTask.samplesToDiscard--;
+        return;
+    }
+
     /* in uT */
     xs = (float)raw_x * kScale_mag;
     ys = (float)raw_y * kScale_mag;
@@ -781,6 +789,7 @@ static void sensorMagConfig(void)
     switch (mTask.subState) {
     case CONFIG_POWER_UP:
         mTask.subState = CONFIG_POWER_UP_2;
+        mTask.comm_tx(ST_MAG40_CFG_B_REG_ADDR, ST_MAG40_OFF_CANC, 0, false);
         mTask.comm_tx(ST_MAG40_CFG_A_REG_ADDR,
                       ST_MAG40_TEMP_COMP_EN | ST_MAG40_POWER_ON | mTask.currentODR, 0, true);
         break;
@@ -838,6 +847,7 @@ static void sensorInit(void)
 
     case INIT_ENABLE_DRDY:
         mTask.subState = INIT_DONE;
+        mTask.comm_rx(ST_MAG40_OUTXL_REG_ADDR, 6, 0, false);
         mTask.comm_tx(ST_MAG40_CFG_C_REG_ADDR,
                     ST_MAG40_BDU_ON | ST_MAG40_INT_MAG, 0, true);
         break;
@@ -883,6 +893,7 @@ static void handleCommDoneEvt(const void* evtData)
 
     case SENSOR_INITIALIZATION:
         if (mTask.subState == INIT_DONE) {
+            INFO_PRINT( "Initialization completed\n");
             returnIdle = true;
             sensorRegisterInitComplete(mTask.magHandle);
         } else {