Merge Android 12android-s-v2-preview-2android-s-v2-preview-1android-s-v2-beta-2android-s-v2-preview-1

Bug: 202323961
Merged-In: Id5f58e8cb24bcba19ec2253a9ca5d0703a54b43b
Change-Id: I0aef63344daf978b35157528bd0caac37405d3e8

20 files changed, 360 insertions, 255 deletions

diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index d1716386..c955f3ed 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -16,7 +16,6 @@
 
 #include "calibration/accelerometer/accel_cal.h"
 
-#include <errno.h>
 #include <inttypes.h>
 #include <math.h>
 #include <stdio.h>
@@ -40,7 +39,10 @@
 #define MAX_OFF 0.1f    // Will not accept offsets that are larger than 100 mg.
 #define MIN_TEMP 20.0f  // No Data is collected below 20 degree C.
 #define MAX_TEMP 45.0f  // No Data is collected above 45 degree C.
-#define TEMP_CUT 30     // Separation point for temperature buckets 30 degree C.
+#define TEMP_CUT \
+  ((MAX_TEMP - MIN_TEMP) / \
+   ACCEL_CAL_NUM_TEMP_WINDOWS) // Separation window size for temperature buckets
+                               // in degrees C.
 #define EIGEN_RATIO 0.35f  // EIGEN_RATIO (must be greater than 0.35).
 #define EIGEN_MAG 0.97f    // Eigen value magnitude (must be greater than 0.97).
 #define ACCEL_NEW_BIAS_THRESHOLD (0.0f)  // Bias update detection threshold.
@@ -179,13 +181,14 @@ bool accelCalNewBiasAvailable(struct AccelCal *acc) {
 // Accel cal init.
 void accelCalInit(struct AccelCal *acc,
                   const struct AccelCalParameters *parameters) {
-  // Init core accel data.
-  accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb,
-                   parameters->fy, parameters->fyb, parameters->fz,
-                   parameters->fzb, parameters->fle);
-  accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb,
-                   parameters->fy, parameters->fyb, parameters->fz,
-                   parameters->fzb, parameters->fle);
+  int i;
+
+  for (i = 0; i < ACCEL_CAL_NUM_TEMP_WINDOWS; ++i) {
+    // Init core accel data.
+    accelCalAlgoInit(&acc->ac1[i], parameters->fx, parameters->fxb,
+                     parameters->fy, parameters->fyb, parameters->fz,
+                     parameters->fzb, parameters->fle);
+  }
 
   // Stillness Reset.
   accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
@@ -483,12 +486,7 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
       accelTempHisto(&acc->adf, temp);
 #endif
 
-      // Two temp buckets.
-      if (temp < TEMP_CUT) {
-        temp_gate = 0;
-      } else {
-        temp_gate = 1;
-      }
+      temp_gate = (int) ((temp - MIN_TEMP) / TEMP_CUT);
 #ifdef ACCEL_CAL_DBG_ENABLED
       accelStatsCounter(&acc->asd, &acc->adf);
 #endif
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 3324875d..ad7de9e7 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -27,6 +27,7 @@
 
 #include <stdint.h>
 #include <sys/types.h>
+
 #include "common/math/kasa.h"
 #include "common/math/mat.h"
 
diff --git a/firmware/os/algos/calibration/diversity_checker/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
index 3fab81f8..d9de89ab 100644
--- a/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
@@ -16,17 +16,17 @@
 
 #include "calibration/diversity_checker/diversity_checker.h"
 
-#include <errno.h>
 #include <stdarg.h>
 #include <stdio.h>
 #include <string.h>
 
 #include "common/math/vec.h"
+#include "chre/util/nanoapp/assert.h"
 
 // Struct initialization.
 void diversityCheckerInit(struct DiversityChecker* diverse_data,
                           const struct DiversityCheckerParameters* parameters) {
-  ASSERT_NOT_NULL(diverse_data);
+  CHRE_ASSERT_NOT_NULL(diverse_data);
 
   // Initialize parameters.
   diverse_data->threshold_tuning_param_sq =
@@ -61,7 +61,7 @@ void diversityCheckerInit(struct DiversityChecker* diverse_data,
 
 // Reset
 void diversityCheckerReset(struct DiversityChecker* diverse_data) {
-  ASSERT_NOT_NULL(diverse_data);
+  CHRE_ASSERT_NOT_NULL(diverse_data);
   // Clear data memory.
   memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data));
 
@@ -109,7 +109,7 @@ bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
 
 void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
                             float y, float z) {
-  ASSERT_NOT_NULL(diverse_data);
+  CHRE_ASSERT_NOT_NULL(diverse_data);
 
   // If memory is full, no need to run through the data.
   if (!diverse_data->data_full) {
@@ -138,7 +138,7 @@ void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
 
 bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
                                  float x_bias, float y_bias, float z_bias) {
-  ASSERT_NOT_NULL(diverse_data);
+  CHRE_ASSERT_NOT_NULL(diverse_data);
   // If not enough diverse data points or max distance violations return false.
   if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
       diverse_data->num_max_dist_violations >=
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
index fda5170a..1948df32 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -16,7 +16,6 @@
 
 #include "calibration/magnetometer/mag_cal/mag_cal.h"
 
-#include <errno.h>
 #include <inttypes.h>
 #include <string.h>
 
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index a8daaeb8..122af489 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -19,7 +19,7 @@
 #include <cstdint>
 #include <cstring>
 
-#include "chre/util/nanoapp/log.h"
+#include "common/techeng_log_util.h"
 
 namespace nano_calibration {
 namespace {
@@ -35,16 +35,15 @@ constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
 // messages will be produced at a rate determined by
 // 'slow_message_interval_min'.
 struct LogMessageRegimen {
-  uint8_t rapid_message_interval_sec;   // Assists device verification.
-  uint8_t slow_message_interval_min;    // Avoids long-term log spam.
+  uint8_t rapid_message_interval_sec;  // Assists device verification.
+  uint8_t slow_message_interval_min;   // Avoids long-term log spam.
   uint8_t duration_of_rapid_messages_min;
 };
 
 constexpr LogMessageRegimen kGyroscopeMessagePlan = {
     /*rapid_message_interval_sec*/ 20,
     /*slow_message_interval_min*/ 5,
-    /*duration_of_rapid_messages_min*/ 3
-};
+    /*duration_of_rapid_messages_min*/ 3};
 
 using ::online_calibration::CalibrationDataThreeAxis;
 using ::online_calibration::CalibrationTypeFlags;
@@ -58,18 +57,22 @@ using ::online_calibration::SensorType;
 #endif
 
 #ifdef NANO_SENSOR_CAL_DBG_ENABLED
-#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
-#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__)
-#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGD(tag, format, ...) \
+  TECHENG_LOGD("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) \
+  TECHENG_LOGW("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) \
+  TECHENG_LOGE("%s " format, tag, ##__VA_ARGS__)
 #else
-#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
-#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
-#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGD(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__)
 #endif  // NANO_SENSOR_CAL_DBG_ENABLED
 
 // NOTE: LOGI is defined to ensure calibration updates are always logged for
 // field diagnosis and verification.
-#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) \
+  TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__)
 
 }  // namespace
 
@@ -199,6 +202,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) {
                            accel_cal_update_flags_, kAccelTag);
       PrintCalibration(accel_cal_->GetSensorCalibration(),
                        accel_cal_update_flags_, kAccelTag);
+
+      if (result_callback_ != nullptr) {
+        result_callback_->SetCalibrationEvent(sample.timestamp_nanos,
+                                              SensorType::kAccelerometerMps2,
+                                              accel_cal_update_flags_);
+      }
     }
   }
 
@@ -210,7 +219,19 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) {
       if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
                                gyro_cal_->GetSensorCalibration(),
                                gyro_cal_update_flags_, kGyroTag)) {
-        HandleGyroLogMessage(sample.timestamp_nanos);
+        const bool print_gyro_log =
+            HandleGyroLogMessage(sample.timestamp_nanos);
+
+        if (result_callback_ != nullptr &&
+            (print_gyro_log ||
+             gyro_cal_update_flags_ != CalibrationTypeFlags::BIAS)) {
+          // Rate-limits OTC gyro telemetry updates since they can happen
+          // frequently with temperature change. However, all GyroCal stillness
+          // and OTC model parameter updates will be recorded.
+          result_callback_->SetCalibrationEvent(sample.timestamp_nanos,
+                                                SensorType::kGyroscopeRps,
+                                                gyro_cal_update_flags_);
+        }
       }
     }
   }
@@ -224,6 +245,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) {
                            mag_cal_update_flags_, kMagTag);
       PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_,
                        kMagTag);
+
+      if (result_callback_ != nullptr) {
+        result_callback_->SetCalibrationEvent(sample.timestamp_nanos,
+                                              SensorType::kMagnetometerUt,
+                                              mag_cal_update_flags_);
+      }
     }
   }
 }
@@ -294,7 +321,7 @@ bool NanoSensorCal::NotifyAshCalibration(
 
 bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
                                        OnlineCalibrationThreeAxis *online_cal,
-                                       CalibrationTypeFlags* flags,
+                                       CalibrationTypeFlags *flags,
                                        const char *sensor_tag) {
   ashCalParams recalled_ash_cal_parameters;
   if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
@@ -443,7 +470,7 @@ void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
   }
 }
 
-void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) {
+bool NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) {
   // Limits the log messaging update rate for the gyro calibrations since
   // these can occur frequently with rapid temperature changes.
   const int64_t next_log_interval_nanos =
@@ -454,14 +481,15 @@ void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) {
           : SEC_TO_NANOS(kGyroscopeMessagePlan.rapid_message_interval_sec);
 
   const bool print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-        timestamp_nanos, gyro_notification_time_nanos_,
-        next_log_interval_nanos);
+      timestamp_nanos, gyro_notification_time_nanos_, next_log_interval_nanos);
 
   if (print_gyro_log) {
     gyro_notification_time_nanos_ = timestamp_nanos;
     PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_,
                      kGyroTag);
   }
+
+  return print_gyro_log;
 }
 
 }  // namespace nano_calibration
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
index 5dad0f68..82a8396d 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -42,16 +42,17 @@
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
 
+#include <ash.h>
+#include <chre.h>
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <ash.h>
-#include <chre.h>
 #include <cstdint>
 
 #include "calibration/online_calibration/common_data/calibration_callback.h"
 #include "calibration/online_calibration/common_data/calibration_data.h"
 #include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/result_callback_interface.h"
 #include "calibration/online_calibration/common_data/sensor_data.h"
 #include "common/math/macros.h"
 
@@ -88,6 +89,11 @@ class NanoSensorCal {
   void HandleTemperatureSamples(uint16_t event_type,
                                 const chreSensorFloatData *event_data);
 
+  void set_result_callback(
+      online_calibration::ResultCallbackInterface *result_callback) {
+    result_callback_ = result_callback;
+  }
+
  private:
   // Passes sensor data to the runtime calibration algorithms.
   void ProcessSample(const online_calibration::SensorData &sample);
@@ -99,7 +105,7 @@ class NanoSensorCal {
   // which runtime calibration parameters were recalled.
   bool LoadAshCalibration(uint8_t chreSensorType,
                           OnlineCalibrationThreeAxis *online_cal,
-                          online_calibration::CalibrationTypeFlags* flags,
+                          online_calibration::CalibrationTypeFlags *flags,
                           const char *sensor_tag);
 
   // Provides sensor calibration updates using the ASH API for the specified
@@ -126,7 +132,7 @@ class NanoSensorCal {
       const online_calibration::CalibrationDataThreeAxis &cal_data,
       online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
 
-  void HandleGyroLogMessage(uint64_t timestamp_nanos);
+  bool HandleGyroLogMessage(uint64_t timestamp_nanos);
 
   // Pointer to the accelerometer runtime calibration object.
   OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
@@ -154,6 +160,9 @@ class NanoSensorCal {
       online_calibration::CalibrationTypeFlags::NONE;
   online_calibration::CalibrationTypeFlags mag_cal_update_flags_ =
       online_calibration::CalibrationTypeFlags::NONE;
+
+  // Pointer to telemetry logger.
+  online_calibration::ResultCallbackInterface *result_callback_ = nullptr;
 };
 
 }  // namespace nano_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
index 7d66f756..f21108e2 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
@@ -48,6 +48,12 @@ namespace online_calibration {
  *                    behavior with temperature (e.g., linear bias sensitivity
  *                    model).
  * QUALITY_DEGRADED - Indicates a degradation in calibration quality.
+ * OTC_STILL_BIAS   - Indicates that a stillness-induced bias update occurred as
+ *                    an input to the over-temperature compensation algorithm
+ *                    NOTE: Stillness bias values (e.g., GyroCal) may be
+ *                    different from the OTC bias. If these bias value are
+ *                    desired, they should be retrieved directly (see related
+ *                    calibration wrappers for access [e.g., GyroOffsetOtcCal]).
  */
 enum class CalibrationTypeFlags : uint8_t {
   NONE = 0x00,
@@ -56,6 +62,7 @@ enum class CalibrationTypeFlags : uint8_t {
   CROSS_AXIS = 0x04,
   OVER_TEMP = 0x08,
   QUALITY_DEGRADED = 0x10,
+  OTC_STILL_BIAS = 0x20,
   ALL = 0xFF
 };
 
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h
new file mode 100644
index 00000000..ca54f2fb
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h
@@ -0,0 +1,32 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_
+
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+// Interface for a results callback implementation (useful for building
+// calibration event loggers).
+class ResultCallbackInterface {
+ protected:
+  // Protected destructor. The implementation can destroy itself, it can't be
+  // destroyed through this interface.
+  virtual ~ResultCallbackInterface() = default;
+
+ public:
+  // Sets a calibration event, such as a magnetometer calibration event.
+  //
+  // event_timestamp_nanos: Timestamp in nanoseconds of when the calibration
+  //                        event was produced in the sensor timebase.
+  // sensor_type: Which sensor the calibration was produced for.
+  // flags: What kind of update the calibration was, e.g. offset, quality
+  //        degradation (like a magnetization event), over temperature, etc.
+  virtual void SetCalibrationEvent(uint64_t event_timestamp_nanos,
+                                   SensorType sensor_type,
+                                   CalibrationTypeFlags flags) = 0;
+};
+
+}  // namespace online_calibration
+
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
index d73ed3b4..02c26709 100644
--- a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
@@ -71,6 +71,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
   }
 
   // Checks for a new calibration, and updates the OTC.
+  CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE;
   if (gyroCalNewBiasAvailable(&gyro_cal_)) {
     float offset[3];
     float temperature_celsius = kInvalidTemperatureCelsius;
@@ -79,6 +80,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
                    &temperature_celsius, &calibration_time_nanos);
     overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
                                     offset, temperature_celsius);
+    cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS;
   }
 
   // Checks the OTC for a new calibration model update.
@@ -89,7 +91,6 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
   const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_);
 
   // Sets the new calibration data.
-  CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE;
   if (new_otc_offset) {
     overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius,
                          cal_data_.offset);
@@ -111,7 +112,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
 
   // Sets the new calibration quality, polling flag, and notifies a calibration
   // callback listener of the new update.
-  if (new_otc_model_update || new_otc_offset) {
+  if (cal_update_callback_flags != CalibrationTypeFlags::NONE) {
     cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
     cal_data_.calibration_quality.value = kHighQualityRps;
     cal_update_polling_flags_ |= cal_update_callback_flags;
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 6f887f78..782dce93 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -25,7 +25,7 @@
 #include "calibration/util/cal_log.h"
 #endif  // OVERTEMPCAL_DBG_ENABLED || OVERTEMPCAL_DBG_LOG_TEMP
 
-#include "util/nano_assert.h"
+#include "chre/util/nanoapp/assert.h"
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
 
@@ -256,7 +256,7 @@ static void createDebugTag(struct OverTempCal *over_temp_cal,
 
 void overTempCalInit(struct OverTempCal *over_temp_cal,
                      const struct OverTempCalParameters *parameters) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // Clears OverTempCal memory.
   memset(over_temp_cal, 0, sizeof(struct OverTempCal));
@@ -312,10 +312,10 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
                          float offset_temp_celsius, uint64_t timestamp_nanos,
                          const float *temp_sensitivity,
                          const float *sensor_intercept, bool jump_start_model) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(offset);
-  ASSERT_NOT_NULL(temp_sensitivity);
-  ASSERT_NOT_NULL(sensor_intercept);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT_NOT_NULL(temp_sensitivity);
+  CHRE_ASSERT_NOT_NULL(sensor_intercept);
 
   // Initializes the OTC linear model parameters.
   resetOtcLinearModel(over_temp_cal);
@@ -415,12 +415,12 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
 void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
                          float *offset_temp_celsius, uint64_t *timestamp_nanos,
                          float *temp_sensitivity, float *sensor_intercept) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(offset);
-  ASSERT_NOT_NULL(offset_temp_celsius);
-  ASSERT_NOT_NULL(timestamp_nanos);
-  ASSERT_NOT_NULL(temp_sensitivity);
-  ASSERT_NOT_NULL(sensor_intercept);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT_NOT_NULL(offset_temp_celsius);
+  CHRE_ASSERT_NOT_NULL(timestamp_nanos);
+  CHRE_ASSERT_NOT_NULL(temp_sensitivity);
+  CHRE_ASSERT_NOT_NULL(sensor_intercept);
 
   // Gets the latest over-temp calibration model data.
   memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity,
@@ -436,8 +436,8 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
 void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
                              size_t data_length, uint64_t timestamp_nanos,
                              const struct OverTempModelThreeAxis *model_data) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(model_data);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(model_data);
 
   // Load only "good" data from the input 'model_data'.
   over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE);
@@ -488,7 +488,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
 void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
                              size_t *data_length,
                              struct OverTempModelThreeAxis *model_data) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_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 OverTempModelThreeAxis));
@@ -506,10 +506,10 @@ void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
 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(xo);
-  ASSERT_NOT_NULL(yo);
-  ASSERT_NOT_NULL(zo);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(xo);
+  CHRE_ASSERT_NOT_NULL(yo);
+  CHRE_ASSERT_NOT_NULL(zo);
 
   // Determines whether over-temp compensation will be applied.
   if (over_temp_cal->over_temp_enable) {
@@ -526,7 +526,7 @@ void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
 }
 
 bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
   const bool update_available = over_temp_cal->new_overtemp_model_available &&
                                 over_temp_cal->over_temp_enable;
 
@@ -537,7 +537,7 @@ bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) {
 }
 
 bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
   const bool update_available = over_temp_cal->new_overtemp_offset_available &&
                                 over_temp_cal->over_temp_enable;
 
@@ -551,9 +551,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
                                      uint64_t timestamp_nanos,
                                      const float *offset,
                                      float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(offset);
-  ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
   // Updates the age of each OTC model data point.
   modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
@@ -687,7 +687,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
 void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
                                uint64_t timestamp_nanos,
                                float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
 #ifdef OVERTEMPCAL_DBG_ENABLED
 #ifdef OVERTEMPCAL_DBG_LOG_TEMP
@@ -752,8 +752,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
 bool overTempValidateAndSetWeight(
     struct OverTempCal *over_temp_cal, size_t index,
     const struct OverTempCalWeight *new_otc_weight) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(new_otc_weight);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(new_otc_weight);
 
   // The input weighting coefficient must be positive.
   if (new_otc_weight->weight <= 0.0f) {
@@ -784,7 +784,7 @@ bool overTempValidateAndSetWeight(
 void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
                                uint64_t timestamp_nanos,
                                float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // Defaults to using the current compensated offset value.
   float compensated_offset[3];
@@ -811,8 +811,8 @@ void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
 void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
                                        float *compensated_offset,
                                        float delta_temp_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(compensated_offset);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(compensated_offset);
 
   // Adds a delta term to the 'compensated_offset' using the temperature
   // difference defined by 'delta_temp_celsius'.
@@ -830,8 +830,8 @@ void compensateWithEstimate(struct OverTempCal *over_temp_cal,
                             uint64_t timestamp_nanos,
                             struct OverTempModelThreeAxis *estimate,
                             float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(estimate);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(estimate);
 
   // Uses the most recent offset estimate for offset compensation.
   float compensated_offset[3];
@@ -857,8 +857,8 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
                                      uint64_t timestamp_nanos,
                                      float temperature_celsius,
                                      bool compare_to_linear_model) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
 
   // The default compensated offset is the nearest-temperature offset vector.
   float compensated_offset[3];
@@ -914,7 +914,7 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
 
 void updateCalOffset(struct OverTempCal *over_temp_cal,
                      uint64_t timestamp_nanos, float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // If 'temperature_celsius' is invalid, then no changes to the compensated
   // offset are computed.
@@ -1050,8 +1050,8 @@ void updateCalOffset(struct OverTempCal *over_temp_cal,
 void setCompensatedOffset(struct OverTempCal *over_temp_cal,
                           const float *compensated_offset,
                           uint64_t timestamp_nanos, float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(compensated_offset);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(compensated_offset);
 
   // If the 'compensated_offset' value has changed significantly, then set
   // 'new_overtemp_offset_available' true.
@@ -1077,8 +1077,8 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal,
 
 void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
                        float offset_temp_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(offset);
 
   if (over_temp_cal->latest_offset != NULL) {
     // Sets the latest over-temp calibration estimate.
@@ -1091,7 +1091,7 @@ void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
 
 void refreshOtcModel(struct OverTempCal *over_temp_cal,
                      uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
   if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
           timestamp_nanos, over_temp_cal->last_model_update_nanos,
           OTC_REFRESH_MODEL_NANOS)) {
@@ -1109,7 +1109,7 @@ void refreshOtcModel(struct OverTempCal *over_temp_cal,
 
 void computeModelUpdate(struct OverTempCal *over_temp_cal,
                         uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // Ensures that the minimum number of points required for a model fit has been
   // satisfied.
@@ -1171,7 +1171,7 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal,
 
 void findNearestEstimate(struct OverTempCal *over_temp_cal,
                          float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // If 'temperature_celsius' is invalid, then do not search.
   if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
@@ -1195,7 +1195,7 @@ void findNearestEstimate(struct OverTempCal *over_temp_cal,
 
 void removeStaleModelData(struct OverTempCal *over_temp_cal,
                           uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   bool removed_one = false;
   for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
@@ -1223,7 +1223,7 @@ void removeStaleModelData(struct OverTempCal *over_temp_cal,
 
 bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
                             size_t model_index) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // This function will not remove all of the model data. At least one model
   // sample will be left.
@@ -1263,8 +1263,8 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal,
 }
 
 bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT(over_temp_cal->delta_temp_per_bin > 0);
 
   // Prevent a divide by zero below.
   if (over_temp_cal->delta_temp_per_bin <= 0) {
@@ -1321,10 +1321,10 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
 
 void updateModel(const struct OverTempCal *over_temp_cal,
                  float *temp_sensitivity, float *sensor_intercept) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(temp_sensitivity);
-  ASSERT_NOT_NULL(sensor_intercept);
-  ASSERT(over_temp_cal->num_model_pts > 0);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(temp_sensitivity);
+  CHRE_ASSERT_NOT_NULL(sensor_intercept);
+  CHRE_ASSERT(over_temp_cal->num_model_pts > 0);
 
   float sw = 0.0f;
   float st = 0.0f, stt = 0.0f;
@@ -1347,7 +1347,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   }
 
   // Second pass computes the mean corrected second moment values.
-  ASSERT(sw > 0.0f);
+  CHRE_ASSERT(sw > 0.0f);
   const float inv_sw = 1.0f / sw;
   for (size_t i = 0; i < n; ++i) {
     weight = evaluateWeightingFunction(
@@ -1362,7 +1362,7 @@ void updateModel(const struct OverTempCal *over_temp_cal,
   }
 
   // Calculates the linear model fit parameters.
-  ASSERT(stt > 0.0f);
+  CHRE_ASSERT(stt > 0.0f);
   const float inv_stt = 1.0f / stt;
   temp_sensitivity[0] = stsx * inv_stt;
   sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_sw;
@@ -1374,8 +1374,8 @@ void updateModel(const struct OverTempCal *over_temp_cal,
 
 bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
                   size_t axis_index, float temperature_celsius) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(offset);
 
   // If a model has been defined, then check to see if this offset could be a
   // potential outlier:
@@ -1394,7 +1394,7 @@ bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
 }
 
 void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_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"
@@ -1420,7 +1420,7 @@ bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
 
 bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
                            float temp_sensitivity, float sensor_intercept) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // Simple check to ensure that the linear model parameters are:
   //   1. Within the valid range, AND
@@ -1432,7 +1432,7 @@ bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
 }
 
 bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
-  ASSERT_NOT_NULL(offset);
+  CHRE_ASSERT_NOT_NULL(offset);
 
   // Simple check to ensure that:
   //   1. All of the input data is non "zero".
@@ -1450,7 +1450,7 @@ bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
 
 float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
                                 uint64_t offset_age_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
   for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
     if (offset_age_nanos <=
         over_temp_cal->weighting_function[i].offset_age_nanos) {
@@ -1464,7 +1464,7 @@ float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
 
 void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
                            uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
   if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) {
     // Age updates must be monotonic.
     return;
@@ -1496,7 +1496,7 @@ void createDebugTag(struct OverTempCal *over_temp_cal,
 }
 
 void updateDebugData(struct OverTempCal *over_temp_cal) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // Only update this data if debug printing is not currently in progress
   // (i.e., don't want to risk overwriting debug information that is actively
@@ -1550,7 +1550,7 @@ void updateDebugData(struct OverTempCal *over_temp_cal) {
 
 void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
                            uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
 
   // This is a state machine that controls the reporting out of debug data.
   createDebugTag(over_temp_cal, ":REPORT]");
@@ -1739,9 +1739,9 @@ void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal,
                                  const char *otc_sensor_tag,
                                  const char *otc_unit_tag,
                                  float otc_unit_conversion) {
-  ASSERT_NOT_NULL(over_temp_cal);
-  ASSERT_NOT_NULL(otc_sensor_tag);
-  ASSERT_NOT_NULL(otc_unit_tag);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(otc_sensor_tag);
+  CHRE_ASSERT_NOT_NULL(otc_unit_tag);
 
   // Sets the sensor descriptor, displayed units, and unit conversion factor.
   strcpy(over_temp_cal->otc_sensor_tag, otc_sensor_tag);
@@ -1752,10 +1752,10 @@ void overTempCalDebugDescriptors(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);
-  ASSERT_NOT_NULL(temp_sensitivity);
-  ASSERT_NOT_NULL(sensor_intercept);
-  ASSERT_NOT_NULL(max_error);
+  CHRE_ASSERT_NOT_NULL(over_temp_cal);
+  CHRE_ASSERT_NOT_NULL(temp_sensitivity);
+  CHRE_ASSERT_NOT_NULL(sensor_intercept);
+  CHRE_ASSERT_NOT_NULL(max_error);
 
   float max_error_test;
   memset(max_error, 0, 3 * sizeof(float));
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
index d1b66195..5ab7dd83 100644
--- a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
@@ -19,7 +19,7 @@
 #include <string.h>
 
 #include "common/math/macros.h"
-#include "util/nano_assert.h"
+#include "chre/util/nanoapp/assert.h"
 
 // Helper function used to reset the sampling rate estimator accumulator.
 static void sampleRateEstimatorResetAccumulator(
@@ -32,7 +32,7 @@ static void sampleRateEstimatorResetAccumulator(
 void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
                              size_t num_intervals_to_collect,
                              float max_interval_sec) {
-  ASSERT_NOT_NULL(sample_rate_estimator);
+  CHRE_ASSERT_NOT_NULL(sample_rate_estimator);
   memset(sample_rate_estimator, 0, sizeof(struct SampleRateEstimator));
   sample_rate_estimator->mean_sampling_rate_estimate_hz =
       SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ;
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
index f506eea5..5d965dd6 100644
--- a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
@@ -43,6 +43,7 @@
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
 
 #include <stdbool.h>
+#include <stddef.h>
 #include <stdint.h>
 #include <sys/types.h>
 
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
index 8729e0dd..4b3eb4aa 100644
--- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -27,6 +27,7 @@
 
 #include "common/math/mat.h"
 #include "common/math/vec.h"
+#include "chre/util/nanoapp/assert.h"
 
 // FORWARD DECLARATIONS
 ///////////////////////////////////////////////////////////////////////////////
@@ -45,8 +46,8 @@ static bool runCalibration(struct SphereFitCal *sphere_cal,
 void sphereFitInit(struct SphereFitCal *sphere_cal,
                    const struct LmParams *lm_params,
                    const size_t min_num_points_for_cal) {
-  ASSERT_NOT_NULL(sphere_cal);
-  ASSERT_NOT_NULL(lm_params);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(lm_params);
 
   // Initialize LM solver.
   lmSolverInit(&sphere_cal->lm_solver, lm_params,
@@ -64,7 +65,7 @@ void sphereFitInit(struct SphereFitCal *sphere_cal,
 }
 
 void sphereFitReset(struct SphereFitCal *sphere_cal) {
-  ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
 
   // Set state to default (diagonal scale matrix and zero offset).
   memset(&sphere_cal->x0[0], 0, sizeof(float) * SF_STATE_DIM);
@@ -79,8 +80,8 @@ void sphereFitReset(struct SphereFitCal *sphere_cal) {
 
 void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
                             struct LmData *lm_data) {
-  ASSERT_NOT_NULL(sphere_cal);
-  ASSERT_NOT_NULL(lm_data);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(lm_data);
 
   // Set solver data.
   lmSolverSetData(&sphere_cal->lm_solver, lm_data);
@@ -89,8 +90,8 @@ void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
 bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
                      const struct SphereFitData *data,
                      uint64_t timestamp_nanos) {
-  ASSERT_NOT_NULL(sphere_cal);
-  ASSERT_NOT_NULL(data);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(data);
 
   // Run calibration if have enough points.
   if (data->num_fit_points >= sphere_cal->min_points_for_cal) {
@@ -102,7 +103,7 @@ bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
 
 void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
                              const float initial_bias[THREE_AXIS_DIM]) {
-  ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
   sphere_cal->x0[eParamOffset1] = initial_bias[0];
   sphere_cal->x0[eParamOffset2] = initial_bias[1];
   sphere_cal->x0[eParamOffset3] = initial_bias[2];
@@ -110,23 +111,23 @@ void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
 
 void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
                            struct ThreeAxisCalData *cal_data) {
-  ASSERT_NOT_NULL(sphere_cal);
-  ASSERT_NOT_NULL(cal_data);
+  CHRE_ASSERT_NOT_NULL(sphere_cal);
+  CHRE_ASSERT_NOT_NULL(cal_data);
   convertStateToCalStruct(sphere_cal->x, cal_data);
   cal_data->calibration_time_nanos = sphere_cal->estimate_time_nanos;
 }
 
 void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
                                    float *residual, float *jacobian) {
-  ASSERT_NOT_NULL(state);
-  ASSERT_NOT_NULL(f_data);
-  ASSERT_NOT_NULL(residual);
+  CHRE_ASSERT_NOT_NULL(state);
+  CHRE_ASSERT_NOT_NULL(f_data);
+  CHRE_ASSERT_NOT_NULL(residual);
 
   const struct SphereFitData *data = (const struct SphereFitData *)f_data;
 
   // Verify that expected norm is non-zero, else use default of 1.0.
   float expected_norm = 1.0;
-  ASSERT(data->expected_norm > MIN_VALID_DATA_NORM);
+  CHRE_ASSERT(data->expected_norm > MIN_VALID_DATA_NORM);
   if (data->expected_norm > MIN_VALID_DATA_NORM) {
     expected_norm = data->expected_norm;
   }
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 57179a7e..2f13151c 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -42,6 +42,10 @@
 # include <chre.h>
 # define CAL_DEBUG_LOG(tag, fmt, ...) \
    chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
+#elif ROHAN_DEBUG_LOG
+# include "caraway/logging.h"
+# define CAL_DEBUG_LOG(tag, format, ...) \
+   LOG_DEBUG("%s " format, tag, ##__VA_ARGS__)
 #else
 // CHRE/SLPI Nanoapp Logging.
 # include "chre/util/nanoapp/log.h"
diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c
index a24a31bf..911afba2 100644
--- a/firmware/os/algos/common/math/kasa.c
+++ b/firmware/os/algos/common/math/kasa.c
@@ -6,6 +6,7 @@
 #include "common/math/mat.h"
 
 void kasaReset(struct KasaFit *kasa) {
+  kasa->acc_mean_x = kasa->acc_mean_y = kasa->acc_mean_z = 0.0f;
   kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
   kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
   kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
@@ -16,6 +17,21 @@ void kasaReset(struct KasaFit *kasa) {
 void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
 
 void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
+  // KASA fit runs into numerical accuracy issues for large offset and small
+  // radii. Assuming that all points are on an sphere we can substract the
+  // first x,y,z value from all incoming data, making sure that the sphere will
+  // always go through 0,0,0 ensuring the highest possible numerical accuracy.
+  if (kasa->nsamples == 0) {
+    kasa->acc_mean_x = x;
+    kasa->acc_mean_y = y;
+    kasa->acc_mean_z = z;
+  }
+
+  x = x - kasa->acc_mean_x;
+  y = y - kasa->acc_mean_y;
+  z = z - kasa->acc_mean_z;
+
+  // Accumulation.
   float w = x * x + y * y + z * z;
 
   kasa->acc_x += x;
@@ -108,7 +124,10 @@ int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
   float r_square = vec3Dot(&v, &v) - out.w;
   float r = (r_square > 0) ? sqrtf(r_square) : 0;
 
-  initVec3(bias, v.x, v.y, v.z);
+  // Need to correct the bias with the first sample, which was used to shift
+  // the sphere in order to have best accuracy.
+  initVec3(bias, v.x + kasa->acc_mean_x, v.y + kasa->acc_mean_y,
+           v.z + kasa->acc_mean_z);
   *radius = r;
 
   int success = 0;
diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h
index e9652d60..d3504b6f 100644
--- a/firmware/os/algos/common/math/kasa.h
+++ b/firmware/os/algos/common/math/kasa.h
@@ -20,6 +20,7 @@ extern "C" {
 #endif
 
 struct KasaFit {
+  float acc_mean_x, acc_mean_y, acc_mean_z;
   float acc_x, acc_y, acc_z, acc_w;
   float acc_xx, acc_xy, acc_xz, acc_xw;
   float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 89986808..2957253e 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -7,6 +7,7 @@
 #include "common/math/macros.h"
 #include "common/math/mat.h"
 #include "common/math/vec.h"
+#include "chre/util/nanoapp/assert.h"
 
 // FORWARD DECLARATIONS
 ////////////////////////////////////////////////////////////////////////
@@ -30,9 +31,9 @@ const static float kEps = 1e-10f;
 ////////////////////////////////////////////////////////////////////////
 void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
                   ResidualAndJacobianFunction func) {
-  ASSERT_NOT_NULL(solver);
-  ASSERT_NOT_NULL(params);
-  ASSERT_NOT_NULL(func);
+  CHRE_ASSERT_NOT_NULL(solver);
+  CHRE_ASSERT_NOT_NULL(params);
+  CHRE_ASSERT_NOT_NULL(func);
   memset(solver, 0, sizeof(struct LmSolver));
   memcpy(&solver->params, params, sizeof(struct LmParams));
   solver->func = func;
@@ -40,8 +41,8 @@ void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
 }
 
 void lmSolverSetData(struct LmSolver *solver, struct LmData *data) {
-  ASSERT_NOT_NULL(solver);
-  ASSERT_NOT_NULL(data);
+  CHRE_ASSERT_NOT_NULL(solver);
+  CHRE_ASSERT_NOT_NULL(data);
   solver->data = data;
 }
 
@@ -59,10 +60,10 @@ enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
 
   // Check pointers (note that f_data can be null if no additional data is
   // required by the error function).
-  ASSERT_NOT_NULL(solver);
-  ASSERT_NOT_NULL(initial_state);
-  ASSERT_NOT_NULL(state);
-  ASSERT_NOT_NULL(solver->data);
+  CHRE_ASSERT_NOT_NULL(solver);
+  CHRE_ASSERT_NOT_NULL(initial_state);
+  CHRE_ASSERT_NOT_NULL(state);
+  CHRE_ASSERT_NOT_NULL(solver->data);
 
   // Allocate memory for intermediate variables.
   float state_new[MAX_LM_STATE_DIMENSION];
@@ -215,10 +216,10 @@ bool computeResidualAndGradients(ResidualAndJacobianFunction func,
                                  float *residual, float *gradient,
                                  float *hessian) {
   // Compute residual and Jacobian.
-  ASSERT_NOT_NULL(state);
-  ASSERT_NOT_NULL(residual);
-  ASSERT_NOT_NULL(gradient);
-  ASSERT_NOT_NULL(hessian);
+  CHRE_ASSERT_NOT_NULL(state);
+  CHRE_ASSERT_NOT_NULL(residual);
+  CHRE_ASSERT_NOT_NULL(gradient);
+  CHRE_ASSERT_NOT_NULL(hessian);
   func(state, f_data, residual, jacobian);
 
   // Compute the cost function hessian = jacobian' jacobian and
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 8b62cceb..12037968 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -16,9 +16,10 @@
 
 #include "common/math/mat.h"
 
-#include <assert.h>
 #include <float.h>
 
+#include "chre/util/nanoapp/assert.h"
+
 #ifdef _OS_BUILD_
 #include <nanohub_math.h>
 #include <seos.h>
@@ -44,13 +45,13 @@ static void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k,
 static void mat44SwapRows(struct Mat44 *A, uint32_t i, uint32_t j);
 
 void initZeroMatrix(struct Mat33 *A) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   memset(A->elem, 0.0f, sizeof(A->elem));
 }
 
 UNROLLED
 void initDiagonalMatrix(struct Mat33 *A, float x) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   initZeroMatrix(A);
 
   uint32_t i;
@@ -61,10 +62,10 @@ void initDiagonalMatrix(struct Mat33 *A, float x) {
 
 void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
                        const struct Vec3 *v2, const struct Vec3 *v3) {
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(v1);
-  ASSERT_NOT_NULL(v2);
-  ASSERT_NOT_NULL(v3);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(v1);
+  CHRE_ASSERT_NOT_NULL(v2);
+  CHRE_ASSERT_NOT_NULL(v3);
   A->elem[0][0] = v1->x;
   A->elem[0][1] = v2->x;
   A->elem[0][2] = v3->x;
@@ -79,9 +80,9 @@ void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1,
 }
 
 void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(v);
   out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z;
   out->y = A->elem[1][0] * v->x + A->elem[1][1] * v->y + A->elem[1][2] * v->z;
   out->z = A->elem[2][0] * v->x + A->elem[2][1] * v->y + A->elem[2][2] * v->z;
@@ -90,11 +91,11 @@ void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) {
 UNROLLED
 void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
                    const struct Mat33 *B) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(B);
-  ASSERT(out != A);
-  ASSERT(out != B);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(B);
+  CHRE_ASSERT(out != A);
+  CHRE_ASSERT(out != B);
 
   uint32_t i;
   for (i = 0; i < 3; ++i) {
@@ -113,7 +114,7 @@ void mat33Multiply(struct Mat33 *out, const struct Mat33 *A,
 
 UNROLLED
 void mat33ScalarMul(struct Mat33 *A, float c) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -125,8 +126,8 @@ void mat33ScalarMul(struct Mat33 *A, float c) {
 
 UNROLLED
 void mat33Add(struct Mat33 *out, const struct Mat33 *A) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -138,8 +139,8 @@ void mat33Add(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 void mat33Sub(struct Mat33 *out, const struct Mat33 *A) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -151,7 +152,7 @@ void mat33Sub(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     if (A->elem[i][i] < 0.0f) {
@@ -174,11 +175,11 @@ int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) {
 UNROLLED
 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
                              const struct Mat33 *B) {
-  ASSERT(out != A);
-  ASSERT(out != B);
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(B);
+  CHRE_ASSERT(out != A);
+  CHRE_ASSERT(out != B);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(B);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -197,11 +198,11 @@ void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
 UNROLLED
 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
                               const struct Mat33 *B) {
-  ASSERT(out != A);
-  ASSERT(out != B);
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(B);
+  CHRE_ASSERT(out != A);
+  CHRE_ASSERT(out != B);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(B);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -219,8 +220,8 @@ void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,
 
 UNROLLED
 void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
   float t;
   initDiagonalMatrix(out, 1.0f);
 
@@ -248,7 +249,7 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
       }
     }
     // divide by zero guard.
-    ASSERT(fabsf(tmp.elem[i][i]) > 0);
+    CHRE_ASSERT(fabsf(tmp.elem[i][i]) > 0);
     if(!(fabsf(tmp.elem[i][i]) > 0)) {
       return;
     }
@@ -272,8 +273,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
   uint32_t i;
   for (i = 0; i < 3; ++i) {
     uint32_t j;
@@ -285,7 +286,7 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) {
 
 UNROLLED
 void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   const uint32_t N = 3;
   uint32_t k;
 
@@ -303,9 +304,9 @@ void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) {
 UNROLLED
 void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
                         struct Mat33 *eigenvecs) {
-  ASSERT_NOT_NULL(S);
-  ASSERT_NOT_NULL(eigenvals);
-  ASSERT_NOT_NULL(eigenvecs);
+  CHRE_ASSERT_NOT_NULL(S);
+  CHRE_ASSERT_NOT_NULL(eigenvals);
+  CHRE_ASSERT_NOT_NULL(eigenvecs);
   const uint32_t N = 3;
   uint32_t i, j, k, l, m;
 
@@ -408,7 +409,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
 }
 
 float mat33Determinant(const struct Mat33 *A) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   return A->elem[0][0] *
       (A->elem[1][1] * A->elem[2][2] - A->elem[1][2] * A->elem[2][1])
       - A->elem[0][1] *
@@ -420,7 +421,7 @@ float mat33Determinant(const struct Mat33 *A) {
 // index of largest off-diagonal element in row k
 UNROLLED
 uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   const uint32_t N = 3;
 
   uint32_t m = k + 1;
@@ -437,16 +438,16 @@ uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
 
 void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l,
                  uint32_t i, uint32_t j) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   float tmp = c * A->elem[k][l] - s * A->elem[i][j];
   A->elem[i][j] = s * A->elem[k][l] + c * A->elem[i][j];
   A->elem[k][l] = tmp;
 }
 
 void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(v);
 
   out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z +
            A->elem[0][3] * v->w;
@@ -463,8 +464,8 @@ void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) {
 
 UNROLLED
 void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) {
-  ASSERT_NOT_NULL(LU);
-  ASSERT_NOT_NULL(pivot);
+  CHRE_ASSERT_NOT_NULL(LU);
+  CHRE_ASSERT_NOT_NULL(pivot);
   const uint32_t N = 4;
   uint32_t i, j, k;
 
@@ -500,7 +501,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) {
 
 UNROLLED
 void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) {
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(A);
   const uint32_t N = 4;
   uint32_t k;
 
@@ -518,10 +519,10 @@ void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) {
 UNROLLED
 void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
                 const struct Size4 *pivot) {
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(x);
-  ASSERT_NOT_NULL(b);
-  ASSERT_NOT_NULL(pivot);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(x);
+  CHRE_ASSERT_NOT_NULL(b);
+  CHRE_ASSERT_NOT_NULL(pivot);
   const uint32_t N = 4;
   uint32_t i, k;
 
@@ -556,8 +557,8 @@ void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b,
 }
 
 float matMaxDiagonalElement(const float *square_mat, size_t n) {
-  ASSERT_NOT_NULL(square_mat);
-  ASSERT(n > 0);
+  CHRE_ASSERT_NOT_NULL(square_mat);
+  CHRE_ASSERT(n > 0);
   size_t i;
   float max = square_mat[0];
   const size_t n_square = n * n;
@@ -571,7 +572,7 @@ float matMaxDiagonalElement(const float *square_mat, size_t n) {
 }
 
 void matAddConstantDiagonal(float *square_mat, float u, size_t n) {
-  ASSERT_NOT_NULL(square_mat);
+  CHRE_ASSERT_NOT_NULL(square_mat);
   size_t i;
   const size_t n_square = n * n;
   const size_t offset = n + 1;
@@ -582,8 +583,8 @@ void matAddConstantDiagonal(float *square_mat, float u, size_t n) {
 
 void matTransposeMultiplyMat(float *out, const float *A,
                              size_t nrows, size_t ncols) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
   size_t i;
   size_t j;
   size_t k;
@@ -608,9 +609,9 @@ void matTransposeMultiplyMat(float *out, const float *A,
 
 void matMultiplyVec(float *out, const float *A, const float *v,
                     size_t nrows, size_t ncols) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(v);
   size_t i;
   for (i = 0; i < nrows; ++i) {
     const float *row = &A[i * ncols];
@@ -620,9 +621,9 @@ void matMultiplyVec(float *out, const float *A, const float *v,
 
 void matTransposeMultiplyVec(float *out, const float *A, const float *v,
                              size_t nrows, size_t ncols) {
-  ASSERT_NOT_NULL(out);
-  ASSERT_NOT_NULL(A);
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(out);
+  CHRE_ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(v);
   size_t i, j;
   for (i = 0; i < ncols; ++i) {
     out[i] = 0;
@@ -633,10 +634,10 @@ void matTransposeMultiplyVec(float *out, const float *A, const float *v,
 }
 
 bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n) {
-  ASSERT_NOT_NULL(x);
-  ASSERT_NOT_NULL(L);
-  ASSERT_NOT_NULL(b);
-  ASSERT(n <= INT32_MAX);
+  CHRE_ASSERT_NOT_NULL(x);
+  CHRE_ASSERT_NOT_NULL(L);
+  CHRE_ASSERT_NOT_NULL(b);
+  CHRE_ASSERT(n <= INT32_MAX);
   int32_t i, j;  // Loops below require signed integers.
   int32_t s_n = (int32_t)n; // Signed n.
   float sum = 0.0f;
@@ -667,8 +668,8 @@ bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n)
 }
 
 bool matCholeskyDecomposition(float *L, const float *A, size_t n) {
-  ASSERT_NOT_NULL(L);
-  ASSERT_NOT_NULL(A);
+  CHRE_ASSERT_NOT_NULL(L);
+  CHRE_ASSERT_NOT_NULL(A);
   size_t i, j, k;
   float sum = 0.0f;
   // initialize L to zero.
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index bb9f929c..794bb413 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,13 +15,14 @@
  */
 
 #include "common/math/vec.h"
+
 #include "common/math/macros.h"
 
 void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
                           float *outY, float *outZ) {
-  ASSERT_NOT_NULL(outX);
-  ASSERT_NOT_NULL(outY);
-  ASSERT_NOT_NULL(outZ);
+  CHRE_ASSERT_NOT_NULL(outX);
+  CHRE_ASSERT_NOT_NULL(outY);
+  CHRE_ASSERT_NOT_NULL(outZ);
   float x, y, z;
 
   // discard the one with the smallest absolute value
@@ -40,7 +41,7 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
   }
 
   float magSquared = x * x + y * y + z * z;
-  ASSERT(magSquared > 0);
+  CHRE_ASSERT(magSquared > 0);
   // Only set invMag if magSquared is non-zero.
   float invMag = 1.0f;
   if (magSquared > 0) {
@@ -52,9 +53,9 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
 }
 
 void vecAdd(float *u, const float *v, const float *w, size_t dim) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   size_t i;
   for (i = 0; i < dim; i++) {
     u[i] = v[i] + w[i];
@@ -62,8 +63,8 @@ void vecAdd(float *u, const float *v, const float *w, size_t dim) {
 }
 
 void vecAddInPlace(float *v, const float *w, size_t dim) {
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   size_t i;
   for (i = 0; i < dim; i++) {
     v[i] += w[i];
@@ -71,9 +72,9 @@ void vecAddInPlace(float *v, const float *w, size_t dim) {
 }
 
 void vecSub(float *u, const float *v, const float *w, size_t dim) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   size_t i;
   for (i = 0; i < dim; i++) {
     u[i] = v[i] - w[i];
@@ -81,8 +82,8 @@ void vecSub(float *u, const float *v, const float *w, size_t dim) {
 }
 
 void vecScalarMul(float *u, const float *v, float c, size_t dim) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
   size_t i;
   for (i = 0; i < dim; i++) {
     u[i] = c * v[i];
@@ -90,7 +91,7 @@ void vecScalarMul(float *u, const float *v, float c, size_t dim) {
 }
 
 void vecScalarMulInPlace(float *v, float c, size_t dim) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   size_t i;
   for (i = 0; i < dim; i++) {
     v[i] *= c;
@@ -98,19 +99,19 @@ void vecScalarMulInPlace(float *v, float c, size_t dim) {
 }
 
 float vecNorm(const float *v, size_t dim) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   float norm_sq = vecNormSquared(v, dim);
   return sqrtf(norm_sq);
 }
 
 float vecNormSquared(const float *v, size_t dim) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   return vecDot(v, v, dim);
 }
 
 float vecDot(const float *v, const float *w, size_t dim) {
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   size_t i;
   float result = 0;
   for (i = 0; i < dim; ++i) {
@@ -120,7 +121,7 @@ float vecDot(const float *v, const float *w, size_t dim) {
 }
 
 float vecMaxAbsoluteValue(const float *v, size_t dim) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   float max = NANO_ABS(v[0]);
   float tmp;
   size_t i;
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index e839ad53..8dbf0eb4 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -39,7 +39,8 @@
 #endif  // NANOHUB_NON_CHRE_API
 
 #include <stddef.h>
-#include "util/nano_assert.h"
+
+#include "chre/util/nanoapp/assert.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -55,7 +56,7 @@ struct Vec4 {
 
 // 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
 static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   v->x = x;
   v->y = y;
   v->z = z;
@@ -63,8 +64,8 @@ static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
 
 // Updates v as the sum of v and w.
 static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   v->x += w->x;
   v->y += w->y;
   v->z += w->z;
@@ -73,9 +74,9 @@ static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) {
 // Sets u as the sum of v and w.
 static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v,
                                const struct Vec3 *w) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   u->x = v->x + w->x;
   u->y = v->y + w->y;
   u->z = v->z + w->z;
@@ -83,8 +84,8 @@ static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v,
 
 // Updates v as the subtraction of w from v.
 static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   v->x -= w->x;
   v->y -= w->y;
   v->z -= w->z;
@@ -93,9 +94,9 @@ static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
 // Sets u as the difference of v and w.
 static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v,
                                const struct Vec3 *w) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   u->x = v->x - w->x;
   u->y = v->y - w->y;
   u->z = v->z - w->z;
@@ -103,7 +104,7 @@ static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v,
 
 // Scales v by the scalar c, i.e. v = c * v.
 static inline void vec3ScalarMul(struct Vec3 *v, float c) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   v->x *= c;
   v->y *= c;
   v->z *= c;
@@ -111,29 +112,29 @@ static inline void vec3ScalarMul(struct Vec3 *v, float c) {
 
 // Returns the dot product of v and w.
 static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) {
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   return v->x * w->x + v->y * w->y + v->z * w->z;
 }
 
 // Returns the square of the L2-norm of the given vector.
 static inline float vec3NormSquared(const struct Vec3 *v) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   return vec3Dot(v, v);
 }
 
 // Returns the L2-norm of the given vector.
 static inline float vec3Norm(const struct Vec3 *v) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   return sqrtf(vec3NormSquared(v));
 }
 
 // Normalizes the provided vector to unit norm. If the provided vector has a
 // norm of zero, the vector will be unchanged.
 static inline void vec3Normalize(struct Vec3 *v) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   float norm = vec3Norm(v);
-  ASSERT(norm > 0);
+  CHRE_ASSERT(norm > 0);
   // Only normalize if norm is non-zero.
   if (norm > 0) {
     float invNorm = 1.0f / norm;
@@ -146,9 +147,9 @@ static inline void vec3Normalize(struct Vec3 *v) {
 // Updates u as the cross product of v and w.
 static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v,
                              const struct Vec3 *w) {
-  ASSERT_NOT_NULL(u);
-  ASSERT_NOT_NULL(v);
-  ASSERT_NOT_NULL(w);
+  CHRE_ASSERT_NOT_NULL(u);
+  CHRE_ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(w);
   u->x = v->y * w->z - v->z * w->y;
   u->y = v->z * w->x - v->x * w->z;
   u->z = v->x * w->y - v->y * w->x;
@@ -167,7 +168,7 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
 // Initialize the Vec4 structure with the provided component values.
 static inline void initVec4(struct Vec4 *v, float x, float y, float z,
                             float w) {
-  ASSERT_NOT_NULL(v);
+  CHRE_ASSERT_NOT_NULL(v);
   v->x = x;
   v->y = y;
   v->z = z;