Merge "OTC-Gyro Parameter Storage/Recall (Code Sync)" into oc-dev

11 files changed, 571 insertions, 203 deletions

diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index a789ce9d..0a6d96d8 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -17,7 +17,6 @@
 #include "calibration/accelerometer/accel_cal.h"
 #include <errno.h>
 #include <math.h>
-#include <seos.h>
 #include <stdio.h>
 #include <string.h>
 #include "calibration/magnetometer/mag_cal.h"
@@ -46,6 +45,10 @@
   62  // Putting all Temp counts in last bucket for temp > 62 degree C.
 #define HIST_COUNT 9
 #endif
+#ifdef IMU_TEMP_DBG_ENABLED
+#define IMU_TEMP_DELTA_TIME_NANOS \
+  5000000000 // Printing every 5 seconds IMU temp.
+#endif
 
 /////////// Start Debug //////////////////////
 
@@ -90,7 +93,7 @@ static void accelTempHisto(struct AccelStatsMem *adf, float temp) {
   int index = 0;
 
   // Take temp at every stillness detection.
-  adf->start_time = 0;
+  adf->start_time_nanos = 0;
   if (temp <= TEMP_HIST_LOW) {
     adf->t_hist[0] += 1;
     return;
@@ -177,24 +180,28 @@ void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
 
   acc->x_bias = acc->y_bias = acc->z_bias = 0;
   acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
+
+#ifdef IMU_TEMP_DBG_ENABLED
+  acc->temp_time_nanos = 0;
+#endif
 }
 
 // Stillness time check.
 static int stillnessBatchComplete(struct AccelStillDet *asd,
-                                  uint64_t sample_time_nsec) {
+                                  uint64_t sample_time_nanos) {
   int complete = 0;
 
   // Checking if enough data is accumulated to calc Mean and Var.
-  if ((sample_time_nsec - asd->start_time > asd->min_batch_window) &&
+  if ((sample_time_nanos - asd->start_time > asd->min_batch_window) &&
       (asd->nsamples > asd->min_batch_size)) {
-    if (sample_time_nsec - asd->start_time < asd->max_batch_window) {
+    if (sample_time_nanos - asd->start_time < asd->max_batch_window) {
       complete = 1;
     } else {
       // Checking for too long batch window, if yes reset and start over.
       asdReset(asd);
       return complete;
     }
-  } else if (sample_time_nsec - asd->start_time > asd->min_batch_window &&
+  } else if (sample_time_nanos - asd->start_time > asd->min_batch_window &&
              (asd->nsamples < asd->min_batch_size)) {
     // Not enough samples collected in max_batch_window during sample window.
     asdReset(asd);
@@ -207,7 +214,7 @@ void accelCalDestroy(struct AccelCal *acc) { (void)acc; }
 
 // Stillness Detection.
 static int accelStillnessDetection(struct AccelStillDet *asd,
-                                   uint64_t sample_time_nsec, float x, float y,
+                                   uint64_t sample_time_nanos, float x, float y,
                                    float z) {
   float inv = 0.0f;
   int complete = 0.0f;
@@ -223,9 +230,9 @@ static int accelStillnessDetection(struct AccelStillDet *asd,
 
   // Setting a new start time and wait until T0 is reached.
   if (++asd->nsamples == 1) {
-    asd->start_time = sample_time_nsec;
+    asd->start_time = sample_time_nanos;
   }
-  if (stillnessBatchComplete(asd, sample_time_nsec)) {
+  if (stillnessBatchComplete(asd, sample_time_nanos)) {
     // Getting 1/#samples and checking asd->nsamples != 0.
     if (0 < asd->nsamples) {
       inv = 1.0f / asd->nsamples;
@@ -462,19 +469,33 @@ void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z) {
 }
 
 // Accel Cal Runner.
-void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
+void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
                  float y, float z, float temp) {
   // Scaling to 1g, better for the algorithm.
   x *= KSCALE;
   y *= KSCALE;
   z *= KSCALE;
 
+  // DBG: IMU temp messages every 5s.
+#ifdef IMU_TEMP_DBG_ENABLED
+  if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
+    CAL_DEBUG_LOG("IMU Temp Data: ",
+                  ", %s%d.%02d,  %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n",
+                  CAL_ENCODE_FLOAT(temp, 2),
+                  (unsigned long long int)sample_time_nanos,
+                  CAL_ENCODE_FLOAT(acc->x_bias_new,5),
+                  CAL_ENCODE_FLOAT(acc->y_bias_new,5),
+                  CAL_ENCODE_FLOAT(acc->z_bias_new,5));
+    acc->temp_time_nanos = sample_time_nanos;
+    }
+#endif
+
   int temp_gate = 0;
 
   // Temp GATE.
   if (temp < MAX_TEMP && temp > MIN_TEMP) {
     // Checking if accel is still.
-    if (accelStillnessDetection(&acc->asd, sample_time_nsec, x, y, z)) {
+    if (accelStillnessDetection(&acc->asd, sample_time_nanos, x, y, z)) {
 #ifdef ACCEL_CAL_DBG_ENABLED
       // Creating temp hist data.
       accelTempHisto(&acc->adf, temp);
@@ -527,7 +548,7 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
           acc->adf.e_z[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_z;
           acc->adf.var_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.var_t;
           acc->adf.mean_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.mean_t;
-          acc->adf.cal_time[acc->adf.n_o] = sample_time_nsec;
+          acc->adf.cal_time[acc->adf.n_o] = sample_time_nanos;
           acc->adf.rad[acc->adf.n_o] = radius;
           acc->adf.n_o += 1;
 #endif
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 03c65e92..5c63d788 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -95,7 +95,7 @@ struct AccelGoodData {
 struct AccelStatsMem {
   // Temp (in degree C).
   uint32_t t_hist[TEMP_HISTOGRAM];
-  uint64_t start_time;
+  uint64_t start_time_nanos;
 
   // Offset update counter.
   uint32_t noff;
@@ -136,22 +136,27 @@ struct AccelCal {
 #endif
 
   // Offsets are only updated while the accelerometer is not running. Hence need
-  // to store a new offset,
-  // which gets updated during a power down event.
+  // to store a new offset, which gets updated during a power down event.
   float x_bias_new, y_bias_new, z_bias_new;
 
   // Offset values that get subtracted from live data
   float x_bias, y_bias, z_bias;
+
+#ifdef IMU_TEMP_DBG_ENABLED
+  // Temporary time variable used to to print an IMU temperature value with a
+  // lower custom sample rate.
+  uint64_t temp_time_nanos;
+#endif
 };
 
 /* This function runs the accel calibration algorithm.
- * sample_time_nsec -> is the  sensor timestamp in ns and
+ * sample_time_nanos -> is the  sensor timestamp in ns and
  *                     is used to check the stillness time.
  * x,y,z            -> is the sensor data (m/s^2) for the three axes.
  *                     Data is converted to g’s inside the function.
  * temp             -> is the temperature of the IMU (degree C).
  */
-void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x,
+void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
                  float y, float z, float temp);
 
 /* This function initializes the accCalRun data struct.
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index e6bc275f..750ef66d 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -27,10 +27,10 @@
 
 // Maximum gyro bias correction (should be set based on expected max bias
 // of the given sensor).
-#define MAX_GYRO_BIAS (0.096f)  // [rad/sec]
+#define MAX_GYRO_BIAS (0.1f)  // [rad/sec]
 
 // Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
 
 #ifdef GYRO_CAL_DBG_ENABLED
 // The time value used to throttle debug messaging.
@@ -102,7 +102,10 @@ enum DebugPrintData {
   GYRO_MINMAX_STILLNESS_MEAN,
   ACCEL_STATS,
   GYRO_STATS,
-  MAG_STATS
+  MAG_STATS,
+  ACCEL_STATS_TUNING,
+  GYRO_STATS_TUNING,
+  MAG_STATS_TUNING
 };
 
 /*
@@ -579,10 +582,11 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) {
     return;
   }
 
-  // Check for timeout condition of watchdog.
+  // Check for the watchdog timeout condition (i.e., the time elapsed since the
+  // last received sample has exceeded the allowed watchdog duration).
   watchdog_timeout =
-      ((sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos) >
-       gyro_cal->gyro_watchdog_timeout_duration_nanos);
+      (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos +
+                               gyro_cal->gyro_watchdog_start_nanos);
 
   // If a timeout occurred then reset to known good state.
   if (watchdog_timeout) {
@@ -759,9 +763,9 @@ bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
       }
 #ifdef GYRO_CAL_DBG_ENABLED
       if (mean_not_stable) {
-        CAL_DEBUG_LOG(
-            "[GYRO_CAL:MEAN_STABILITY_GATE]",
-            "Exceeded the max variation in the stillness window mean values.");
+        CAL_DEBUG_LOG("[GYRO_CAL:MEAN_STABILITY_GATE]",
+                      "Exceeded the max variation in the gyro's stillness "
+                      "window mean values.");
       }
 #endif  // GYRO_CAL_DBG_ENABLED
       break;
@@ -1034,26 +1038,76 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
 
     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",
-            (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.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));
+        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",
+                      (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));
       } else {
         CAL_DEBUG_LOG(debug_tag,
                       "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
                       "-1.0",
                       (unsigned long int)gyro_cal->debug_calibration_count);
       }
+      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.%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));
       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",
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
+                               RAD_TO_MILLI_DEGREES,
+                           6),
+          CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
+                               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));
+      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));
+      } 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;
   }
@@ -1159,11 +1213,11 @@ void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
   static uint64_t wait_timer_nanos = 0;
 
   // Output sensor variance levels to assist with tuning thresholds.
-  //   i.  Within the first 180 seconds of boot: output interval = 5
+  //   i.  Within the first 300 seconds of boot: output interval = 5
   //       seconds.
   //   ii. Thereafter: output interval is 60 seconds.
   bool condition_i =
-      ((timestamp_nanos <= 180000000000) &&
+      ((timestamp_nanos <= 300000000000) &&
        ((timestamp_nanos - wait_timer_nanos) > 5000000000));  // nsec
   bool condition_ii = ((timestamp_nanos > 60000000000) &&
                        ((timestamp_nanos - wait_timer_nanos) > 60000000000));
@@ -1173,8 +1227,11 @@ void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
     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, "");
-        debug_state = GYRO_PRINT_OFFSET;
+        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;
       }
@@ -1187,29 +1244,22 @@ void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
       }
       break;
 
-    case GYRO_PRINT_OFFSET:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, OFFSET);
-      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.
-      break;
-
     case GYRO_PRINT_ACCEL_STATS:
-      gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, 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);
+      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);
+      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.
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 f1d02b56..9cbcdc4c 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -26,23 +26,18 @@
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
 
-// The 'temp_sensitivity' parameters are set to this value to indicate that the
-// model is in its initial state.
-#define MODEL_INITIAL_STATE (1e6f)
-
 // Rate-limits the search for the nearest offset estimate to every 2 seconds.
 #define OVERTEMPCAL_NEAREST_NANOS (2000000000)
 
 // Rate-limits the check of old data to every 2 hours.
 #define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000)
 
-// A common sensor operating temperature at which to start producing the model
-// jump-start data.
-#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
+// Value used to check whether OTC model parameters are near zero.
+#define OTC_MODELDATA_NEAR_ZERO_TOL (1e-7f)  // [rad/sec]
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // A debug version label to help with tracking results.
-#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
+#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Apr 05, 2017]"
 
 // The time value used to throttle debug messaging.
 #define OVERTEMPCAL_WAIT_TIME_NANOS (300000000)
@@ -51,7 +46,7 @@
 #define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]"
 
 // Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
+#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
 
 // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
 static const char  kDebugAxisLabel[3] = "XYZ";
@@ -59,7 +54,7 @@ static const char  kDebugAxisLabel[3] = "XYZ";
 
 /////// FORWARD DECLARATIONS //////////////////////////////////////////////////
 
-// Updates the most recent model estimate data.
+// Updates the model estimate data nearest to the sensor's temperature.
 static void setNearestEstimate(struct OverTempCal *over_temp_cal,
                               const float *offset, float offset_temp_celsius,
                               uint64_t timestamp_nanos);
@@ -82,6 +77,23 @@ static void computeModelUpdate(struct OverTempCal *over_temp_cal,
 static void findNearestEstimate(struct OverTempCal *over_temp_cal);
 
 /*
+ * Provides the current over-temperature compensated offset vector.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   timestamp_nanos:  The current system timestamp.
+ * OUTPUTS:
+ *   compensated_offset: Temperature compensated offset estimate array.
+ *   compensated_offset_temperature_celsius: Compensated offset temperature.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+static void getCalOffset(struct OverTempCal *over_temp_cal,
+                         uint64_t timestamp_nanos,
+                         float *compensated_offset_temperature_celsius,
+                         float *compensated_offset);
+
+/*
  * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
  * drift-compromised data). Returns 'true' if any data was removed.
  */
@@ -121,19 +133,6 @@ static void updateModel(const struct OverTempCal *over_temp_cal,
                         float *temp_sensitivity, float *sensor_intercept);
 
 /*
- * Removes the over-temp compensated offset from the input sensor data.
- *
- * INPUTS:
- *   over_temp_cal:    Over-temp data structure.
- *   axis_in:          Single axis sensor data to be compensated.
- *   index:            Index for model parameter compensation (0=x, 1=y, 2=z).
- * OUTPUTS:
- *   axis_out:         Single axis sensor data that has been compensated.
- */
-static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
-                               float axis_in, size_t index, float *axis_out);
-
-/*
  * Checks new offset estimates to determine if they could be an outlier that
  * should be rejected. Operates on a per-axis basis determined by 'axis_index'.
  *
@@ -148,6 +147,63 @@ static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
 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));
+}
+
+// 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 > OVERTEMPCAL_TEMP_MAX_CELSIUS) {
+    *temperature_celsius = OVERTEMPCAL_TEMP_MAX_CELSIUS;
+    return false;
+  }
+  if (*temperature_celsius < OVERTEMPCAL_TEMP_MIN_CELSIUS) {
+    *temperature_celsius = OVERTEMPCAL_TEMP_MIN_CELSIUS;
+    return false;
+  }
+  return true;
+}
+
+// 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;
+}
+
+// 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);
+}
+
 #ifdef OVERTEMPCAL_DBG_ENABLED
 // This helper function stores all of the debug tracking information necessary
 // for printing log messages.
@@ -171,11 +227,8 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
   // as the first element in 'model_data'.
   over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
 
-  // Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to
-  // indicate that the model is in an "initial" state.
-  over_temp_cal->temp_sensitivity[0] = MODEL_INITIAL_STATE;
-  over_temp_cal->temp_sensitivity[1] = MODEL_INITIAL_STATE;
-  over_temp_cal->temp_sensitivity[2] = MODEL_INITIAL_STATE;
+  // Initializes the OTC linear model parameters.
+  resetOtcLinearModel(over_temp_cal);
 
   // Initializes the model identification parameters.
   over_temp_cal->new_overtemp_model_available = false;
@@ -188,6 +241,9 @@ void overTempCalInit(struct OverTempCal *over_temp_cal,
   over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
   over_temp_cal->over_temp_enable = over_temp_enable;
 
+  // Initialize the sensor's temperature with a good initial operating point.
+  over_temp_cal->temperature_celsius = JUMPSTART_START_TEMP_CELSIUS;
+
 #ifdef OVERTEMPCAL_DBG_ENABLED
   CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu",
                 (unsigned long int)sizeof(struct OverTempCal));
@@ -211,11 +267,16 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
   ASSERT_NOT_NULL(temp_sensitivity);
   ASSERT_NOT_NULL(sensor_intercept);
 
+  // Initializes the OTC linear model parameters.
+  resetOtcLinearModel(over_temp_cal);
+
   // 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++) {
-    if (NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
-        NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+    if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
+                              sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
       over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
     }
@@ -229,18 +290,48 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
       (jump_start_model) ? jumpStartModelData(over_temp_cal) : false;
 
   if (!model_jump_started) {
-    // Sets the initial over-temp calibration estimate and model data.
-    setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
-                      timestamp_nanos);
-
-    // Now there is one offset estimate in the model.
-    over_temp_cal->num_model_pts = 1;
+    // Checks that the new offset data is valid.
+    if (isValidOtcOffset(offset, offset_temp_celsius)) {
+      // Sets the initial over-temp calibration estimate and model data.
+      over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
+      setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+                         timestamp_nanos);
+      over_temp_cal->num_model_pts = 1;
+    } else {
+      // No valid offset data to load.
+      over_temp_cal->num_model_pts = 0;
+#ifdef OVERTEMPCAL_DBG_ENABLED
+      CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
+                    "No valid sensor offset vector to load.");
+#endif  // OVERTEMPCAL_DBG_ENABLED
+    }
+  } else {
+    // Finds the offset nearest the sensor's current temperature.
+    findNearestEstimate(over_temp_cal);
   }
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
   // Prints the updated model data.
-  CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
-                "Over-temperature model parameters recalled.");
+  CAL_DEBUG_LOG(
+      "[OVER_TEMP_CAL:RECALL]",
+      "Temperature|Offset|Sensitivity|Intercept [rps]: %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));
+
+  // Resets the debug print machine to ensure that updateDebugData() can
+  // produce a debug report and interupt any ongoing report.
+  over_temp_cal->debug_state = OTC_IDLE;
 
   // Triggers a debug print out to view the new model parameters.
   updateDebugData(over_temp_cal);
@@ -258,54 +349,142 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
   ASSERT_NOT_NULL(temp_sensitivity);
   ASSERT_NOT_NULL(sensor_intercept);
 
-  // Gets the over-temp calibration estimate and model data.
-  memcpy(offset, over_temp_cal->nearest_offset->offset, 3 * sizeof(float));
+  // 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));
-  *offset_temp_celsius = over_temp_cal->nearest_offset->offset_temp_celsius;
-  *timestamp_nanos = over_temp_cal->nearest_offset->timestamp_nanos;
+  *timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;
+
+  // Gets the latest temperature compensated offset estimate.
+  getCalOffset(over_temp_cal, *timestamp_nanos, offset_temp_celsius, offset);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+  // Prints the updated model data.
+  CAL_DEBUG_LOG(
+      "[OVER_TEMP_CAL:STORED]",
+      "Temperature|Offset|Sensitivity|Intercept [rps]: %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,
+                             size_t data_length,
+                             const struct OverTempCalDataPt *model_data) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(model_data);
+
+  // Load only "good" data from the input 'model_data'.
+  over_temp_cal->num_model_pts = NANO_MIN(data_length, OVERTEMPCAL_MODEL_SIZE);
+  size_t i;
+  size_t valid_data_count = 0;
+  for (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],
+             sizeof(struct OverTempCalDataPt));
+      valid_data_count++;
+    }
+  }
+  over_temp_cal->num_model_pts = valid_data_count;
+
+  // Initializes the OTC linear model parameters.
+  resetOtcLinearModel(over_temp_cal);
+
+  // Finds the offset nearest the sensor's current temperature.
+  findNearestEstimate(over_temp_cal);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
-  CAL_DEBUG_LOG("[OVER_TEMP_CAL:STORED]",
-                "Over-temperature model parameters stored.");
+  // Prints the updated model data.
+  CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
+                "Over-temperature full model data set recalled.");
+  // Resets the debug print machine to ensure that computeModelUpdate() can
+  // produce a debug report and interupt any ongoing report.
+  over_temp_cal->debug_state = OTC_IDLE;
 #endif  // OVERTEMPCAL_DBG_ENABLED
+
+  // Ensures that minimum number of points required for a model fit has been
+  // satisfied and recomputes the OTC model parameters.
+  if (over_temp_cal->num_model_pts > over_temp_cal->min_num_model_pts) {
+    // Computes and replaces the model parameters. If successful, this will
+    // trigger a "new calibration" update.
+    computeModelUpdate(over_temp_cal,
+                       over_temp_cal->modelupdate_timestamp_nanos);
+  }
+}
+
+void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
+                             size_t *data_length,
+                             struct OverTempCalDataPt *model_data) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  *data_length = over_temp_cal->num_model_pts;
+  memcpy(model_data, over_temp_cal->model_data,
+         over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
+}
+
+bool overTempCalGetOffset(struct OverTempCal *over_temp_cal,
+                          uint64_t timestamp_nanos,
+                          float *compensated_offset_temperature_celsius,
+                          float *compensated_offset) {
+  // Gets the temperature compensated sensor offset estimate.
+  getCalOffset(over_temp_cal, timestamp_nanos,
+               compensated_offset_temperature_celsius, compensated_offset);
+
+  // If the compensated_offset value has changed significantly then return
+  // 'true' status.
+  bool offset_has_changed = false;
+  int i;
+  for (i = 0; i < 3; i++) {
+    if (NANO_ABS(over_temp_cal->compensated_offset_previous[i] -
+                 compensated_offset[i]) >= SIGNIFICANT_OFFSET_CHANGE_RPS) {
+      offset_has_changed = true;
+
+      // Update the 'compensated_offset_previous' vector.
+      memcpy(over_temp_cal->compensated_offset_previous, compensated_offset,
+             3 * sizeof(float));
+      break;
+    }
+  }
+
+  return offset_has_changed;
 }
 
 void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
                              uint64_t timestamp_nanos, float xi, float yi,
                              float zi, float *xo, float *yo, float *zo) {
   ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
   ASSERT_NOT_NULL(xo);
   ASSERT_NOT_NULL(yo);
   ASSERT_NOT_NULL(zo);
 
-  // Removes very old data from the collected model estimates (eliminates
-  // drift-compromised data). Only does this when there is more than one
-  // estimate in the model (i.e., don't want to remove all data, even if it is
-  // very old [something is likely better than nothing]).
-  if ((timestamp_nanos - over_temp_cal->stale_data_timer) >=
-          OVERTEMPCAL_STALE_CHECK_TIME_NANOS &&
-      over_temp_cal->num_model_pts > 1) {
-    over_temp_cal->stale_data_timer = timestamp_nanos;  // Resets timer.
-
-    if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
-      // If anything was removed, then this attempts to recompute the model.
-      if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
-        computeModelUpdate(over_temp_cal, timestamp_nanos);
-      }
-    }
-  }
-
   // Determines whether over-temp compensation will be applied.
-  if (!over_temp_cal->over_temp_enable) {
-    return;
+  if (over_temp_cal->over_temp_enable) {
+    // Gets the temperature compensated sensor offset estimate.
+    float compensated_offset[3] = {0.0f, 0.0f, 0.0f};
+    float compensated_offset_temperature_celsius = 0.0f;
+    getCalOffset(over_temp_cal, timestamp_nanos,
+                 &compensated_offset_temperature_celsius, compensated_offset);
+
+    // Removes the over-temperature compensated offset from the input sensor
+    // data.
+    *xo = xi - compensated_offset[0];
+    *yo = yi - compensated_offset[1];
+    *zo = zi - compensated_offset[2];
+  } else {
+    *xo = xi;
+    *yo = yi;
+    *zo = zi;
   }
-
-  // Removes the over-temperature compensated offset from the input sensor data.
-  removeSensorOffset(over_temp_cal, xi, 0, xo);
-  removeSensorOffset(over_temp_cal, yi, 1, yo);
-  removeSensorOffset(over_temp_cal, zi, 2, zo);
 }
 
 bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
@@ -328,6 +507,11 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
   ASSERT_NOT_NULL(offset);
   ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
+  // Checks that the new offset data is valid, returns if bad.
+  if (!isValidOtcOffset(offset, temperature_celsius)) {
+    return;
+  }
+
   // Prevent a divide by zero below.
   if (over_temp_cal->delta_temp_per_bin <= 0) {
     return;
@@ -354,7 +538,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
                     (unsigned long long int)timestamp_nanos);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
-      return;  // Skips the process of adding this offset to the model.
+      return;  // Outlier detected: skips adding this offset to the model.
     } else {
       // Resets the count of rejected outliers.
       over_temp_cal->num_outliers = 0;
@@ -420,11 +604,6 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
                      timestamp_nanos);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
-  // Updates the latest sensor offset estimate so this can be tracked for debug
-  // printout later.
-  memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
-         over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
-
   // Updates the total number of received sensor offset estimates.
   over_temp_cal->debug_num_estimates++;
 #endif  // OVERTEMPCAL_DBG_ENABLED
@@ -474,10 +653,14 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
 #endif  // OVERTEMPCAL_DBG_LOG_TEMP
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
+  // Checks that the offset temperature is within a valid range, saturates if
+  // outside.
+  checkAndEnforceTemperatureRange(&temperature_celsius);
+
   // Updates the sensor temperature.
   over_temp_cal->temperature_celsius = temperature_celsius;
 
-  // This searches for the sensor offset estimate closest to the current
+  // Searches for the sensor offset estimate closest to the current
   // temperature. A timer is used to limit the rate at which this search is
   // performed.
   if (over_temp_cal->num_model_pts > 0 &&
@@ -488,7 +671,7 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
   }
 }
 
-void getModelError(const struct OverTempCal *over_temp_cal,
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
                    const float *temp_sensitivity, const float *sensor_intercept,
                    float *max_error) {
   ASSERT_NOT_NULL(over_temp_cal);
@@ -517,6 +700,64 @@ void getModelError(const struct OverTempCal *over_temp_cal,
 
 /////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
 
+void getCalOffset(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
+                  float *compensated_offset_temperature_celsius,
+                  float *compensated_offset) {
+  ASSERT_NOT_NULL(over_temp_cal);
+  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
+  ASSERT_NOT_NULL(compensated_offset);
+  ASSERT_NOT_NULL(compensated_offset_temperature_celsius);
+
+  // Sets the sensor temperature associated with the compensated offset.
+  *compensated_offset_temperature_celsius = over_temp_cal->temperature_celsius;
+
+  // Removes very old data from the collected model estimates (eliminates
+  // drift-compromised data). Only does this when there is more than one
+  // estimate in the model (i.e., don't want to remove all data, even if it is
+  // very old [something is likely better than nothing]).
+  if ((timestamp_nanos - over_temp_cal->stale_data_timer) >=
+          OVERTEMPCAL_STALE_CHECK_TIME_NANOS &&
+      over_temp_cal->num_model_pts > 1) {
+    over_temp_cal->stale_data_timer = timestamp_nanos;  // Resets timer.
+
+    if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
+      // If anything was removed, then this attempts to recompute the model.
+      if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
+        computeModelUpdate(over_temp_cal, timestamp_nanos);
+      }
+    }
+  }
+
+  size_t index;
+  for (index = 0; index < 3; index++) {
+    if (over_temp_cal->temp_sensitivity[index] >= OTC_INITIAL_SENSITIVITY ||
+        NANO_ABS(over_temp_cal->temperature_celsius -
+                 over_temp_cal->nearest_offset->offset_temp_celsius) <
+            over_temp_cal->delta_temp_per_bin) {
+      // Use the nearest estimate to perform the compensation if either of the
+      // following is true:
+      //    1) This axis model is in its initial state.
+      //    2) The sensor's temperature is within a small neighborhood of the
+      //       'nearest_offset'.
+      // compensated_offset = nearest_offset
+      //
+      // If either of the above conditions applies and 'nearest_offset' is not
+      // defined, then the offset returned is zero.
+      compensated_offset[index] =
+          (over_temp_cal->num_model_pts > 0)
+              ? over_temp_cal->nearest_offset->offset[index]
+              : 0.0f;
+    } else {
+      // Offset computed from the linear model:
+      //  compensated_offset = (temp_sensitivity * temperature +
+      //  sensor_intercept)
+      compensated_offset[index] = (over_temp_cal->temp_sensitivity[index] *
+                                       over_temp_cal->temperature_celsius +
+                                   over_temp_cal->sensor_intercept[index]);
+    }
+  }
+}
+
 void setNearestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
                        float offset_temp_celsius, uint64_t timestamp_nanos) {
   ASSERT_NOT_NULL(over_temp_cal);
@@ -540,11 +781,12 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
 
   // Computes the maximum error over all of the model data.
   float max_error[3];
-  getModelError(over_temp_cal, temp_sensitivity, sensor_intercept, max_error);
+  overTempGetModelError(over_temp_cal, temp_sensitivity, sensor_intercept,
+                        max_error);
 
   //    3) A new set of model parameters are accepted if:
   //         i.  The model fit error is less than, 'max_error_limit'. See
-  //             getModelError() for error metric description.
+  //             overTempGetModelError() for error metric description.
   //         ii. The model fit parameters must be within certain absolute
   //             bounds:
   //               a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
@@ -553,8 +795,8 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
   bool updated_one = false;
   for (i = 0; i < 3; i++) {
     if (max_error[i] < over_temp_cal->max_error_limit &&
-        NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
-        NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
+        isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
+                              sensor_intercept[i])) {
       over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
       over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
       updated_one = true;
@@ -682,17 +924,19 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
   // In normal operation the offset estimates enter into the 'model_data' array
   // 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 (i.e., no models in an initial state) and are all within the valid
-  // range (this is assumed to have been checked prior to this function). There
-  // must also be no preexisting model data; that is, this function will not
-  // replace any actual offset estimates already buffered.
-  if (over_temp_cal->num_model_pts > 0 ||
-      over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE ||
-      over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE ||
-      over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) {
-    return false;
+  // defined and are all within the valid range.
+  size_t i;
+  for (i = 0; i < 3; i++) {
+    if (!isValidOtcLinearModel(over_temp_cal,
+                               over_temp_cal->temp_sensitivity[i],
+                               over_temp_cal->sensor_intercept[i])) {
+      return false;
+    }
   }
 
+  // Any pre-existing model data points will be overwritten.
+  over_temp_cal->num_model_pts = 0;
+
   // This defines the minimum contiguous set of points to allow a model update
   // when the next offset estimate is received. They are placed at a common
   // temperature range that is likely to get replaced with actual data soon.
@@ -701,7 +945,6 @@ 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 i;
   size_t j;
   for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
     float offset[3];
@@ -718,13 +961,13 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
   }
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
-  if (over_temp_cal->min_num_model_pts > 0) {
+  if (over_temp_cal->num_model_pts > 0) {
     CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Model Jump-Start:  #Points = %lu.",
-                  (unsigned long int)over_temp_cal->min_num_model_pts);
+                  (unsigned long int)over_temp_cal->num_model_pts);
   }
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
-  return (over_temp_cal->min_num_model_pts > 0);
+  return (over_temp_cal->num_model_pts > 0);
 }
 
 void updateModel(const struct OverTempCal *over_temp_cal,
@@ -771,34 +1014,6 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
 }
 
-void removeSensorOffset(const struct OverTempCal *over_temp_cal, float axis_in,
-                        size_t index, float *axis_out) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
-  ASSERT_NOT_NULL(axis_out);
-
-  // Removes the over-temperature compensated offset from the input sensor data.
-  if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE ||
-      NANO_ABS(over_temp_cal->temperature_celsius -
-               over_temp_cal->nearest_offset->offset_temp_celsius) <
-          over_temp_cal->delta_temp_per_bin) {
-    // Use the nearest estimate to perform the compensation if either of the
-    // following is true:
-    //    1) This axis model is in its initial state.
-    //    2) The current temperature is within a small neighborhood of the
-    //       'nearest_offset'.
-    // axis_out = axis_in - nearest_offset
-    *axis_out = axis_in - over_temp_cal->nearest_offset->offset[index];
-  } else {
-    // axis_out = axis_in - compensated_offset
-    // Where,
-    //  compensated_offset = (temp_sensitivity * temperature + sensor_intercept)
-    *axis_out = axis_in - (over_temp_cal->temp_sensitivity[index] *
-                               over_temp_cal->temperature_celsius +
-                           over_temp_cal->sensor_intercept[index]);
-  }
-}
-
 bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
                   size_t axis_index, float temperature_celsius) {
   ASSERT_NOT_NULL(over_temp_cal);
@@ -806,7 +1021,7 @@ bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
 
   // If a model has been defined, then check to see if this offset could be a
   // potential outlier:
-  if (over_temp_cal->temp_sensitivity[axis_index] < MODEL_INITIAL_STATE) {
+  if (over_temp_cal->temp_sensitivity[axis_index] < OTC_INITIAL_SENSITIVITY) {
     const float max_error_test = NANO_ABS(
         offset[axis_index] -
         (over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius +
@@ -841,6 +1056,8 @@ void updateDebugData(struct OverTempCal* over_temp_cal) {
   memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
 
   // Copies over the relevant data.
+  memcpy(over_temp_cal->debug_overtempcal.temp_sensitivity,
+         over_temp_cal->temp_sensitivity, 3 * sizeof(float));
   memcpy(over_temp_cal->debug_overtempcal.sensor_intercept,
          over_temp_cal->sensor_intercept, 3 * sizeof(float));
   memcpy(&over_temp_cal->debug_overtempcal.nearest_offset,
@@ -852,16 +1069,8 @@ void updateDebugData(struct OverTempCal* over_temp_cal) {
   over_temp_cal->debug_overtempcal.temperature_celsius =
       over_temp_cal->temperature_celsius;
 
-  size_t j;
-  for (j = 0; j < 3; j++) {
-    over_temp_cal->debug_overtempcal.temp_sensitivity[j] =
-        (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
-            ? 0.0f
-            : over_temp_cal->temp_sensitivity[j];
-  }
-
   // Computes the maximum error over all of the model data.
-  getModelError(over_temp_cal,
+  overTempGetModelError(over_temp_cal,
                 over_temp_cal->debug_overtempcal.temp_sensitivity,
                 over_temp_cal->debug_overtempcal.sensor_intercept,
                 over_temp_cal->debug_overtempcal.max_error);
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 4ae8952d..3aa3d392 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -68,9 +68,24 @@ extern "C" {
 // Defines the maximum size of the 'model_data' array.
 #define OVERTEMPCAL_MODEL_SIZE (40)
 
+// A common sensor operating temperature at which to start producing the model
+// jump-start data.
+#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
+
 // The maximum number of successive outliers that may be rejected.
 #define OVERTEMPCAL_MAX_OUTLIER_COUNT (3)
 
+// The 'temp_sensitivity' parameters are set to this value to indicate that the
+// model is in its initial state.
+#define OTC_INITIAL_SENSITIVITY (1e6f)
+
+// Minimum "significant" change of offset value.
+#define SIGNIFICANT_OFFSET_CHANGE_RPS (5.23e-5f)  // 3mDPS
+
+// Valid sensor temperature operating range.
+#define OVERTEMPCAL_TEMP_MIN_CELSIUS (-40.0f)
+#define OVERTEMPCAL_TEMP_MAX_CELSIUS (85.0f)
+
 // Over-temperature sensor offset estimate structure.
 struct OverTempCalDataPt {
   // Sensor offset estimate, temperature, and timestamp.
@@ -94,9 +109,6 @@ enum OverTempCalDebugState {
 struct DebugOverTempCal {
   uint64_t modelupdate_timestamp_nanos;
 
-  // The most recent offset estimate received.
-  struct OverTempCalDataPt latest_offset;
-
   // The offset estimate nearest the current sensor temperature.
   struct OverTempCalDataPt nearest_offset;
 
@@ -132,6 +144,9 @@ struct OverTempCal {
   // The temperature at which the offset compensation is performed.
   float temperature_celsius;
 
+  // The stored value of the temperature compensated sensor offset.
+  float compensated_offset_previous[3];
+
   // Pointer to the offset estimate closest to the current sensor temperature.
   struct OverTempCalDataPt *nearest_offset;
 
@@ -150,7 +165,7 @@ struct OverTempCal {
   //            min_update_interval_nanos
   //    3) A new set of model parameters are accepted if:
   //         i.  The model fit error is less than, 'max_error_limit'. See
-  //             getModelError() for error metric description.
+  //             overTempGetModelError() for error metric description.
   //         ii. The model fit parameters must be within certain absolute
   //             bounds:
   //               a. ABS(temp_sensitivity) < temp_sensitivity_limit
@@ -287,6 +302,56 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
                          float *temp_sensitivity, float *sensor_intercept);
 
 /*
+ * Sets the over-temp compensation model data set, and computes new model
+ * parameters provided that 'min_num_model_pts' is satisfied.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp main data structure.
+ *   model_data:       Array of the new model data set.
+ *   data_length:      Number of model data entries in 'model_data'.
+ *
+ * NOTE: Max array length for 'model_data' is OVERTEMPCAL_MODEL_SIZE.
+ */
+void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
+                             size_t data_length,
+                             const struct OverTempCalDataPt *model_data);
+
+/*
+ * Gets the over-temp compensation model data set.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp main data structure.
+ * OUTPUTS:
+ *   model_data:       Array containing the model data set.
+ *   data_length:      Number of model data entries in 'model_data'.
+ *
+ * NOTE: Max array length for 'model_data' is OVERTEMPCAL_MODEL_SIZE.
+ */
+void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
+                             size_t *data_length,
+                             struct OverTempCalDataPt *model_data);
+
+/*
+ * Returns 'true' if the estimated offset has changed by
+ * 'SIGNIFICANT_OFFSET_CHANGE_RPS' and provides the current over-temperature
+ * compensated offset vector. This function is useful for detecting changes in
+ * the offset vector.
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   timestamp_nanos:  The current system timestamp.
+ * OUTPUTS:
+ *   compensated_offset: Temperature compensated offset estimate array.
+ *   compensated_offset_temperature_celsius: Compensated offset temperature.
+ *
+ * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
+ */
+bool overTempCalGetOffset(struct OverTempCal *over_temp_cal,
+                          uint64_t timestamp_nanos,
+                          float *compensated_offset_temperature_celsius,
+                          float *compensated_offset);
+
+/*
  * Removes the over-temp compensated offset from the input sensor data.
  *
  * INPUTS:
@@ -347,9 +412,9 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
  * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
  * NOTE 2: This function is provided for testing purposes.
  */
-void getModelError(const struct OverTempCal *over_temp_cal,
-                   const float *temp_sensitivity, const float *sensor_intercept,
-                   float *max_error);
+void overTempGetModelError(const struct OverTempCal *over_temp_cal,
+                           const float *temp_sensitivity,
+                           const float *sensor_intercept, float *max_error);
 
 #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 64f70f8e..f2e711f7 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -25,16 +25,20 @@
 #ifdef GCC_DEBUG_LOG
 # include <stdio.h>
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
-  printf("%s " fmt "\n", tag, ##__VA_ARGS__);
-#else  // GCC_DEBUG_LOG
+   printf("%s " fmt "\n", tag, ##__VA_ARGS__);
+#elif _OS_BUILD_
 # include <seos.h>
 # ifndef LOG_FUNC
 #  define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__)
 # endif  // LOG_FUNC
 # define LOGD_TAG(tag, fmt, ...) \
-  LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
+   LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
-  osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
+   osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
+#else  // _OS_BUILD_
+# include <chre.h>
+# define CAL_DEBUG_LOG(tag, fmt, ...) \
+   chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
 #endif  // GCC_DEBUG_LOG
 
 #ifdef __cplusplus
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 5ab66254..de47a5c8 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -18,8 +18,17 @@
 
 #include <assert.h>
 #include <float.h>
+
+#ifdef _OS_BUILD_
 #include <nanohub_math.h>
 #include <seos.h>
+#else
+#include <math.h>
+#ifndef UNROLLED
+#define UNROLLED
+#endif
+#endif  // _OS_BUILD_
+
 #include <stddef.h>
 #include <string.h>
 
@@ -620,7 +629,7 @@ bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n)
   int32_t i, j;  // Loops below require signed integers.
   float sum = 0.0f;
   // 1. Solve Ly = b through forward substitution. Use x[] to store y.
-  for (i = 0; i < n; ++i) {
+  for (i = 0; i < (int32_t)n; ++i) {
     sum = 0.0f;
     for (j = 0; j < i; ++j) {
       sum += L[i * n + j] * x[j];
@@ -636,7 +645,7 @@ bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n)
   // y and x.
   for (i = n - 1; i >= 0; --i) {
     sum = 0.0f;
-    for (j = i + 1; j < n; ++j) {
+    for (j = i + 1; j < (int32_t)n; ++j) {
       sum += L[j * n + i] * x[j];
     }
     x[i] = (x[i] - sum) / L[i * n + i];
diff --git a/firmware/os/algos/common/math/quat.c b/firmware/os/algos/common/math/quat.c
index 89727ff9..f7fb3c73 100644
--- a/firmware/os/algos/common/math/quat.c
+++ b/firmware/os/algos/common/math/quat.c
@@ -15,7 +15,6 @@
  */
 
 #include "common/math/quat.h"
-#include <nanohub_math.h>
 
 static float clamp(float x) { return x < 0.0f ? 0.0f : x; }
 
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index 97b2b8cb..62039bba 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,7 +15,6 @@
  */
 
 #include "common/math/vec.h"
-#include <nanohub_math.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 77ab492d..098c6d14 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -32,7 +32,12 @@
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_
 
+#ifdef NANOHUB_NON_CHRE_API
 #include <nanohub_math.h>
+#else
+#include <math.h>
+#endif  // NANOHUB_NON_CHRE_API
+
 #include <stddef.h>
 #include "util/nano_assert.h"
 
@@ -48,13 +53,13 @@ struct Vec4 {
   float x, y, z, w;
 };
 
-#ifndef NANO_ABS
+#define NANO_PI (3.14159265359f)
+
 #define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
-#endif
 
-#ifndef NANO_MAX
 #define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
-#endif
+
+#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
 
 // 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
index c6389c33..e4f1467d 100644
--- a/firmware/os/algos/util/nano_assert.h
+++ b/firmware/os/algos/util/nano_assert.h
@@ -34,11 +34,13 @@
 
 #endif
 
+#ifndef ASSERT
 #ifdef NANO_ASSERT_ENABLED
 #define ASSERT(x) ASSERT_IMPL(x)
 #else
 #define ASSERT(x) ((void)(x))
-#endif
+#endif  // NANO_ASSERT_ENABLED
+#endif  // ASSERT
 
 // Use NULL when compiling for C and nullptr for C++.
 #ifdef __cplusplus