[IMU_Cal] Runtime sensor calibration code update.

This code transfers all of the refactored changes developed and
tested in Google3.

Tip of G3 CL: 191014272
Bug: 75333210
Test: ./load_app.sh and verified calibrations on device.
Change-Id: Ieb414468296ce620ecf7fe275595c561c33779c1

1 files changed, 410 insertions, 0 deletions

diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
new file mode 100644
index 00000000..68b477b7
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -0,0 +1,410 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/nano_calibration/nano_calibration.h"
+
+#include <cstring>
+
+#include "chre/util/nanoapp/log.h"
+
+namespace nano_calibration {
+namespace {
+
+using ::online_calibration::CalibrationDataThreeAxis;
+using ::online_calibration::CalibrationTypeFlags;
+using ::online_calibration::SensorData;
+using ::online_calibration::SensorIndex;
+using ::online_calibration::SensorType;
+
+// NanoSensorCal logging macros.
+#ifdef NANO_SENSOR_CAL_DBG_ENABLED
+#ifndef LOG_TAG
+#define LOG_TAG "[ImuCal]"
+#endif
+#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) LOGI("%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__)
+#else
+#define NANO_CAL_LOGD(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
+#endif  // NANO_SENSOR_CAL_DBG_ENABLED
+
+}  // namespace
+
+void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
+                               OnlineCalibrationThreeAxis *gyro_cal,
+                               OnlineCalibrationThreeAxis *mag_cal) {
+  // Loads stored calibration data and initializes the calibration algorithms.
+  accel_cal_ = accel_cal;
+  if (accel_cal_ != nullptr) {
+    if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
+      if (LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
+                             kAccelTag)) {
+        PrintCalibration(accel_cal_->GetSensorCalibration(),
+                         CalibrationTypeFlags::BIAS, kAccelTag);
+      }
+      NANO_CAL_LOGI(kAccelTag,
+                    "Accelerometer runtime calibration initialized.");
+    } else {
+      accel_cal_ = nullptr;
+      NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+
+  gyro_cal_ = gyro_cal;
+  if (gyro_cal_ != nullptr) {
+    if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
+      if (LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, kGyroTag)) {
+        PrintCalibration(
+            gyro_cal_->GetSensorCalibration(),
+            CalibrationTypeFlags::BIAS | CalibrationTypeFlags::OVER_TEMP,
+            kGyroTag);
+      }
+      NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
+    } else {
+      gyro_cal_ = nullptr;
+      NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+
+  mag_cal_ = mag_cal;
+  if (mag_cal != nullptr) {
+    if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
+      if (LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
+                             kMagTag)) {
+        PrintCalibration(mag_cal_->GetSensorCalibration(),
+                         CalibrationTypeFlags::BIAS, kMagTag);
+      }
+      NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
+    } else {
+      mag_cal_ = nullptr;
+      NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
+    }
+  }
+}
+
+void NanoSensorCal::HandleSensorSamples(
+    uint16_t event_type, const chreSensorThreeAxisData *event_data) {
+  // Converts CHRE Event -> SensorData::SensorType.
+  SensorData sample;
+  switch (event_type) {
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
+      sample.type = SensorType::kAccelerometerMps2;
+      break;
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
+      sample.type = SensorType::kGyroscopeRps;
+      break;
+    case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
+      sample.type = SensorType::kMagnetometerUt;
+      break;
+    default:
+      // This sensor type is not used.
+      NANO_CAL_LOGW("[NanoSensorCal]",
+                    "Unexpected 3-axis sensor type received.");
+      return;
+  }
+
+  // Sends the sensor payload to the calibration algorithms and checks for
+  // calibration updates.
+  const auto &header = event_data->header;
+  const auto *data = event_data->readings;
+  sample.timestamp_nanos = header.baseTimestamp;
+  for (size_t i = 0; i < header.readingCount; i++) {
+    sample.timestamp_nanos += data[i].timestampDelta;
+    memcpy(sample.data, data[i].v, sizeof(sample.data));
+    ProcessSample(sample);
+  }
+}
+
+void NanoSensorCal::HandleTemperatureSamples(
+    uint16_t event_type, const chreSensorFloatData *event_data) {
+  // Computes the mean of the batched temperature samples and delivers it to the
+  // calibration algorithms. Note, the temperature sensor batch size determines
+  // its minimum update interval.
+  if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
+      event_data->header.readingCount > 0) {
+    const auto header = event_data->header;
+    const auto *data = event_data->readings;
+
+    SensorData sample;
+    sample.type = SensorType::kTemperatureCelsius;
+    sample.timestamp_nanos = header.baseTimestamp;
+
+    float accum_temperature_celsius = 0.0f;
+    for (size_t i = 0; i < header.readingCount; i++) {
+      sample.timestamp_nanos += data[i].timestampDelta;
+      accum_temperature_celsius += data[i].value;
+    }
+    sample.data[SensorIndex::kSingleAxis] =
+        accum_temperature_celsius / header.readingCount;
+    ProcessSample(sample);
+  } else {
+    NANO_CAL_LOGW("[NanoSensorCal]",
+                  "Unexpected single-axis sensor type received.");
+  }
+}
+
+void NanoSensorCal::ProcessSample(const SensorData &sample) {
+  // Sends a new sensor sample to each active calibration algorithm and sends
+  // out notifications for new calibration updates.
+  if (accel_cal_ != nullptr) {
+    const CalibrationTypeFlags accel_cal_flags =
+        accel_cal_->SetMeasurement(sample);
+    if (accel_cal_flags != CalibrationTypeFlags::NONE) {
+      NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
+                           accel_cal_->GetSensorCalibration(), accel_cal_flags,
+                           kAccelTag);
+      PrintCalibration(accel_cal_->GetSensorCalibration(), accel_cal_flags,
+                       kAccelTag);
+    }
+  }
+
+  if (gyro_cal_ != nullptr) {
+    const CalibrationTypeFlags gyro_cal_flags =
+        gyro_cal_->SetMeasurement(sample);
+    if (gyro_cal_flags != CalibrationTypeFlags::NONE) {
+      if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
+                               gyro_cal_->GetSensorCalibration(),
+                               gyro_cal_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_flags,
+                           kGyroTag);
+        }
+      }
+    }
+  }
+
+  if (mag_cal_ != nullptr) {
+    const CalibrationTypeFlags mag_cal_flags = mag_cal_->SetMeasurement(sample);
+    if (mag_cal_flags != CalibrationTypeFlags::NONE) {
+      NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
+                           mag_cal_->GetSensorCalibration(), mag_cal_flags,
+                           kMagTag);
+      PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_flags,
+                       kMagTag);
+    }
+  }
+}
+
+bool NanoSensorCal::NotifyAshCalibration(
+    uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
+    CalibrationTypeFlags flags, const char *sensor_tag) {
+  // Updates the sensor offset calibration using the ASH API.
+  ashCalInfo ash_cal_info;
+  memset(&ash_cal_info, 0, sizeof(ashCalInfo));
+  ash_cal_info.compMatrix[0] = 1.0f;  // Sets diagonal to unity (scale factor).
+  ash_cal_info.compMatrix[4] = 1.0f;
+  ash_cal_info.compMatrix[8] = 1.0f;
+  memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias));
+  ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
+
+  if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
+    return false;
+  }
+
+  // Uses the ASH API to store ONLY the algorithm calibration parameters that
+  // have been modified by the calibration algorithm.
+  ashCalParams ash_cal_parameters;
+  if (flags & CalibrationTypeFlags::BIAS) {
+    ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius;
+    memcpy(ash_cal_parameters.offset, cal_data.offset,
+           sizeof(ash_cal_parameters.offset));
+    ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+    ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+  }
+
+  if (flags & CalibrationTypeFlags::OVER_TEMP) {
+    memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity,
+           sizeof(ash_cal_parameters.tempSensitivity));
+    memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept,
+           sizeof(ash_cal_parameters.tempIntercept));
+    ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+    ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+  }
+
+  if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
+    return false;
+  }
+
+  return true;
+}
+
+bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
+                                       OnlineCalibrationThreeAxis *online_cal,
+                                       const char *sensor_tag) {
+  ashCalParams recalled_ash_cal_parameters;
+  if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
+                               &recalled_ash_cal_parameters)) {
+    // Checks whether a valid set of runtime calibration parameters was received
+    // and can be used for initialization.
+    CalibrationTypeFlags flags = CalibrationTypeFlags::NONE;
+    if (DetectRuntimeCalibration(chreSensorType, sensor_tag, &flags,
+                                 &recalled_ash_cal_parameters)) {
+      CalibrationDataThreeAxis cal_data;
+      cal_data.type = online_cal->get_sensor_type();
+      cal_data.cal_update_time_nanos = chreGetTime();
+
+      // Analyzes the calibration flags and sets only the runtime calibration
+      // values that were received.
+      if (flags & CalibrationTypeFlags::BIAS) {
+        cal_data.offset_temp_celsius =
+            recalled_ash_cal_parameters.offsetTempCelsius;
+        memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
+               sizeof(cal_data.offset));
+      }
+
+      if (flags & CalibrationTypeFlags::OVER_TEMP) {
+        memcpy(cal_data.temp_sensitivity,
+               recalled_ash_cal_parameters.tempSensitivity,
+               sizeof(cal_data.temp_sensitivity));
+        memcpy(cal_data.temp_intercept,
+               recalled_ash_cal_parameters.tempIntercept,
+               sizeof(cal_data.temp_intercept));
+      }
+
+      // Sets the algorithm's initial calibration data and notifies ASH to apply
+      // the recalled calibration data.
+      if (online_cal->SetInitialCalibration(cal_data)) {
+        return NotifyAshCalibration(chreSensorType,
+                                    online_cal->GetSensorCalibration(), flags,
+                                    sensor_tag);
+      } else {
+        NANO_CAL_LOGE(sensor_tag,
+                      "Calibration data failed to initialize algorithm.");
+      }
+    }
+  } else {
+    NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data.");
+  }
+
+  return false;
+}
+
+bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType,
+                                             const char *sensor_tag,
+                                             CalibrationTypeFlags *flags,
+                                             ashCalParams *ash_cal_parameters) {
+  // Analyzes calibration source flags to determine whether factory calibration
+  // data was received. A valid factory calibration source will include at least
+  // an offset.
+  bool factory_cal_detected =
+      ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
+      ash_cal_parameters->offsetTempCelsiusSource ==
+          ASH_CAL_PARAMS_SOURCE_FACTORY;
+
+  bool runtime_cal_detected = false;
+  if (factory_cal_detected) {
+    // Prints the retrieved factory calibration data.
+    NANO_CAL_LOGI(sensor_tag, "Factory calibration detected.");
+    PrintAshCalParams(*ash_cal_parameters, sensor_tag);
+
+    // Since the factory calibration is applied lower in the sensor framework,
+    // initialization using these parameters must be skipped to avoid the
+    // possibility of double calibration. Returns 'false' to avoid further
+    // initialization. Note, the runtime calibration algorithms dynamically
+    // correct the residuals that the factory calibration doesn't or can't
+    // account for (e.g., aging induced error charateristics).
+    return false;
+  } else {
+    // Analyzes calibration source flags to determine whether runtime
+    // calibration values have been loaded and may be used for initialization. A
+    // valid runtime calibration source will include at least an offset.
+
+    // Converts the ASH calibration source flags to CalibrationTypeFlags. These
+    // will be used to determine which values to copy from 'ash_cal_parameters'
+    // and provide to the calibration algorithms for initialization.
+    if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+        ash_cal_parameters->offsetTempCelsiusSource ==
+            ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+      runtime_cal_detected = true;
+      *flags = CalibrationTypeFlags::BIAS;
+    }
+
+    if (ash_cal_parameters->tempSensitivitySource ==
+            ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+        ash_cal_parameters->tempInterceptSource ==
+            ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+      runtime_cal_detected = true;
+      *flags |= CalibrationTypeFlags::OVER_TEMP;
+    }
+
+    if (!runtime_cal_detected) {
+      // This is a warning (not an error) since the runtime algorithms will
+      // function correctly with no recalled calibration values. They will
+      // eventually trigger and update the system with valid calibration data.
+      NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
+    }
+  }
+
+  return runtime_cal_detected;
+}
+
+// Helper functions for logging calibration information.
+void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params,
+                                      const char *sensor_tag) {
+  NANO_CAL_LOGI(sensor_tag, "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+                cal_params.offset[0], cal_params.offset[1],
+                cal_params.offset[2], cal_params.offsetTempCelsius);
+
+  NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
+                cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
+                cal_params.tempSensitivity[2]);
+
+  NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
+                cal_params.tempIntercept[0], cal_params.tempIntercept[1],
+                cal_params.tempIntercept[2]);
+
+  NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
+                cal_params.scaleFactor[0], cal_params.scaleFactor[1],
+                cal_params.scaleFactor[2]);
+
+  NANO_CAL_LOGI(sensor_tag,
+                "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
+                cal_params.crossAxis[0], cal_params.crossAxis[1],
+                cal_params.crossAxis[2]);
+}
+
+void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
+                                     CalibrationTypeFlags flags,
+                                     const char *sensor_tag) {
+  if (flags & CalibrationTypeFlags::BIAS) {
+    NANO_CAL_LOGI(sensor_tag,
+                  "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+                  cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
+                  cal_data.offset_temp_celsius);
+  }
+
+  if (flags & CalibrationTypeFlags::OVER_TEMP) {
+    NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f",
+                  cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1],
+                  cal_data.temp_sensitivity[2]);
+    NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f",
+                  cal_data.temp_intercept[0], cal_data.temp_intercept[1],
+                  cal_data.temp_intercept[2]);
+  }
+}
+
+}  // namespace nano_calibration