Snap for 7339742 from 846b602d3936757bf722778b355a2d97100efccd to sc-d1-releaseandroid-12.0.0_r7android-12.0.0_r6android-12.0.0_r5android-12.0.0_r4android-12.0.0_r15android-12.0.0_r14android-12.0.0_r13android-12.0.0_r12android12-d1-s6-releaseandroid12-d1-s5-releaseandroid12-d1-s4-releaseandroid12-d1-s3-releaseandroid12-d1-s2-releaseandroid12-d1-s1-releaseandroid12-d1-release

Change-Id: I842d23c91de7dd51812f4a1f4474991571286927

7 files changed, 121 insertions, 24 deletions

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/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;