Merge "[GyroCal/OTC-Gyro] Algorithm Sync for MR1" into oc-mr1-dev

9 files changed, 489 insertions, 534 deletions

diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index d6a69f3e..3179b0eb 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -21,7 +21,7 @@
 #include <string.h>
 
 #include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
 
 /////// DEFINITIONS AND MACROS ///////////////////////////////////////
 
@@ -29,28 +29,19 @@
 // of the given sensor).
 #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)
+#define GYRO_WATCHDOG_TIMEOUT_NANOS (SEC_TO_NANOS(5))
 
 #ifdef GYRO_CAL_DBG_ENABLED
 // The time value used to throttle debug messaging.
-#define GYROCAL_WAIT_TIME_NANOS (100000000)
-
-// Unit conversion: nanoseconds to seconds.
-#define NANOS_TO_SEC (1.0e-9f)
+#define GYROCAL_WAIT_TIME_NANOS (MSEC_TO_NANOS(100))
 
 // A debug version label to help with tracking results.
 #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]"
-
-// Debug log tag string used to identify debug tuning output data.
-#define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]"
 #endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FORWARD DECLARATIONS /////////////////////////////////////////
@@ -113,7 +104,7 @@ enum DebugPrintData {
 };
 
 /*
- * Updates running calculation of the gyro's mean sampling rate.
+ * Updates the running calculation of the gyro's mean sampling rate.
  *
  * Behavior:
  *   1)  If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
@@ -123,11 +114,13 @@ enum DebugPrintData {
  *   3)  Otherwise, the local estimate of the mean sampling rates is updated.
  *
  * INPUTS:
- *   debug_mean_sampling_rate_hz:   Pointer to the mean sampling rate to update.
+ *   sample_rate_estimator:  Pointer to the estimator data structure.
+ *   debug_mean_sampling_rate_hz:  Pointer to the mean sampling rate to update.
  *   timestamp_nanos:  Time stamp (nanoseconds).
  *   reset_stats:  Flag that signals a reset of the sampling rate estimate.
  */
-static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+                                   float* debug_mean_sampling_rate_hz,
                                    uint64_t timestamp_nanos, bool reset_stats);
 
 // Updates the information used for debug printouts.
@@ -146,17 +139,11 @@ static float floatFromUint64(uint64_t v)
     uint32_t hi = v >> 32, lo = v;
 
     if (!hi) //this is very fast for cases where we fit into a uint32_t
-        return(float)lo;
+        return (float)lo;
     else {
         return ((float)hi) * 4294967296.0f + (float)lo;
     }
 }
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-// Prints debug information useful for tuning the GyroCal parameters.
-static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
-                                  uint64_t timestamp_nanos);
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
 #endif  // GYRO_CAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS /////////////////////////////////////////
@@ -235,7 +222,8 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
   }
 
   // Ensures that the gyro sampling rate estimate is reset.
-  gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
+                         /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -264,10 +252,10 @@ void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
 
 #ifdef GYRO_CAL_DBG_ENABLED
   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));
+                "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
+                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
+                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
@@ -310,7 +298,8 @@ void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
 
 #ifdef GYRO_CAL_DBG_ENABLED
   // Update the gyro sampling rate estimate.
-  gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                         sample_time_nanos, /*reset_stats=*/false);
 #endif  // GYRO_CAL_DBG_ENABLED
 
   // Pass gyro data to stillness detector
@@ -453,7 +442,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
 
 #ifdef GYRO_CAL_DBG_ENABLED
       // Resets the sampling rate estimate.
-      gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+      gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                             sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
       // Update stillness flag. Force the start of a new stillness period.
@@ -496,7 +486,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal,
 
 #ifdef GYRO_CAL_DBG_ENABLED
     // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                           sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Update stillness flag.
@@ -517,20 +508,18 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
         gyro_cal->gyro_stillness_detect.prev_mean_z <  MAX_GYRO_BIAS &&
         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.%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,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
-                                       RAD_TO_MILLI_DEGREES,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
-                                       RAD_TO_MILLI_DEGREES,
-                                   3),
-                  CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
-                  (unsigned long long int)calibration_time_nanos);
+    CAL_DEBUG_LOG(
+        "[GYRO_CAL:REJECT]",
+        "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+        ", " CAL_FORMAT_3DIGITS ", %llu",
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(
+            gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
+        CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
+        (unsigned long long int)calibration_time_nanos);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Outside of range. Ignore, reset, and continue.
@@ -620,7 +609,8 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
 
 #ifdef GYRO_CAL_DBG_ENABLED
     // Resets the sampling rate estimate.
-    gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+    gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+                           sample_time_nanos, /*reset_stats=*/true);
 #endif  // GYRO_CAL_DBG_ENABLED
 
     // Resets the stillness window end-time.
@@ -661,10 +651,8 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
       gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f;
 
       // Initializes the min/max temperatures values.
-      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
-          FLT_MAX;
-      gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
-          -FLT_MAX;
+      gyro_cal->temperature_mean_tracker.temperature_min_celsius = FLT_MAX;
+      gyro_cal->temperature_mean_tracker.temperature_max_celsius = -FLT_MAX;
       break;
 
     case DO_UPDATE_DATA:
@@ -676,14 +664,14 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
       // Tracks the min, max, and latest temperature values.
       gyro_cal->temperature_mean_tracker.latest_temperature_celsius =
           temperature_celsius;
-      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] >
+      if (gyro_cal->temperature_mean_tracker.temperature_min_celsius >
           temperature_celsius) {
-        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
+        gyro_cal->temperature_mean_tracker.temperature_min_celsius =
             temperature_celsius;
       }
-      if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] <
+      if (gyro_cal->temperature_mean_tracker.temperature_max_celsius <
           temperature_celsius) {
-        gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
+        gyro_cal->temperature_mean_tracker.temperature_max_celsius =
             temperature_celsius;
       }
       break;
@@ -709,9 +697,10 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
       // Records the min/max and mean temperature values for debug purposes.
       gyro_cal->debug_gyro_cal.temperature_mean_celsius =
           gyro_cal->temperature_mean_celsius;
-      memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
-             gyro_cal->temperature_mean_tracker.temperature_min_max_celsius,
-             2 * sizeof(float));
+      gyro_cal->debug_gyro_cal.temperature_min_celsius =
+          gyro_cal->temperature_mean_tracker.temperature_min_celsius;
+      gyro_cal->debug_gyro_cal.temperature_max_celsius =
+          gyro_cal->temperature_mean_tracker.temperature_max_celsius;
 #endif
       break;
 
@@ -719,9 +708,8 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
       // Determines if the min/max delta exceeded the set limit.
       if (gyro_cal->temperature_mean_tracker.num_points > 0) {
         min_max_temp_exceeded =
-            (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] -
-             gyro_cal->temperature_mean_tracker
-                 .temperature_min_max_celsius[0]) >
+            (gyro_cal->temperature_mean_tracker.temperature_max_celsius -
+             gyro_cal->temperature_mean_tracker.temperature_min_celsius) >
             gyro_cal->temperature_delta_limit_celsius;
 
 #ifdef GYRO_CAL_DBG_ENABLED
@@ -743,41 +731,50 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
 
 bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
                           enum GyroCalTrackerCommand do_this) {
-  static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f};
-  static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f};
   bool mean_not_stable = false;
-  size_t i;
 
   switch (do_this) {
     case DO_RESET:
       // Resets the min/max window mean values to a default value.
-      for (i = 0; i < 3; i++) {
-        gyro_winmean_min[i] = FLT_MAX;
-        gyro_winmean_max[i] = -FLT_MAX;
+      for (size_t i = 0; i < 3; i++) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX;
+        gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX;
       }
       break;
 
     case DO_UPDATE_DATA:
       // Computes the min/max window mean values.
-      if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) {
-        gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[0] >
+          gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[0] =
+            gyro_cal->gyro_stillness_detect.win_mean_x;
       }
-      if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) {
-        gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[0] <
+          gyro_cal->gyro_stillness_detect.win_mean_x) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[0] =
+            gyro_cal->gyro_stillness_detect.win_mean_x;
       }
 
-      if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) {
-        gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[1] >
+          gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[1] =
+            gyro_cal->gyro_stillness_detect.win_mean_y;
       }
-      if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) {
-        gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[1] <
+          gyro_cal->gyro_stillness_detect.win_mean_y) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[1] =
+            gyro_cal->gyro_stillness_detect.win_mean_y;
       }
 
-      if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) {
-        gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_min[2] >
+          gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_cal->window_mean_tracker.gyro_winmean_min[2] =
+            gyro_cal->gyro_stillness_detect.win_mean_z;
       }
-      if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
-        gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+      if (gyro_cal->window_mean_tracker.gyro_winmean_max[2] <
+          gyro_cal->gyro_stillness_detect.win_mean_z) {
+        gyro_cal->window_mean_tracker.gyro_winmean_max[2] =
+            gyro_cal->gyro_stillness_detect.win_mean_z;
       }
       break;
 
@@ -785,34 +782,45 @@ bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
       // Store the most recent "stillness" mean data to the GyroCal data
       // structure. This functionality allows previous results to be recalled
       // when the device suddenly becomes "not still".
-      memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float));
-      memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float));
-    break;
+      memcpy(gyro_cal->gyro_winmean_min,
+             gyro_cal->window_mean_tracker.gyro_winmean_min,
+             sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min));
+      memcpy(gyro_cal->gyro_winmean_max,
+             gyro_cal->window_mean_tracker.gyro_winmean_max,
+             sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max));
+      break;
 
     case DO_EVALUATE:
       // Performs the stability check and returns the 'true' if the difference
       // between min/max window mean value is outside the stable range.
-      for (i = 0; i < 3; i++) {
-        mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
+      for (size_t i = 0; i < 3; i++) {
+        mean_not_stable |= (gyro_cal->window_mean_tracker.gyro_winmean_max[i] -
+                            gyro_cal->window_mean_tracker.gyro_winmean_min[i]) >
                            gyro_cal->stillness_mean_delta_limit;
       }
 #ifdef GYRO_CAL_DBG_ENABLED
       if (mean_not_stable) {
         CAL_DEBUG_LOG(
             "[GYRO_CAL:MEAN_STABILITY_GATE]",
-            "Variation Limit|Delta [mDPS]: %s%d.%03d | %s%d.%03d, %s%d.%03d, "
-            "%s%d.%03d",
-            CAL_ENCODE_FLOAT(
-                gyro_cal->stillness_mean_delta_limit * RAD_TO_MILLI_DEGREES, 3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[0] - gyro_winmean_min[0]) *
-                                 RAD_TO_MILLI_DEGREES,
-                             3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[1] - gyro_winmean_min[1]) *
-                                 RAD_TO_MILLI_DEGREES,
+            "Variation Limit|Delta [mDPS]: " CAL_FORMAT_3DIGITS
+            " | " CAL_FORMAT_3DIGITS_TRIPLET,
+            CAL_ENCODE_FLOAT(gyro_cal->stillness_mean_delta_limit * RAD_TO_MDEG,
                              3),
-            CAL_ENCODE_FLOAT((gyro_winmean_max[2] - gyro_winmean_min[2]) *
-                                 RAD_TO_MILLI_DEGREES,
-                             3));
+            CAL_ENCODE_FLOAT(
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[0] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[0]) *
+                    RAD_TO_MDEG,
+                3),
+            CAL_ENCODE_FLOAT(
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[1] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[1]) *
+                    RAD_TO_MDEG,
+                3),
+            CAL_ENCODE_FLOAT(
+                (gyro_cal->window_mean_tracker.gyro_winmean_max[2] -
+                 gyro_cal->window_mean_tracker.gyro_winmean_min[2]) *
+                    RAD_TO_MDEG,
+                3));
       }
 #endif  // GYRO_CAL_DBG_ENABLED
       break;
@@ -825,21 +833,19 @@ bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
 }
 
 #ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+                            float* debug_mean_sampling_rate_hz,
                             uint64_t timestamp_nanos, bool reset_stats) {
-  // This is used for local calculations of average sampling rate.
-  static uint64_t last_timestamp_nanos = 0;
-  static uint64_t time_delta_accumulator = 0;
-  static size_t num_samples = 0;
-
   // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
   // out the estimate of the sampling rate.
   if (debug_mean_sampling_rate_hz) {
-    if (num_samples > 1 && time_delta_accumulator > 0) {
+    if (sample_rate_estimator->num_samples > 1 &&
+        sample_rate_estimator->time_delta_accumulator > 0) {
       // Computes the final mean calculation.
       *debug_mean_sampling_rate_hz =
-          num_samples /
-          (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC);
+          sample_rate_estimator->num_samples /
+          (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
+           NANOS_TO_SEC);
     } else {
       // Not enough samples to compute a valid sample rate estimate. Indicate
       // this with a -1 value.
@@ -850,26 +856,28 @@ void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
 
   // Resets the sampling rate mean estimator data.
   if (reset_stats) {
-    last_timestamp_nanos = 0;
-    time_delta_accumulator = 0;
-    num_samples = 0;
+    sample_rate_estimator->last_timestamp_nanos = 0;
+    sample_rate_estimator->time_delta_accumulator = 0;
+    sample_rate_estimator->num_samples = 0;
     return;
   }
 
   // Skip adding this data to the accumulator if:
   //   1. A bad timestamp was received (i.e., time not monotonic).
   //   2. 'last_timestamp_nanos' is zero.
-  if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) {
-    last_timestamp_nanos = timestamp_nanos;
+  if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+      sample_rate_estimator->last_timestamp_nanos == 0) {
+    sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
     return;
   }
 
   // Increments the number of samples.
-  num_samples++;
+  sample_rate_estimator->num_samples++;
 
   // Accumulate the time steps.
-  time_delta_accumulator += timestamp_nanos - last_timestamp_nanos;
-  last_timestamp_nanos = timestamp_nanos;
+  sample_rate_estimator->time_delta_accumulator +=
+      timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
+  sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
 }
 
 void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
@@ -905,14 +913,15 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
   gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
 
   // Records the mean gyroscope sampling rate.
-  gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
+  gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
+                         &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
                          /*reset_stats=*/true);
 
   // Records the min/max gyroscope window stillness mean values.
   memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
-         3 * sizeof(float));
+         sizeof(gyro_cal->gyro_winmean_min));
   memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
-         3 * sizeof(float));
+         sizeof(gyro_cal->gyro_winmean_max));
 
   // Records the previous stillness window means.
   gyro_cal->debug_gyro_cal.accel_mean[0] =
@@ -971,23 +980,21 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
   float mag_data;
   switch (print_data) {
     case OFFSET:
-      CAL_DEBUG_LOG(debug_tag,
-                    "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,
-                                     3),
-                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
-                                         RAD_TO_MILLI_DEGREES,
-                                     3),
-                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
-                                         RAD_TO_MILLI_DEGREES,
-                                     3),
-                    CAL_ENCODE_FLOAT(
-                        gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
-                    (unsigned long long int)
-                        gyro_cal->debug_gyro_cal.end_still_time_nanos);
+      CAL_DEBUG_LOG(
+          debug_tag,
+          "Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+          (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(
+              gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
+                           3),
+          (unsigned long long int)
+              gyro_cal->debug_gyro_cal.end_still_time_nanos);
       break;
 
     case STILLNESS_DATA:
@@ -996,8 +1003,8 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
                      : -1.0f;  // Signals that magnetometer was not used.
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Stillness|Confidence [nsec]: %lu, %llu, %s%d.%03d, %s%d.%03d, "
-          "%s%d.%03d",
+          "Cal#|Stillness|Confidence [nsec]: %lu, "
+          "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
           (unsigned long int)gyro_cal->debug_calibration_count,
           (unsigned long long int)(gyro_cal->debug_gyro_cal
                                        .end_still_time_nanos -
@@ -1011,91 +1018,90 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
     case SAMPLE_RATE_AND_TEMPERATURE:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d",
+          "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+          ", " CAL_FORMAT_3DIGITS,
           (unsigned long int)gyro_cal->debug_calibration_count,
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
                            3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3),
-          CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] -
-                  gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0],
-              4),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius, 3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
+                               gyro_cal->debug_gyro_cal.temperature_min_celsius,
+                           3),
           CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
       break;
 
     case GYRO_MINMAX_STILLNESS_MEAN:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Peak Stillness Variation [mDPS]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d",
+          "Cal#|Gyro Peak Stillness Variation [mDPS]: "
+          "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           (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,
+                               RAD_TO_MDEG,
                            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,
+                               RAD_TO_MDEG,
                            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,
+                               RAD_TO_MDEG,
                            3));
       break;
 
     case ACCEL_STATS:
-      CAL_DEBUG_LOG(
-          debug_tag,
-          "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^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.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], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
+      CAL_DEBUG_LOG(debug_tag,
+                    "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
+                    "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+                    ", " CAL_FORMAT_6DIGITS_TRIPLET,
+                    (unsigned long int)gyro_cal->debug_calibration_count,
+                    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], 6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
+                    CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
       break;
 
     case GYRO_STATS:
       CAL_DEBUG_LOG(
           debug_tag,
-          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, %s%d.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS_TRIPLET,
           (unsigned long int)gyro_cal->debug_calibration_count,
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
+                           3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
+                           3),
+          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MDEG,
+                           3),
           CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
+              gyro_cal->debug_gyro_cal.gyro_var[0] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3),
           CAL_ENCODE_FLOAT(
-              gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 3),
+              gyro_cal->debug_gyro_cal.gyro_var[1] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3),
           CAL_ENCODE_FLOAT(
-              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,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
-                               RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
-                           3));
+              gyro_cal->debug_gyro_cal.gyro_var[2] * RAD_TO_MDEG * RAD_TO_MDEG,
+              3));
       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.%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], 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));
+        CAL_DEBUG_LOG(
+            debug_tag,
+            "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+            ", " CAL_FORMAT_6DIGITS_TRIPLET,
+            (unsigned long int)gyro_cal->debug_calibration_count,
+            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, "
@@ -1104,73 +1110,12 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
       }
       break;
 
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-    case ACCEL_STATS_TUNING:
-      CAL_DEBUG_LOG(
-          debug_tag,
-          "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|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,
-                           3),
-          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
-                               RAD_TO_MILLI_DEGREES,
-                           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_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.%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");
-      }
-      break;
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
-
     default:
       break;
   }
 }
 
 void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
-  static enum GyroCalDebugState next_state = GYRO_IDLE;
-  static uint64_t wait_timer_nanos = 0;
-
   // This is a state machine that controls the reporting out of debug data.
   switch (gyro_cal->debug_state) {
     case GYRO_IDLE:
@@ -1188,144 +1133,70 @@ void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
 
     case GYRO_WAIT_STATE:
       // This helps throttle the print statements.
-      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
-        gyro_cal->debug_state = next_state;
+      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos,
+                                                   gyro_cal->wait_timer_nanos,
+                                                   GYROCAL_WAIT_TIME_NANOS)) {
+        gyro_cal->debug_state = gyro_cal->next_state;
       }
       break;
 
     case GYRO_PRINT_OFFSET:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_STILLNESS_DATA;   // Sets the next state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_STILLNESS_DATA;  // Sets the next state.
       gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
     case GYRO_PRINT_STILLNESS_DATA:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state =
+          GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;     // First, go to wait state.
       break;
 
     case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                             SAMPLE_RATE_AND_TEMPERATURE);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state =
+          GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN;  // Sets next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;    // First, go to wait state.
       break;
 
     case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
                             GYRO_MINMAX_STILLNESS_MEAN);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_ACCEL_STATS;      // Sets the next state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
       gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
       break;
 
     case GYRO_PRINT_ACCEL_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_STATS;       // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     case GYRO_PRINT_GYRO_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_PRINT_MAG_STATS;        // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     case GYRO_PRINT_MAG_STATS:
       gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      next_state = GYRO_IDLE;                   // Sets the next state.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
-      break;
-
-    default:
-      // Sends this state machine to its idle state.
-      wait_timer_nanos = timestamp_nanos;       // Starts the wait timer.
-      gyro_cal->debug_state = GYRO_WAIT_STATE;  // First, go to wait state.
-  }
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-  if (gyro_cal->debug_state == GYRO_IDLE) {
-    // This check keeps the tuning printout from interleaving with the above
-    // debug print data.
-    gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
-  }
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
-}
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
-                           uint64_t timestamp_nanos) {
-  static enum GyroCalDebugState debug_state = GYRO_IDLE;
-  static enum GyroCalDebugState next_state = GYRO_IDLE;
-  static uint64_t wait_timer_nanos = 0;
-
-  // Output sensor variance levels to assist with tuning thresholds.
-  //   i.  Within the first 300 seconds of boot: output interval = 5
-  //       seconds.
-  //   ii. Thereafter: output interval is 60 seconds.
-  bool condition_i = ((timestamp_nanos <= 300000000000) &&
-                      NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-                          timestamp_nanos, wait_timer_nanos, 5000000000));
-  bool condition_ii = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-      timestamp_nanos, wait_timer_nanos, 60000000000);
-
-  // This is a state machine that controls the reporting out of tuning data.
-  switch (debug_state) {
-    case GYRO_IDLE:
-      // Wait for a trigger and start the data tuning printout sequence.
-      if (condition_i || condition_ii) {
-        CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Temp [C]: %s%d.%03d",
-                      CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3));
-        wait_timer_nanos = timestamp_nanos;   // Starts the wait timer.
-        next_state = GYRO_PRINT_ACCEL_STATS;  // Sets the next state.
-        debug_state = GYRO_WAIT_STATE;        // First, go to wait state.
-      } else {
-        debug_state = GYRO_IDLE;
-      }
-      break;
-
-    case GYRO_WAIT_STATE:
-      // This helps throttle the print statements.
-      if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-              timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
-        debug_state = next_state;
-      }
-      break;
-
-    case GYRO_PRINT_ACCEL_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_PRINT_GYRO_STATS;  // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
-      break;
-
-    case GYRO_PRINT_GYRO_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_PRINT_MAG_STATS;   // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
-      break;
-
-    case GYRO_PRINT_MAG_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS_TUNING);
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      next_state = GYRO_IDLE;              // Sets the next state.
-      debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->next_state = GYRO_IDLE;              // Sets the next state.
+      gyro_cal->debug_state = GYRO_WAIT_STATE;       // First, go to wait state.
       break;
 
     default:
       // Sends this state machine to its idle state.
-      wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
-      debug_state = GYRO_IDLE;
+      gyro_cal->wait_timer_nanos = timestamp_nanos;  // Starts the wait timer.
+      gyro_cal->debug_state = GYRO_IDLE;             // Go to idle state.
   }
 }
-#endif  // GYRO_CAL_DBG_TUNE_ENABLED
 #endif  // GYRO_CAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index cd96676d..5e7d5eec 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
  *       - Temperature   [Celsius]
  *
  * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
  * data to assist in tuning the GyroCal parameters.
  */
 
@@ -80,15 +79,30 @@ struct DebugGyroCal {
   float mag_var[3];
   float gyro_winmean_min[3];
   float gyro_winmean_max[3];
-  float temperature_min_max_celsius[2];  // 0=min; 1=max
+  float temperature_min_celsius;
+  float temperature_max_celsius;
   float temperature_mean_celsius;
   bool using_mag_sensor;
 };
+
+// Data structure for sample rate estimation.
+struct SampleRateData {
+  uint64_t last_timestamp_nanos;
+  uint64_t time_delta_accumulator;
+  size_t num_samples;
+};
 #endif  // GYRO_CAL_DBG_ENABLED
 
+// Data structure for tracking min/max window mean during device stillness.
+struct MinMaxWindowMeanData {
+  float gyro_winmean_min[3];
+  float gyro_winmean_max[3];
+};
+
 // Data structure for tracking temperature data during device stillness.
 struct TemperatureMeanData {
-  float temperature_min_max_celsius[2];
+  float temperature_min_celsius;
+  float temperature_max_celsius;
   float latest_temperature_celsius;
   float mean_accumulator;
   size_t num_points;
@@ -103,6 +117,9 @@ struct GyroCal {
   // Data for tracking temperature mean during periods of device stillness.
   struct TemperatureMeanData temperature_mean_tracker;
 
+  // Data for tracking gyro mean during periods of device stillness.
+  struct MinMaxWindowMeanData window_mean_tracker;
+
   // Aggregated sensor stillness threshold required for gyro bias calibration.
   float stillness_threshold;
 
@@ -166,6 +183,11 @@ struct GyroCal {
   // Debug info.
   struct DebugGyroCal debug_gyro_cal;  // Debug data structure.
   enum GyroCalDebugState debug_state;  // Debug printout state machine.
+  enum GyroCalDebugState next_state;   // Debug state machine next state.
+  uint64_t wait_timer_nanos;           // Debug message throttle timer.
+
+  struct SampleRateData sample_rate_estimator;  // Debug sample rate estimator.
+
   size_t debug_calibration_count;      // Total number of cals performed.
   size_t debug_watchdog_count;         // Total number of watchdog timeouts.
   bool debug_print_trigger;            // Flag used to trigger data printout.
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 254dd7b0..d92f1daa 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -22,7 +22,7 @@
 #include <string.h>
 
 #include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
 #include "util/nano_assert.h"
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,9 +32,17 @@
 
 // Defines the default weighting function for the linear model fit routine.
 // Weighting = 10.0; for offsets newer than 5 minutes.
-#define OTC_WEIGHT_DEFINITION_0  0, 300000000000, 10.0f
+static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+    .offset_age_nanos = MIN_TO_NANOS(5),
+    .weight = 10.0f,
+};
+
 // Weighting = 0.1; for offsets newer than 15 minutes.
-#define OTC_WEIGHT_DEFINITION_1  1, 900000000000, 0.1f
+static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+    .offset_age_nanos = MIN_TO_NANOS(15),
+    .weight = 0.1f,
+};
+
 // The default weighting used for all older offsets.
 #define OTC_MIN_WEIGHT_VALUE  (0.04f)
 
@@ -42,11 +50,11 @@
 // A debug version label to help with tracking results.
 #define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
 
-// The time value used to throttle debug messaging (100msec).
-#define OTC_WAIT_TIME_NANOS (100000000)
+// The time interval used to throttle debug messaging (100msec).
+#define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
 
-// The time value used to throttle temperture print messaging (1 second).
-#define OTC_PRINT_TEMP_NANOS (1000000000)
+// The time interval used to throttle temperture print messaging (1 second).
+#define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
 
 // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
 static const char  kDebugAxisLabel[3] = "XYZ";
@@ -166,80 +174,25 @@ static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
                          size_t axis_index, float temperature_celsius);
 
 // Sets the OTC model parameters to an "initialized" state.
-static void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
-
-  // Sets the temperature sensitivity model parameters to
-  // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
-  // state.
-  over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
-  over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
-  over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
-  memset(over_temp_cal->sensor_intercept, 0, 3 * sizeof(float));
-}
+static void resetOtcLinearModel(struct OverTempCal *over_temp_cal);
 
 // Checks that the input temperature value is within the valid range. If outside
 // of range, then 'temperature_celsius' is coerced to within the limits.
-static bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
-  if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
-    *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
-    return false;
-  }
-  if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
-    *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
-    return false;
-  }
-  return true;
-}
+static bool checkAndEnforceTemperatureRange(float *temperature_celsius);
 
 // Returns "true" if the candidate linear model parameters are within the valid
 // range, and not all zeros.
 static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
-                   float temp_sensitivity, float sensor_intercept) {
-  ASSERT_NOT_NULL(over_temp_cal);
-
-  return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
-         NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
-         NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
-         NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
-}
+                   float temp_sensitivity, float sensor_intercept);
 
 // Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
-static bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
-  ASSERT_NOT_NULL(offset);
-
-  // Simple check to ensure that:
-  //   1. All of the input data is non "zero".
-  //   2. The offset temperature is within the valid range.
-  if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
-      NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
-    return false;
-  }
-
-  // Only returns the "check" result. Don't care about coercion.
-  return checkAndEnforceTemperatureRange(&offset_temp_celsius);
-}
+static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
 
 // Returns the least-squares weight based on the age of a particular offset
 // estimate.
 static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
                                        uint64_t offset_timestamp_nanos,
-                                       uint64_t current_timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  size_t i;
-  for (i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
-    if (current_timestamp_nanos <=
-        offset_timestamp_nanos +
-            over_temp_cal->weighting_function[i].offset_age_nanos) {
-      return over_temp_cal->weighting_function[i].weight;
-    }
-  }
-
-  // Returning the default weight for all older offsets.
-  return OTC_MIN_WEIGHT_VALUE;
-}
+                                       uint64_t current_timestamp_nanos);
 
 // Updates 'compensated_offset' using the linear OTC model.
 static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -292,14 +245,7 @@ static void updateDebugData(struct OverTempCal* over_temp_cal);
 //     new_debug_tag = "INIT]"
 //   Output: "[OVER_TEMP_CAL:INIT]"
 static void createDebugTag(struct OverTempCal *over_temp_cal,
-                           const char *new_debug_tag) {
-  over_temp_cal->otc_debug_tag[0] = '[';
-  memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
-         strlen(over_temp_cal->otc_sensor_tag));
-  memcpy(
-      over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
-      new_debug_tag, strlen(new_debug_tag) + 1);
-}
+                           const char *new_debug_tag);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
 /////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
@@ -342,13 +288,13 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
       OTC_TEMP_INVALID_CELSIUS;
 
   // Defines the default weighting function for the linear model fit routine.
-  overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_0);
-  overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_1);
+  overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
+  overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Sets the default sensor descriptors for debugging.
   overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
-                              1e3f * 180.0f / NANO_PI);
+                              RAD_TO_MDEG);
 
   createDebugTag(over_temp_cal, ":INIT]");
   if (over_temp_cal->over_temp_enable) {
@@ -376,8 +322,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
   // Sets the model parameters if they are within the acceptable limits.
   // Includes a check to reject input model parameters that may have been passed
   // in as all zeros.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                               sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -394,7 +339,8 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
     // Checks that the new offset data is valid.
     if (isValidOtcOffset(offset, offset_temp_celsius)) {
       // Sets the initial over-temp calibration estimate.
-      memcpy(over_temp_cal->model_data[0].offset, offset, 3 * sizeof(float));
+      memcpy(over_temp_cal->model_data[0].offset, offset,
+             sizeof(over_temp_cal->model_data[0].offset));
       over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
       over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
       over_temp_cal->num_model_pts = 1;
@@ -412,7 +358,8 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
   // If the new offset is valid, then it will be used as the current compensated
   // offset, otherwise the current value will be kept.
   if (isValidOtcOffset(offset, offset_temp_celsius)) {
-    memcpy(over_temp_cal->compensated_offset.offset, offset, 3 * sizeof(float));
+    memcpy(over_temp_cal->compensated_offset.offset, offset,
+           sizeof(over_temp_cal->compensated_offset.offset));
     over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
     over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
   }
@@ -430,7 +377,8 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
   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",
+      "Offset|Temp [%s|C]: " CAL_FORMAT_3DIGITS_TRIPLET
+      " | " CAL_FORMAT_3DIGITS,
       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),
@@ -439,8 +387,8 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 
   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",
+      "Sensitivity|Intercept [%s/C|%s]: " CAL_FORMAT_3DIGITS_TRIPLET
+      " | " CAL_FORMAT_3DIGITS_TRIPLET,
       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),
@@ -475,8 +423,10 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
   ASSERT_NOT_NULL(sensor_intercept);
 
   // Gets the latest over-temp calibration model data.
-  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
-  memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
+  memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity,
+         sizeof(over_temp_cal->temp_sensitivity));
+  memcpy(sensor_intercept, over_temp_cal->sensor_intercept,
+         sizeof(over_temp_cal->sensor_intercept));
   *timestamp_nanos = over_temp_cal->last_model_update_nanos;
 
   // Gets the latest temperature compensated offset estimate.
@@ -491,9 +441,8 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
 
   // Load only "good" data from the input 'model_data'.
   over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE);
-  size_t i;
   size_t valid_data_count = 0;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (isValidOtcOffset(model_data[i].offset,
                          model_data[i].offset_temp_celsius)) {
       memcpy(&over_temp_cal->model_data[i], &model_data[i],
@@ -553,7 +502,7 @@ void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
                           float *compensated_offset_temperature_celsius,
                           float *compensated_offset) {
   memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
-         3 * sizeof(float));
+         sizeof(over_temp_cal->compensated_offset.offset));
   *compensated_offset_temperature_celsius =
       over_temp_cal->compensated_offset.offset_temp_celsius;
 }
@@ -638,7 +587,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "Offset|Temperature|Time [%s|C|nsec]: "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+          CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
           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),
@@ -674,8 +623,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
   //          Check condition:
   //          temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check
   bool replaced_one = false;
-  size_t i = 0;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check &&
         over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) {
       // NOTE - The pointer to the new model data point is set here; the offset
@@ -688,7 +636,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
 
   // NOTE - The pointer to the new model data point is set here; the offset
   // data is set below in the call to 'setLatestEstimate'.
-  if (!replaced_one && over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
+  if (!replaced_one) {
     if (over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
       // 3) If nothing was replaced, and the 'model_data' buffer is not full
       //    then add the estimate data to the array.
@@ -699,7 +647,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
       // 4) Otherwise (nothing was replaced and buffer is full), replace the
       //    oldest data with the incoming one.
       over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
-      for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+      for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
         if (over_temp_cal->latest_offset->timestamp_nanos <
             over_temp_cal->model_data[i].timestamp_nanos) {
           over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
@@ -757,7 +705,7 @@ 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.%03d, %llu",
+                  "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
                   CAL_ENCODE_FLOAT(temperature_celsius, 3),
                   (unsigned long long int)timestamp_nanos);
   }
@@ -805,13 +753,11 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
   ASSERT_NOT_NULL(sensor_intercept);
   ASSERT_NOT_NULL(max_error);
 
-  size_t i;
-  size_t j;
   float max_error_test;
   memset(max_error, 0, 3 * sizeof(float));
 
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
-    for (j = 0; j < 3; j++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+    for (size_t j = 0; j < 3; j++) {
       max_error_test =
           NANO_ABS(over_temp_cal->model_data[i].offset[j] -
                    (temp_sensitivity[j] *
@@ -824,16 +770,13 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
   }
 }
 
-// TODO: Refactor to implement a compliance check on the storage of
+// TODO(davejacobs): Refactor to implement a compliance check on the storage of
 // 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
-                                  size_t index,
-                                  uint64_t offset_age_nanos,
-                                  float weight) {
+void overTempSetWeightingFunction(
+    struct OverTempCal *over_temp_cal, size_t index,
+    const struct OverTempCalWeightPt *new_otc_weight) {
   if (index < OTC_NUM_WEIGHT_LEVELS) {
-    over_temp_cal->weighting_function[index].offset_age_nanos =
-        offset_age_nanos;
-    over_temp_cal->weighting_function[index].weight = weight;
+    over_temp_cal->weighting_function[index] = *new_otc_weight;
   }
 }
 
@@ -847,10 +790,9 @@ void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
   // Defaults to using the current compensated offset value.
   float compensated_offset[3];
   memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
-         3 * sizeof(float));
+         sizeof(over_temp_cal->compensated_offset.offset));
 
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined then the default compensation will
       // use the linear model:
@@ -875,8 +817,7 @@ void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
 
   // Adds a delta term to the 'compensated_offset' using the temperature
   // difference defined by 'delta_temp_celsius'.
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined, then use the linear model to assist
       // with computing an extrapolated compensation term.
@@ -894,7 +835,7 @@ void compensateWithEstimate(
 
   // Uses the most recent offset estimate for offset compensation.
   float compensated_offset[3];
-  memcpy(compensated_offset, estimate->offset, 3 * sizeof(float));
+  memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
 
   // Checks that the offset temperature is valid.
   if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
@@ -922,12 +863,11 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
   // The default compensated offset is the nearest-temperature offset vector.
   float compensated_offset[3];
   memcpy(compensated_offset, over_temp_cal->nearest_offset->offset,
-         3 * sizeof(float));
+         sizeof(compensated_offset));
   const float compensated_offset_temperature_celsius =
       over_temp_cal->nearest_offset->offset_temp_celsius;
 
-  size_t index;
-  for (index = 0; index < 3; index++) {
+  for (size_t index = 0; index < 3; index++) {
     if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
       // If a valid axis model is defined, then use the linear model to assist
       // with computing an extrapolated compensation term.
@@ -1099,9 +1039,8 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal,
 
   // If the 'compensated_offset' value has changed significantly, then set
   // 'new_overtemp_offset_available' true.
-  size_t i;
   bool new_overtemp_offset_available = false;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (NANO_ABS(over_temp_cal->compensated_offset.offset[i] -
                  compensated_offset[i]) >=
         over_temp_cal->significant_offset_change) {
@@ -1115,7 +1054,7 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal,
   // vector and timestamp are updated.
   if (new_overtemp_offset_available) {
     memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
-           3 * sizeof(float));
+           sizeof(over_temp_cal->compensated_offset.offset));
     over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
     over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
   }
@@ -1128,7 +1067,8 @@ void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
 
   if (over_temp_cal->latest_offset) {
     // Sets the latest over-temp calibration estimate.
-    memcpy(over_temp_cal->latest_offset->offset, offset, 3 * sizeof(float));
+    memcpy(over_temp_cal->latest_offset->offset, offset,
+           sizeof(over_temp_cal->latest_offset->offset));
     over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
     over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
   }
@@ -1178,9 +1118,8 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
   // set. Otherwise, a lockout condition could occur where the entire model
   // data set would need to be replaced in order to bring the model fit error
   // below the error limit and allow a successful model update.
-  size_t i;
   bool updated_one = false;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
                               sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -1191,7 +1130,8 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
       createDebugTag(over_temp_cal, ":REJECT]");
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
-          "%c-Axis Parameters|Time [%s/C|%s|nsec]: %s%d.%03d, %s%d.%03d, %llu",
+          "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
+          ", " CAL_FORMAT_3DIGITS ", %llu",
           kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
           over_temp_cal->otc_unit_tag,
           CAL_ENCODE_FLOAT(
@@ -1227,11 +1167,10 @@ void findNearestEstimate(struct OverTempCal *over_temp_cal,
 
   // Performs a brute force search for the estimate nearest
   // 'temperature_celsius'.
-  size_t i = 0;
   float dtemp_new = 0.0f;
   float dtemp_old = FLT_MAX;
   over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
                          temperature_celsius);
     if (dtemp_new < dtemp_old) {
@@ -1245,9 +1184,8 @@ void removeStaleModelData(struct OverTempCal *over_temp_cal,
                           uint64_t timestamp_nanos) {
   ASSERT_NOT_NULL(over_temp_cal);
 
-  size_t i;
   bool removed_one = false;
-  for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
     if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
         timestamp_nanos > over_temp_cal->age_limit_nanos +
                               over_temp_cal->model_data[i].timestamp_nanos) {
@@ -1285,8 +1223,8 @@ 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.%03d, %s%d.%03d, %s%d.%03d, "
-      "%s%d.%03d, %llu",
+      "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+      ", " CAL_FORMAT_3DIGITS ", %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,
@@ -1304,8 +1242,7 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
   // Remove the model data at 'model_index'.
-  size_t i;
-  for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
+  for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
     memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
            sizeof(struct OverTempCalDataPt));
   }
@@ -1328,8 +1265,7 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal,
   // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart
   // data produced here requires that the model parameters have all been fully
   // defined and are all within the valid range.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (!isValidOtcLinearModel(over_temp_cal,
                                over_temp_cal->temp_sensitivity[i],
                                over_temp_cal->sensor_intercept[i])) {
@@ -1348,9 +1284,8 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal,
   float offset_temp_celsius =
       (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
 
-  size_t j;
-  for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
-    for (j = 0; j < 3; j++) {
+  for (size_t i = 0; i < over_temp_cal->min_num_model_pts; i++) {
+    for (size_t j = 0; j < 3; j++) {
       over_temp_cal->model_data[i].offset[j] =
           over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
           over_temp_cal->sensor_intercept[j];
@@ -1391,8 +1326,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
 
   // First pass computes the weighted mean values.
   const size_t n = over_temp_cal->num_model_pts;
-  size_t i = 0;
-  for (i = 0; i < n; ++i) {
+  for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
         over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
         timestamp_nanos);
@@ -1407,7 +1341,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   // Second pass computes the mean corrected second moment values.
   ASSERT(sw > 0.0f);
   const float inv_sw = 1.0f / sw;
-  for (i = 0; i < n; ++i) {
+  for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
         over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
         timestamp_nanos);
@@ -1453,9 +1387,87 @@ bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
   return false;
 }
 
+void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  // Sets the temperature sensitivity model parameters to
+  // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
+  // state.
+  over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
+  over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
+  over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
+  memset(over_temp_cal->sensor_intercept, 0,
+         sizeof(over_temp_cal->sensor_intercept));
+}
+
+bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
+  if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
+    *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
+    return false;
+  }
+  if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
+    *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
+    return false;
+  }
+  return true;
+}
+
+bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
+                           float temp_sensitivity, float sensor_intercept) {
+  ASSERT_NOT_NULL(over_temp_cal);
+
+  return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
+         NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
+         NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
+         NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
+}
+
+bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
+  ASSERT_NOT_NULL(offset);
+
+  // Simple check to ensure that:
+  //   1. All of the input data is non "zero".
+  //   2. The offset temperature is within the valid range.
+  if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+      NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
+    return false;
+  }
+
+  // Only returns the "check" result. Don't care about coercion.
+  return checkAndEnforceTemperatureRange(&offset_temp_celsius);
+}
+
+float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
+                                uint64_t offset_timestamp_nanos,
+                                uint64_t current_timestamp_nanos) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
+    if (current_timestamp_nanos <=
+        offset_timestamp_nanos +
+            over_temp_cal->weighting_function[i].offset_age_nanos) {
+      return over_temp_cal->weighting_function[i].weight;
+    }
+  }
+
+  // Returning the default weight for all older offsets.
+  return OTC_MIN_WEIGHT_VALUE;
+}
+
 /////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
+void createDebugTag(struct OverTempCal *over_temp_cal,
+                    const char *new_debug_tag) {
+  over_temp_cal->otc_debug_tag[0] = '[';
+  memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
+         strlen(over_temp_cal->otc_sensor_tag));
+  memcpy(
+      over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
+      new_debug_tag, strlen(new_debug_tag) + 1);
+}
+
 void updateDebugData(struct OverTempCal* over_temp_cal) {
   ASSERT_NOT_NULL(over_temp_cal);
 
@@ -1473,8 +1485,7 @@ void updateDebugData(struct OverTempCal* over_temp_cal) {
   memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
 
   // Copies over the relevant data.
-  size_t i;
-  for (i = 0; i < 3; i++) {
+  for (size_t i = 0; i < 3; i++) {
     if (isValidOtcLinearModel(over_temp_cal, over_temp_cal->temp_sensitivity[i],
                               over_temp_cal->sensor_intercept[i])) {
       over_temp_cal->debug_overtempcal.temp_sensitivity[i] =
@@ -1543,8 +1554,8 @@ 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.%03d, "
-          "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+          "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+          ", " CAL_FORMAT_3DIGITS ", %llu",
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
@@ -1576,7 +1587,7 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       // 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",
+          "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           over_temp_cal->otc_unit_tag,
           (unsigned long int)over_temp_cal->debug_num_estimates,
           CAL_ENCODE_FLOAT(
@@ -1593,7 +1604,7 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
               3));
 
       CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
-                    "Cal#|Intercept [%s]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+                    "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
                     over_temp_cal->otc_unit_tag,
                     (unsigned long int)over_temp_cal->debug_num_estimates,
                     CAL_ENCODE_FLOAT(
@@ -1620,7 +1631,7 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       CAL_DEBUG_LOG(
           over_temp_cal->otc_debug_tag,
           "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
-          "%lu, %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+          "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
           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,
@@ -1647,8 +1658,8 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
       if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
         CAL_DEBUG_LOG(
             over_temp_cal->otc_debug_tag,
-            "  Model[%lu] [%s|C|nsec] = %s%d.%03d, %s%d.%03d, %s%d.%03d, "
-            "%s%d.%03d, %llu",
+            "  Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+            ", " CAL_FORMAT_3DIGITS ", %llu",
             (unsigned long int)over_temp_cal->model_counter,
             over_temp_cal->otc_unit_tag,
             CAL_ENCODE_FLOAT(
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 81b2173f..8a404d32 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -130,17 +130,26 @@ extern "C" {
 #define OTC_NUM_WEIGHT_LEVELS (2)
 
 // Rate-limits the check of old data to every 2 hours.
-#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)
+#define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
 
 // Time duration in which to enforce using the last offset estimate for
 // compensation (30 seconds).
-#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)
+#define OTC_USE_RECENT_OFFSET_TIME_NANOS (SEC_TO_NANOS(30))
 
 // The age at which an offset estimate is considered stale (30 minutes).
-#define OTC_OFFSET_IS_STALE_NANOS (1800000000000)
+#define OTC_OFFSET_IS_STALE_NANOS (MIN_TO_NANOS(30))
 
 // The refresh interval for the OTC model (30 seconds).
-#define OTC_REFRESH_MODEL_NANOS (30000000000)
+#define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
+
+// Defines a weighting function value for the linear model fit routine.
+struct OverTempCalWeightPt {
+  // The age limit below which an offset will use this weight value.
+  uint64_t offset_age_nanos;
+
+  // The weighting applied (>0).
+  float weight;
+};
 
 // Over-temperature sensor offset estimate structure.
 struct OverTempCalDataPt {
@@ -150,15 +159,6 @@ struct OverTempCalDataPt {
   float offset[3];
 };
 
-// Weighting data used to improve the quality of the linear model fit.
-struct OverTempCalWeightPt {
-  // Offset age below which this weight applies.
-  uint64_t offset_age_nanos;
-
-  // Weighting value for offset estimates more recent than 'offset_age_nanos'.
-  float weight;
-};
-
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // Debug printout state enumeration.
 enum OverTempCalDebugState {
@@ -532,14 +532,13 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal,
  * INPUTS:
  *   over_temp_cal:    Over-temp data structure.
  *   index:            Weighting function index.
- *   offset_age_nanos: The age limit below which an offset will use this weight
- *                     value.
- *   weight:           The weighting applied (>0).
+ *   new_otc_weight:   Pointer to the settings for the new non-zero weighting
+ *                     value and corresponding age limit below which an offset
+ *                     will use the weight.
  */
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
-                                  size_t index,
-                                  uint64_t offset_age_nanos,
-                                  float weight);
+void overTempSetWeightingFunction(
+    struct OverTempCal *over_temp_cal, size_t index,
+    const struct OverTempCalWeightPt *new_otc_weight);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index f2e711f7..1bd80fd4 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -54,6 +54,13 @@ extern "C" {
   ((x < 0) ? "-" : ""),                 \
   (int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits))  // NOLINT
 
+// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
+// desired significant digits.
+#define CAL_FORMAT_3DIGITS "%s%d.%03d"
+#define CAL_FORMAT_6DIGITS "%s%d.%06d"
+#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 9c179ac0..66e423fe 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -4,6 +4,7 @@
 #include <stdio.h>
 #include <string.h>
 
+#include "common/math/macros.h"
 #include "common/math/mat.h"
 #include "common/math/vec.h"
 
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
new file mode 100644
index 00000000..5c06e247
--- /dev/null
+++ b/firmware/os/algos/common/math/macros.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This file contains helper macros and definitions.
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+
+// Mathematical constants.
+#define NANO_PI (3.14159265359f)
+
+// Common math operations.
+#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
+#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
+#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+
+// Timestamp conversion macros.
+#ifdef __cplusplus
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#else
+#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000)  // NOLINT
+#endif
+
+#define SEC_TO_NANOS(x)  MSEC_TO_NANOS(x * 1000)
+#define MIN_TO_NANOS(x)  SEC_TO_NANOS(x * 60)
+#define HRS_TO_NANOS(x)  MIN_TO_NANOS(x * 60)
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+
+// Unit conversion: nanoseconds to seconds.
+#define NANOS_TO_SEC (1.0e-9f)
+
+// Unit conversion: milli-degrees to radians.
+#define MDEG_TO_RAD (NANO_PI / 180.0e3f)
+
+// Unit conversion: radians to milli-degrees.
+#define RAD_TO_MDEG (180.0e3f / NANO_PI)
+
+// Time check helper macro that returns true if:
+//    i.  't1' is equal to or exceeds 't2' plus 't_delta'.
+//    ii. Or, a negative timestamp delta occurred since,
+//        't1' should always >= 't2'. This prevents potential lockout conditions
+//        if the timer count 't1' rolls over or an erroneously large
+//        timestamp is passed through.
+#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
+  (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index b55ea8e3..bb9f929c 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,6 +15,7 @@
  */
 
 #include "common/math/vec.h"
+#include "common/math/macros.h"
 
 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
                           float *outY, float *outZ) {
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index 49f197c2..0a4c8b39 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -53,23 +53,6 @@ struct Vec4 {
   float x, y, z, w;
 };
 
-#define NANO_PI (3.14159265359f)
-
-#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
-
-#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
-
-#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
-
-// Time check helper macro that returns true if:
-//    i.  't1' is equal to or exceeds 't2' plus 't_delta'.
-//    ii. Or, a negative timestamp delta occurred since,
-//        't1' should always >= 't2'. This prevents potential lockout conditions
-//        if the timer count 't1' rolls over or an erroneously large
-//        timestamp is passed through.
-#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
-  (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
-
 // 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
   ASSERT_NOT_NULL(v);