[ImuCal] Code Sync

 - Google3 code sync.
 - Reduces gyroscope log message rate.

Tip of G3 CL: 250346261
Bug: 131185374
Test: Built and verified on hardware.

Change-Id: Ie740912b03ba72f28b29fa0e919e71ade415b398

26 files changed, 170 insertions, 234 deletions

diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 99e96ef7..d1716386 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -22,7 +22,9 @@
 #include <stdio.h>
 #include <string.h>
 
+#if defined(ACCEL_CAL_DBG_ENABLED) || defined(IMU_TEMP_DBG_ENABLED)
 #include "calibration/util/cal_log.h"
+#endif  // ACCEL_CAL_DBG_ENABLED || IMU_TEMP_DBG_ENABLED
 
 // clang-format off
 #define KSCALE \
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 90b25446..adc9efc8 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -21,7 +21,10 @@
 #include <math.h>
 #include <string.h>
 
+#ifdef GYRO_CAL_DBG_ENABLED
 #include "calibration/util/cal_log.h"
+#endif  // GYRO_CAL_DBG_ENABLED
+
 #include "common/math/macros.h"
 
 /////// DEFINITIONS AND MACROS ///////////////////////////////////////
@@ -195,10 +198,6 @@ void gyroCalInit(struct GyroCal* gyro_cal,
 #endif  // GYRO_CAL_DBG_ENABLED
 }
 
-// Void pointer in the gyro calibration data structure (doesn't do anything
-// except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
-
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
                     float* bias_z, float* temperature_celsius,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 1f17254c..7b5c1d33 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -213,9 +213,6 @@ struct GyroCal {
 void gyroCalInit(struct GyroCal* gyro_cal,
                  const struct GyroCalParameters* parameters);
 
-// Void all pointers in the gyro calibration data structure.
-void gyroCalDestroy(struct GyroCal* gyro_cal);
-
 // Get the most recent bias calibration value.
 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
                     float* bias_z, float* temperature_celsius,
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 c3f12ae6..fda5170a 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -20,7 +20,9 @@
 #include <inttypes.h>
 #include <string.h>
 
+#ifdef MAG_CAL_DEBUG_ENABLE
 #include "calibration/util/cal_log.h"
+#endif  // MAG_CAL_DEBUG_ENABLE
 
 // Local helper macro for printing log messages.
 #ifdef MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
index 93c2ac61..0a6f799f 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
@@ -60,8 +60,6 @@ void initMagCalSphere(
   magCalSphereOdrUpdate(mocs, default_odr_in_hz);
 }
 
-void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; }
-
 void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz) {
   // Calculate the numbers of samples to be dropped, in order to fill up
   // the data set.
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
index 3ae4687b..0d7ffa63 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
@@ -57,8 +57,6 @@ void initMagCalSphere(
     const struct DiversityCheckerParameters *diverse_parameters,
     float default_odr_in_hz);
 
-void magCalSphereDestroy(struct MagCalSphere *mocs);
-
 enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
                                   uint64_t sample_time_us, float x, float y,
                                   float z);
diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
deleted file mode 100644
index 12b84701..00000000
--- a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * Copyright (C) 2018 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
-
-#ifdef ACCEL_CAL_ENABLED
-#include "calibration/accelerometer/accel_cal.h"
-#endif  // ACCEL_CAL_ENABLED
-
-#ifdef GYRO_CAL_ENABLED
-#include "calibration/gyroscope/gyro_cal.h"
-#include "calibration/over_temp/over_temp_cal.h"
-#endif  // GYRO_CAL_ENABLED
-
-#ifdef MAG_CAL_ENABLED
-#include "calibration/diversity_checker/diversity_checker.h"
-#include "calibration/magnetometer/mag_cal/mag_cal.h"
-#endif  // MAG_CAL_ENABLED
-
-#include "common/math/macros.h"
-
-namespace nano_calibration {
-
-//////// ACCELEROMETER CALIBRATION ////////
-#ifdef ACCEL_CAL_ENABLED
-constexpr AccelCalParameters kAccelCalParameters{
-    MSEC_TO_NANOS(800),  // t0
-    5,                   // n_s
-    15,                  // fx
-    15,                  // fxb
-    15,                  // fy
-    15,                  // fyb
-    15,                  // fz
-    15,                  // fzb
-    15,                  // fle
-    0.00025f             // th
-};
-#endif  // ACCEL_CAL_ENABLED
-
-//////// GYROSCOPE CALIBRATION ////////
-#ifdef GYRO_CAL_ENABLED
-constexpr GyroCalParameters kGyroCalParameters{
-    SEC_TO_NANOS(1.4),    // min_still_duration_nanos
-    SEC_TO_NANOS(1.4),    // max_still_duration_nanos [see, NOTE 1]
-    0,                    // calibration_time_nanos
-    MSEC_TO_NANOS(500),   // window_time_duration_nanos
-    0,                    // bias_x
-    0,                    // bias_y
-    0,                    // bias_z
-    0.95f,                // stillness_threshold
-    MDEG_TO_RAD * 60.0f,  // stillness_mean_delta_limit [rad/sec]
-    3.0e-5f,              // gyro_var_threshold [rad/sec]^2
-    3.0e-6f,              // gyro_confidence_delta [rad/sec]^2
-    4.5e-3f,              // accel_var_threshold [m/sec^2]^2
-    9.0e-4f,              // accel_confidence_delta [m/sec^2]^2
-    5.0f,                 // mag_var_threshold [uTesla]^2
-    1.0f,                 // mag_confidence_delta [uTesla]^2
-    1.5f,                 // temperature_delta_limit_celsius
-    true                  // gyro_calibration_enable
-};
-// [NOTE 1]: 'max_still_duration_nanos' is set to 1.4 seconds to achieve a max
-// stillness period of 1.5 seconds and avoid buffer boundary conditions that
-// could push the max stillness to the next multiple of the analysis window
-// length (i.e., 2.0 seconds).
-
-constexpr OverTempCalParameters kGyroOtcParameters{
-    MSEC_TO_NANOS(100),    // min_temp_update_period_nanos
-    DAYS_TO_NANOS(2),      // age_limit_nanos
-    0.75f,                 // delta_temp_per_bin
-    40.0f * MDEG_TO_RAD,   // jump_tolerance
-    100.0f * MDEG_TO_RAD,  // outlier_limit
-    250.0f * MDEG_TO_RAD,  // temp_sensitivity_limit
-    8.0e3f * MDEG_TO_RAD,  // sensor_intercept_limit
-    0.1f * MDEG_TO_RAD,    // significant_offset_change
-    5,                     // min_num_model_pts
-    true                   // over_temp_enable
-};
-#endif  // GYRO_CAL_ENABLED
-
-//////// MAGNETOMETER CALIBRATION ////////
-#ifdef MAG_CAL_ENABLED
-constexpr MagCalParameters kMagCalParameters{
-    3000000,  // min_batch_window_in_micros
-    0.0f,     // x_bias
-    0.0f,     // y_bias
-    0.0f,     // z_bias
-    1.0f,     // c00
-    0.0f,     // c01
-    0.0f,     // c02
-    0.0f,     // c10
-    1.0f,     // c11
-    0.0f,     // c12
-    0.0f,     // c20
-    0.0f,     // c21
-    1.0f      // c22
-};
-
-constexpr DiversityCheckerParameters kMagDiversityParameters{
-    6.0f,    // var_threshold
-    10.0f,   // max_min_threshold
-    48.0f,   // local_field
-    0.5f,    // threshold_tuning_param
-    2.552f,  // max_distance_tuning_param
-    8,       // min_num_diverse_vectors
-    1        // max_num_max_distance
-};
-#endif  // MAG_CAL_ENABLED
-
-}  // namespace nano_calibration
-
-#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index 0df2ae6b..0471b8d9 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -16,6 +16,7 @@
 
 #include "calibration/nano_calibration/nano_calibration.h"
 
+#include <cstdint>
 #include <cstring>
 
 #include "chre/util/nanoapp/log.h"
@@ -23,6 +24,28 @@
 namespace nano_calibration {
 namespace {
 
+// Common log message sensor-specific identifiers.
+constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
+constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
+constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+
+// Defines a plan for limiting log messages so that upon initialization there
+// begins a period set by 'duration_of_rapid_messages_min' where log messages
+// appear at a rate set by 'rapid_message_interval_sec'. Afterwards, log
+// 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 duration_of_rapid_messages_min;
+};
+
+constexpr LogMessageRegimen kGyroscopeMessagePlan = {
+    /*rapid_message_interval_sec*/ 20,
+    /*slow_message_interval_min*/ 5,
+    /*duration_of_rapid_messages_min*/ 3
+};
+
 using ::online_calibration::CalibrationDataThreeAxis;
 using ::online_calibration::CalibrationTypeFlags;
 using ::online_calibration::SensorData;
@@ -87,6 +110,9 @@ void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
       NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
     }
   }
+
+  // Resets the initialization timestamp. Set below in HandleSensorSamples.
+  initialization_start_time_nanos_ = 0;
 }
 
 void NanoSensorCal::HandleSensorSamples(
@@ -120,6 +146,13 @@ void NanoSensorCal::HandleSensorSamples(
     memcpy(sample.data, data[i].v, sizeof(sample.data));
     ProcessSample(sample);
   }
+
+  // Starts tracking the time after initialization to help rate limit gyro log
+  // messaging.
+  if (initialization_start_time_nanos_ == 0) {
+    initialization_start_time_nanos_ = header.baseTimestamp;
+    gyro_notification_time_nanos_ = 0;
+  }
 }
 
 void NanoSensorCal::HandleTemperatureSamples(
@@ -174,15 +207,7 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) {
       if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
                                gyro_cal_->GetSensorCalibration(),
                                gyro_cal_update_flags_, kGyroTag)) {
-        // Limits the log messaging update rate for the gyro calibrations since
-        // these can occur frequently with rapid temperature changes.
-        if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
-                sample.timestamp_nanos, gyro_notification_time_nanos_,
-                kNanoSensorCalMessageIntervalNanos)) {
-          gyro_notification_time_nanos_ = sample.timestamp_nanos;
-          PrintCalibration(gyro_cal_->GetSensorCalibration(),
-                           gyro_cal_update_flags_, kGyroTag);
-        }
+        HandleGyroLogMessage(sample.timestamp_nanos);
       }
     }
   }
@@ -413,4 +438,25 @@ void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
   }
 }
 
+void 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 =
+      (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+          timestamp_nanos, initialization_start_time_nanos_,
+          MIN_TO_NANOS(kGyroscopeMessagePlan.duration_of_rapid_messages_min)))
+          ? MIN_TO_NANOS(kGyroscopeMessagePlan.slow_message_interval_min)
+          : 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);
+
+  if (print_gyro_log) {
+    gyro_notification_time_nanos_ = timestamp_nanos;
+    PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_,
+                     kGyroTag);
+  }
+}
+
 }  // 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 d56d0347..5dad0f68 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -47,6 +47,7 @@
 
 #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"
@@ -56,14 +57,6 @@
 
 namespace nano_calibration {
 
-// Common log message sensor-specific identifiers.
-constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
-constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
-constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
-
-// Limits NanoSensorCal notifications to once every minute.
-constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
-
 /*
  * NanoSensorCal is a container class for dynamic runtime calibration sensor
  * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
@@ -133,6 +126,8 @@ class NanoSensorCal {
       const online_calibration::CalibrationDataThreeAxis &cal_data,
       online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
 
+  void HandleGyroLogMessage(uint64_t timestamp_nanos);
+
   // Pointer to the accelerometer runtime calibration object.
   OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
 
@@ -142,6 +137,7 @@ class NanoSensorCal {
   // Limits the log messaging update rate for the gyro calibrations since these
   // can occur frequently with rapid temperature changes.
   uint64_t gyro_notification_time_nanos_ = 0;
+  uint64_t initialization_start_time_nanos_ = 0;
 
   // Pointer to the magnetometer runtime calibration object.
   OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
index e9def4e6..66d8fc58 100644
--- a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
@@ -16,7 +16,9 @@
 
 #include "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h"
 
+#ifdef ACCEL_CAL_DBG_ENABLED
 #include "calibration/util/cal_log.h"
+#endif  // ACCEL_CAL_DBG_ENABLED
 
 namespace online_calibration {
 
@@ -71,8 +73,10 @@ bool AccelOffsetCal::SetInitialCalibration(
     const CalibrationDataThreeAxis& input_cal_data) {
   // Checks that the input calibration type matches the algorithm type.
   if (input_cal_data.type != get_sensor_type()) {
+#ifdef ACCEL_CAL_DBG_ENABLED
     CAL_DEBUG_LOG("[AccelOffsetCal]",
                   "SetInitialCalibration failed due to wrong sensor type.");
+#endif  // ACCEL_CAL_DBG_ENABLED
     return false;
   }
 
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
index 0256495a..60b59df4 100644
--- a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
@@ -68,6 +68,9 @@ class AccelOffsetCal final
     return SensorType::kAccelerometerMps2;
   };
 
+  // Accessor for the runtime calibration object.
+  const AccelCal& get_accel_cal() const { return accel_cal_; }
+
  private:
   // Accelerometer offset calibration algorithm data structure.
   AccelCal accel_cal_;
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 0404936b..7d66f756 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
@@ -131,10 +131,9 @@ struct CalibrationDataThreeAxis {
   SensorType type = SensorType::kUndefined;
 
   // Optional pointer to an array of over-temperature model data (null when not
-  // used). For initialization, populating a model dataset will take precedence
-  // over the linear model parameters provided in the calibration data.
+  // used).
   OverTempModelThreeAxis* otc_model_data = nullptr;
-  int16_t num_model_pts = 0;
+  int16_t num_model_pts = 0;  // Length of otc_model_data array.
 
   // Helper function that resets the calibration data to a set of neutral
   // reference values where no calibration correction would be applied if used.
@@ -203,10 +202,9 @@ struct CalibrationDataSingleAxis {
   SensorType type = SensorType::kUndefined;
 
   // Optional pointer to an array of over-temperature model data (null when not
-  // used). For initialization, populating a model dataset will take precedence
-  // over the linear model parameters provided in the calibration data.
+  // used).
   OverTempModelSingleAxis* otc_model_data = nullptr;
-  int16_t num_model_pts = 0;
+  int16_t num_model_pts = 0;  // Length of otc_model_data array.
 
   // Helper function that resets the calibration data to a set of neutral
   // reference values where no calibration correction would be applied if used.
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
index f4c16133..23b63c9b 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -46,7 +46,8 @@ enum class SensorType : int8_t {
   kMagnetometerUt = 3,      // 3-axis sensor (units = micro-Tesla).
   kTemperatureCelsius = 4,  // 1-axis sensor (units = degrees Celsius).
   kBarometerHpa = 5,        // 1-axis sensor (units = hecto-Pascal).
-  kWifiM = 6                // 3-axis sensor (units = meter).
+  kWifiM = 6,               // 3-axis sensor (units = meter).
+  kProximity = 7,           // 1-axis sensor (units = ?).
 };
 
 // Helper function for determining if a sensor type is 3-axis, otherwise it's
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 26eef573..d73ed3b4 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
@@ -16,7 +16,9 @@
 
 #include "calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h"
 
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
 #include "calibration/util/cal_log.h"
+#endif  // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLE
 
 namespace online_calibration {
 
@@ -131,8 +133,10 @@ bool GyroOffsetOtcCal::SetInitialCalibration(
     const CalibrationDataThreeAxis& input_cal_data) {
   // Checks that the input calibration type matches the algorithm type.
   if (input_cal_data.type != get_sensor_type()) {
+#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
     CAL_DEBUG_LOG("[GyroOffsetOtcCal]",
                   "SetInitialCalibration failed due to wrong sensor type.");
+#endif  // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLE
     return false;
   }
 
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
index e21007b3..9b8962a9 100644
--- a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
@@ -74,6 +74,10 @@ class GyroOffsetOtcCal final
     return SensorType::kGyroscopeRps;
   };
 
+  // Accessors for the runtime calibration objects.
+  const GyroCal& get_gyro_cal() const { return gyro_cal_; }
+  const OverTempCal& get_over_temp_cal() const { return over_temp_cal_; }
+
  private:
   // GyroCal algorithm data structure.
   GyroCal gyro_cal_;
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
index fe787b1a..2453dcd5 100644
--- a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
@@ -16,12 +16,13 @@
 
 #include "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h"
 
+#ifdef MAG_CAL_DEBUG_ENABLE
 #include "calibration/util/cal_log.h"
+#endif  // MAG_CAL_DEBUG_ENABLE
 
 namespace online_calibration {
 
 // Empirically estimated upper bounds on offset error.
-constexpr float MagDiverseCal::kLowQualityUt;
 constexpr float MagDiverseCal::kHighQualityUt;
 
 void MagDiverseCal::Initialize(
@@ -71,8 +72,10 @@ bool MagDiverseCal::SetInitialCalibration(
     const CalibrationDataThreeAxis& input_cal_data) {
   // Checks that the input calibration type matches the algorithm type.
   if (input_cal_data.type != get_sensor_type()) {
+#ifdef MAG_CAL_DEBUG_ENABLE
     CAL_DEBUG_LOG("[MagDiverseCal]",
                   "SetInitialCalibration failed due to wrong sensor type.");
+#endif  // MAG_CAL_DEBUG_ENABLE
     return false;
   }
 
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
index 11ede3d4..9b60a7f4 100644
--- a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
@@ -43,8 +43,7 @@ namespace online_calibration {
 class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
  public:
   // Empirically estimated upper bounds on offset error.
-  static constexpr float kLowQualityUt = 1000.0f;  // Units of micro Tesla
-  static constexpr float kHighQualityUt = 5.0f;    // Units of micro Tesla
+  static constexpr float kHighQualityUt = 5.0f;  // Units of micro Tesla
 
   MagDiverseCal() = default;
 
@@ -72,6 +71,9 @@ class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
     return SensorType::kMagnetometerUt;
   };
 
+  // Accessor for the runtime calibration object.
+  const MagCal& get_mag_cal() const { return mag_cal_; }
+
  private:
   // MagCal algorithm data structure.
   MagCal mag_cal_;
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 84d7411f..6f887f78 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -21,7 +21,10 @@
 #include <math.h>
 #include <string.h>
 
+#if defined(OVERTEMPCAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_LOG_TEMP)
 #include "calibration/util/cal_log.h"
+#endif  // OVERTEMPCAL_DBG_ENABLED || OVERTEMPCAL_DBG_LOG_TEMP
+
 #include "util/nano_assert.h"
 
 /////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -603,7 +606,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal,
 
   // Computes the temperature bin range data.
   const int32_t bin_num =
-      CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
+      NANO_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin);
   const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin;
   const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin;
 
@@ -746,31 +749,6 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
   over_temp_cal->last_offset_update_nanos = timestamp_nanos;
 }
 
-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);
-
-  float max_error_test;
-  memset(max_error, 0, 3 * sizeof(float));
-
-  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
-    for (size_t j = 0; j < 3; j++) {
-      max_error_test =
-          NANO_ABS(over_temp_cal->model_data[i].offset[j] -
-                   (temp_sensitivity[j] *
-                        over_temp_cal->model_data[i].offset_temp_celsius +
-                    sensor_intercept[j]));
-      if (max_error_test > max_error[j]) {
-        max_error[j] = max_error_test;
-      }
-    }
-  }
-}
-
 bool overTempValidateAndSetWeight(
     struct OverTempCal *over_temp_cal, size_t index,
     const struct OverTempCalWeight *new_otc_weight) {
@@ -1311,8 +1289,8 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
   // 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.
-  const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
-                                          over_temp_cal->delta_temp_per_bin);
+  const int32_t start_bin_num = NANO_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
+                                           over_temp_cal->delta_temp_per_bin);
   float offset_temp_celsius =
       (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
 
@@ -1771,4 +1749,29 @@ void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal,
   over_temp_cal->otc_unit_conversion = otc_unit_conversion;
 }
 
+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);
+
+  float max_error_test;
+  memset(max_error, 0, 3 * sizeof(float));
+
+  for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+    for (size_t j = 0; j < 3; j++) {
+      max_error_test =
+          NANO_ABS(over_temp_cal->model_data[i].offset[j] -
+                   (temp_sensitivity[j] *
+                        over_temp_cal->model_data[i].offset_temp_celsius +
+                    sensor_intercept[j]));
+      if (max_error_test > max_error[j]) {
+        max_error[j] = max_error_test;
+      }
+    }
+  }
+}
+
 #endif  // OVERTEMPCAL_DBG_ENABLED
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 8f6f0a4d..446d0a2b 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -505,28 +505,6 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
                                float temperature_celsius);
 
 /*
- * Computes the maximum absolute error between the 'model_data' estimates and
- * the estimate determined by the input model parameters.
- *   max_error (over all i)
- *     |model_data[i]->offset_xyz -
- *       getCompensatedOffset(model_data[i]->offset_temp_celsius,
- *         temp_sensitivity, sensor_intercept)|
- *
- * INPUTS:
- *   over_temp_cal:    Over-temp data structure.
- *   temp_sensitivity: Model temperature sensitivity to test (array).
- *   sensor_intercept: Model intercept to test (array).
- * OUTPUTS:
- *   max_error:        Maximum absolute error for the candidate model (array).
- *
- * 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 overTempGetModelError(const struct OverTempCal *over_temp_cal,
-                           const float *temp_sensitivity,
-                           const float *sensor_intercept, float *max_error);
-
-/*
  * Defines an element in the weighting function that is used to control the
  * fitting behavior of the simple linear model regression used in this module.
  * The total number of weighting levels that define this functionality is set by
@@ -571,6 +549,28 @@ void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal,
                                  const char *otc_sensor_tag,
                                  const char *otc_unit_tag,
                                  float otc_unit_conversion);
+
+/*
+ * Computes the maximum absolute error between the 'model_data' estimates and
+ * the estimate determined by the input model parameters.
+ *   max_error (over all i)
+ *     |model_data[i]->offset_xyz -
+ *       getCompensatedOffset(model_data[i]->offset_temp_celsius,
+ *         temp_sensitivity, sensor_intercept)|
+ *
+ * INPUTS:
+ *   over_temp_cal:    Over-temp data structure.
+ *   temp_sensitivity: Model temperature sensitivity to test (array).
+ *   sensor_intercept: Model intercept to test (array).
+ * OUTPUTS:
+ *   max_error:        Maximum absolute error for the candidate model (array).
+ *
+ * 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 overTempGetModelError(const struct OverTempCal *over_temp_cal,
+                           const float *temp_sensitivity,
+                           const float *sensor_intercept, float *max_error);
 #endif  // OVERTEMPCAL_DBG_ENABLED
 
 #ifdef __cplusplus
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 853a73d5..8729e0dd 100644
--- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -21,7 +21,10 @@
 #include <stdio.h>
 #include <string.h>
 
+#ifdef SPHERE_FIT_DBG_ENABLED
 #include "calibration/util/cal_log.h"
+#endif  // SPHERE_FIT_DBG_ENABLED
+
 #include "common/math/mat.h"
 #include "common/math/vec.h"
 
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
index d3bbf7f1..6dff2ada 100644
--- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
@@ -141,4 +141,4 @@ void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
 }
 #endif
 
-#endif  //  LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+#endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 46297dbb..57179a7e 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -22,6 +22,8 @@
 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
 
+#include "common/math/macros.h"
+
 // clang-format off
 #ifdef GCC_DEBUG_LOG
 # include <stdio.h>
@@ -55,10 +57,6 @@
 extern "C" {
 #endif
 
-// Floor macro implementation for platforms that do not supply the standard
-// floorf() math function.
-#define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x)))  // NOLINT
-
 /*
  * On some embedded software platforms numerical string formatting is not fully
  * supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that
@@ -71,8 +69,8 @@ extern "C" {
 #ifdef CAL_NO_FLOAT_FORMAT_STRINGS
 // Macro used to print floating point numbers with a specified number of digits.
 # define CAL_ENCODE_FLOAT(x, num_digits)           \
-  ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)),  \
-      (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) *     \
+  ((x < 0) ? "-" : ""), (int)NANO_FLOOR(fabsf(x)),  \
+      (int)((fabsf(x) - NANO_FLOOR(fabsf(x))) *     \
             powf(10, num_digits))  // NOLINT
 
 // Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 66e423fe..89986808 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -39,10 +39,6 @@ void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
   solver->num_iter = 0;
 }
 
-void lmSolverDestroy(struct LmSolver *solver) {
-  (void)solver;
-}
-
 void lmSolverSetData(struct LmSolver *solver, struct LmData *data) {
   ASSERT_NOT_NULL(solver);
   ASSERT_NOT_NULL(data);
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
index d62308da..996a5bb1 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.h
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -111,8 +111,6 @@ struct LmSolver {
 void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
                   ResidualAndJacobianFunction func);
 
-void lmSolverDestroy(struct LmSolver *solver);
-
 // Sets pointer for temporary data needed for an individual LM solve.
 // Note, this must be called prior to calling lmSolverSolve().
 void lmSolverSetData(struct LmSolver *solver, struct LmData *data);
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index cb75595b..060dc0f4 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -31,25 +31,29 @@
 #define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
 #define SIGMOID(x) (1 / (1 + expf(-x)))
 
+// Floor macro implementation for platforms that do not supply the standard
+// floorf() math function.
+#define NANO_FLOOR(x) ((int)(x) - ((x) < (int)(x)))  // NOLINT
+
 // Timestamp conversion macros.
 #ifdef __cplusplus
-#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000)))
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>((x) * UINT64_C(1000000)))
 #else
-#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000)))  // NOLINT
+#define MSEC_TO_NANOS(x) ((uint64_t)((x) * UINT64_C(1000000)))  // NOLINT
 #endif
 
-#define SEC_TO_NANOS(x)  MSEC_TO_NANOS(x * UINT64_C(1000))
-#define MIN_TO_NANOS(x)  SEC_TO_NANOS (x * UINT64_C(60))
-#define HRS_TO_NANOS(x)  MIN_TO_NANOS (x * UINT64_C(60))
-#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24))
+#define SEC_TO_NANOS(x)  MSEC_TO_NANOS((x) * UINT64_C(1000))
+#define MIN_TO_NANOS(x)  SEC_TO_NANOS ((x) * UINT64_C(60))
+#define HRS_TO_NANOS(x)  MIN_TO_NANOS ((x) * UINT64_C(60))
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS ((x) * UINT64_C(24))
 
 // Sample rate to period conversion.
 #ifdef __cplusplus
 #define HZ_TO_PERIOD_NANOS(hz) \
-  (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024)))
+  (SEC_TO_NANOS(1024) / (static_cast<uint64_t>((hz) * 1024)))
 #else
 #define HZ_TO_PERIOD_NANOS(hz) \
-  (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024)))  // NOLINT
+  (SEC_TO_NANOS(1024) / ((uint64_t)((hz) * 1024)))  // NOLINT
 #endif
 
 // Unit conversion: nanoseconds to seconds.
diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h
index 9d69405e..d5391f96 100644
--- a/firmware/os/algos/common/math/mat.h
+++ b/firmware/os/algos/common/math/mat.h
@@ -45,6 +45,7 @@ struct Mat33 {
   float elem[3][3];
 };
 
+// Note: Keep this code to preserve Android codebase dependencies.
 struct Size3 {
   uint32_t elem[3];
 };
@@ -109,6 +110,7 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A);
 void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A,
                              const struct Mat33 *B);
 
+// Note: Keep this code to preserve Android codebase dependencies.
 // Updates out with the multiplication of A with B's transpose, i.e.:
 // out = A B^T
 void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A,