diff options
author | Xin Li <delphij@google.com> | 2022-06-27 23:37:26 +0000 |
---|---|---|
committer | Xin Li <delphij@google.com> | 2022-06-27 23:37:26 +0000 |
commit | 26514c87e1c6e9446848df25e3cc0da295c58758 (patch) | |
tree | a8fc921c3859fb89eedb0ad742a5be8e2c428fe8 | |
parent | e948b50a4e07ec679da0a78bd869470d92a07a98 (diff) | |
parent | d2d82174cd154ac8c43904f3ad8ced3be17a76b9 (diff) | |
download | contexthub-26514c87e1c6e9446848df25e3cc0da295c58758.tar.gz |
Merge tm-dev-plus-aosp-without-vendor@8763363temp_sam_242648940
Bug: 236760014
Merged-In: I0ff1481d841f5d49346264aa7140ec097d2718ad
Change-Id: I58f3cc16d49d5b99e4e907f799240dcaed5b575a
11 files changed, 372 insertions, 390 deletions
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc index 9d8e04d6..122af489 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc @@ -25,9 +25,9 @@ namespace nano_calibration { namespace { // Common log message sensor-specific identifiers. -char const *kAccelTag = "[ACCEL_MPS2]"; -char const *kGyroTag = "[GYRO_RPS]"; -char const *kMagTag = "[MAG_UT]"; +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 @@ -47,6 +47,8 @@ constexpr LogMessageRegimen kGyroscopeMessagePlan = { using ::online_calibration::CalibrationDataThreeAxis; using ::online_calibration::CalibrationTypeFlags; +using ::online_calibration::SensorData; +using ::online_calibration::SensorIndex; using ::online_calibration::SensorType; // NanoSensorCal logging macros. @@ -54,24 +56,6 @@ using ::online_calibration::SensorType; #define LOG_TAG "[ImuCal]" #endif -// Some devices do not have multisensor ASH API support. These macros remap to -// single-sensor functions. -#ifndef ASH_MULTI_CAL_SUPPORTED -#define ashSetMultiCalibration(chre_sensor_type, sensor_index, \ - calibration_index, ash_cal_info) \ - ashSetCalibration(chre_sensor_type, ash_cal_info) - -#define ashSaveMultiCalibrationParams(chre_sensor_type, sensor_index, \ - calibration_index, ash_cal_parameters) \ - ashSaveCalibrationParams(chre_sensor_type, ash_cal_parameters) - -#define ashLoadMultiCalibrationParams(chre_sensor_type, sensor_index, \ - calibration_index, \ - recalled_ash_cal_parameters) \ - ashLoadCalibrationParams(chre_sensor_type, ASH_CAL_STORAGE_ASH, \ - recalled_ash_cal_parameters) -#endif // ASH_MULTI_CAL_SUPPORTED - #ifdef NANO_SENSOR_CAL_DBG_ENABLED #define NANO_CAL_LOGD(tag, format, ...) \ TECHENG_LOGD("%s " format, tag, ##__VA_ARGS__) @@ -90,83 +74,190 @@ using ::online_calibration::SensorType; #define NANO_CAL_LOGI(tag, format, ...) \ TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__) -bool GetCalMetaData(const NanoSensorCal::OnlineCalibrationThreeAxis &online_cal, - uint8_t *chre_sensor_type, char const **sensor_tag, - uint8_t *sensor_index, uint8_t *calibration_index) { - *chre_sensor_type = 0; - *sensor_tag = nullptr; - *sensor_index = online_cal.get_sensor_index(); - *calibration_index = online_cal.get_calibration_index(); - - switch (online_cal.get_sensor_type()) { - case SensorType::kAccelerometerMps2: - *chre_sensor_type = CHRE_SENSOR_TYPE_ACCELEROMETER; - *sensor_tag = kAccelTag; +} // 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) { + LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_, + &accel_cal_update_flags_, 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) { + LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, + &gyro_cal_update_flags_, 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) { + LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_, + &mag_cal_update_flags_, kMagTag); + NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized."); + } else { + mag_cal_ = nullptr; + 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( + 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 SensorType::kGyroscopeRps: - *chre_sensor_type = CHRE_SENSOR_TYPE_GYROSCOPE; - *sensor_tag = kGyroTag; + case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA: + sample.type = SensorType::kGyroscopeRps; break; - case SensorType::kMagnetometerUt: - *chre_sensor_type = CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD; - *sensor_tag = kMagTag; + case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: + sample.type = SensorType::kMagnetometerUt; break; default: - NANO_CAL_LOGW("[NanoSensorCal]", "Unexpected sensor calibration (%d).", - static_cast<int>(online_cal.get_sensor_type())); - return false; + // This sensor type is not used. + NANO_CAL_LOGW("[NanoSensorCal]", + "Unexpected 3-axis sensor type received."); + return; } - return true; -} -} // namespace + // 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); + } + + // 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::UpdateCalibration( - online_calibration::CalibrationTypeFlags new_cal_flags, - const OnlineCalibrationThreeAxis &online_cal) { - if (new_cal_flags != CalibrationTypeFlags::NONE) { - uint8_t chre_sensor_type = 0; - char const *sensor_tag = nullptr; - uint8_t sensor_index = 0; - uint8_t calibration_index = 0; - if (GetCalMetaData(online_cal, &chre_sensor_type, &sensor_tag, - &sensor_index, &calibration_index)) { - NotifyAshCalibration(chre_sensor_type, sensor_index, calibration_index, - online_cal.GetSensorCalibration(), - online_cal.which_calibration_flags(), sensor_tag); +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."); } } -bool NanoSensorCal::NotifyAshCalibration( - uint8_t chre_sensor_type, uint8_t sensor_index, uint8_t calibration_index, - const CalibrationDataThreeAxis &cal_data, CalibrationTypeFlags flags, - char const *sensor_tag) { - bool is_log_update_allowed = true; - bool send_results_callback = true; - - if (chre_sensor_type == CHRE_SENSOR_TYPE_GYROSCOPE) { - // Rate-limits OTC gyro log updates since they can happen frequently with - // temperature changes. However, all GyroCal stillness and OTC model - // parameter updates will be reported through the results callback. - is_log_update_allowed = - IsGyroLogUpdateAllowed(cal_data.cal_update_time_nanos); - - send_results_callback = - is_log_update_allowed || flags != CalibrationTypeFlags::BIAS; +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 new_cal_flags = + accel_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + accel_cal_update_flags_ |= new_cal_flags; + NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, + accel_cal_->GetSensorCalibration(), + 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_); + } + } } - if (is_log_update_allowed) { - PrintCalibration(cal_data, sensor_index, calibration_index, flags, - sensor_tag); + if (gyro_cal_ != nullptr) { + const CalibrationTypeFlags new_cal_flags = + gyro_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + gyro_cal_update_flags_ |= new_cal_flags; + if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, + gyro_cal_->GetSensorCalibration(), + gyro_cal_update_flags_, kGyroTag)) { + 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_); + } + } + } } - if (result_callback_ != nullptr && send_results_callback) { - result_callback_->SetCalibrationEvent(cal_data.cal_update_time_nanos, - cal_data.type, sensor_index, - calibration_index, flags, cal_data); + if (mag_cal_ != nullptr) { + const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + mag_cal_update_flags_ |= new_cal_flags; + NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, + mag_cal_->GetSensorCalibration(), + 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_); + } + } } +} +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)); @@ -194,8 +285,7 @@ bool NanoSensorCal::NotifyAshCalibration( break; } - if (!ashSetMultiCalibration(chre_sensor_type, sensor_index, calibration_index, - &ash_cal_info)) { + if (!ashSetCalibration(chreSensorType, &ash_cal_info)) { NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update."); return false; } @@ -221,8 +311,7 @@ bool NanoSensorCal::NotifyAshCalibration( ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; } - if (!ashSaveMultiCalibrationParams(chre_sensor_type, sensor_index, - calibration_index, &ash_cal_parameters)) { + if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) { NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update."); return false; } @@ -230,97 +319,91 @@ bool NanoSensorCal::NotifyAshCalibration( return true; } -void NanoSensorCal::LoadAshCalibration(OnlineCalibrationThreeAxis *online_cal) { - uint8_t chre_sensor_type = 0; - char const *sensor_tag = nullptr; - uint8_t sensor_index = 0; - uint8_t calibration_index = 0; - ashCalParams recalled_ash_cal_parameters = {}; - - // Resets the rate limiter for gyro calibration update messages. - initial_gyro_cal_time_nanos_ = 0; - - if (!GetCalMetaData(*online_cal, &chre_sensor_type, &sensor_tag, - &sensor_index, &calibration_index) || - !ashLoadMultiCalibrationParams(chre_sensor_type, sensor_index, - calibration_index, - &recalled_ash_cal_parameters)) { +bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, + OnlineCalibrationThreeAxis *online_cal, + CalibrationTypeFlags *flags, + 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. + 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 { // This is not necessarily an error since there may not be any previously // stored runtime calibration data to load yet (e.g., first device boot). NANO_CAL_LOGW(sensor_tag, "ASH did not recall calibration data."); - return; } - // Checks whether a valid set of runtime calibration parameters was received - // and can be used for initialization. - online_calibration::CalibrationTypeFlags flags = CalibrationTypeFlags::NONE; - if (DetectRuntimeCalibration(chre_sensor_type, sensor_tag, sensor_index, - calibration_index, recalled_ash_cal_parameters, - flags)) { - 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)) { - NotifyAshCalibration(chre_sensor_type, sensor_index, calibration_index, - online_cal->GetSensorCalibration(), flags, - sensor_tag); - } else { - NANO_CAL_LOGE(sensor_tag, - "Calibration data failed to initialize algorithm."); - } - } + return false; } -bool NanoSensorCal::DetectRuntimeCalibration( - uint8_t chre_sensor_type, const char *sensor_tag, uint8_t sensor_index, - uint8_t calibration_index, const ashCalParams &ash_cal_parameters, - CalibrationTypeFlags &flags) { +bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, + const char *sensor_tag, + CalibrationTypeFlags *flags, + ashCalParams *ash_cal_parameters) { // 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. - flags = CalibrationTypeFlags::NONE; // Resets the calibration flags. + *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags. // Uses the ASH calibration source flags to set the appropriate // CalibrationTypeFlags. These will be used to determine which values to copy // from 'ash_cal_parameters' and provide to the calibration algorithms for // initialization. bool runtime_cal_detected = false; - if (ash_cal_parameters.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME && - ash_cal_parameters.offsetTempCelsiusSource == + 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; + *flags = CalibrationTypeFlags::BIAS; } - if (ash_cal_parameters.tempSensitivitySource == + if (ash_cal_parameters->tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME && - ash_cal_parameters.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { - flags |= CalibrationTypeFlags::OVER_TEMP; + ash_cal_parameters->tempInterceptSource == + ASH_CAL_PARAMS_SOURCE_RUNTIME) { + *flags |= CalibrationTypeFlags::OVER_TEMP; } if (runtime_cal_detected) { // Prints the retrieved runtime calibration data. - NANO_CAL_LOGD(sensor_tag, "Runtime calibration data detected."); + NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected."); + PrintAshCalParams(*ash_cal_parameters, sensor_tag); } else { // This is a warning (not an error) since the runtime algorithms will // function correctly with no recalled calibration values. They will @@ -331,55 +414,82 @@ bool NanoSensorCal::DetectRuntimeCalibration( return runtime_cal_detected; } +// Helper functions for logging calibration information. +void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params, + const char *sensor_tag) { + if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + 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); + } + + if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f", + cal_params.tempSensitivity[0], cal_params.tempSensitivity[1], + cal_params.tempSensitivity[2]); + } + + if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f", + cal_params.tempIntercept[0], cal_params.tempIntercept[1], + cal_params.tempIntercept[2]); + } + + if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f", + cal_params.scaleFactor[0], cal_params.scaleFactor[1], + cal_params.scaleFactor[2]); + } + + if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + 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, - uint8_t sensor_index, - uint8_t calibration_index, CalibrationTypeFlags flags, const char *sensor_tag) { if (flags & CalibrationTypeFlags::BIAS) { - NANO_CAL_LOGI( - sensor_tag, - "(%d, %d) Offset | Temp [C] | Quality: %.6f, %.6f, %.6f | %.2f | %d", - sensor_index, calibration_index, cal_data.offset[0], cal_data.offset[1], - cal_data.offset[2], cal_data.offset_temp_celsius, - static_cast<int>(cal_data.calibration_quality.level)); + 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, "(%d) Temp Sensitivity: %.6f, %.6f, %.6f", - sensor_index, cal_data.temp_sensitivity[0], - cal_data.temp_sensitivity[1], cal_data.temp_sensitivity[2]); - NANO_CAL_LOGI(sensor_tag, "(%d) Temp Intercept: %.6f, %.6f, %.6f", - sensor_index, cal_data.temp_intercept[0], - cal_data.temp_intercept[1], cal_data.temp_intercept[2]); + 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]); } } -bool NanoSensorCal::IsGyroLogUpdateAllowed(uint64_t timestamp_nanos) { - if (initial_gyro_cal_time_nanos_ == 0) { - initial_gyro_cal_time_nanos_ = timestamp_nanos; - gyro_notification_time_nanos_ = timestamp_nanos; - return true; - } - +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 = (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - /*t1=*/timestamp_nanos, /*t2=*/initial_gyro_cal_time_nanos_, + 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 is_log_update_allowed = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - /*t1=*/timestamp_nanos, /*t2=*/gyro_notification_time_nanos_, - /*t_delta=*/next_log_interval_nanos); + const bool print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( + timestamp_nanos, gyro_notification_time_nanos_, next_log_interval_nanos); - if (is_log_update_allowed) { + if (print_gyro_log) { gyro_notification_time_nanos_ = timestamp_nanos; + PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, + kGyroTag); } - return is_log_update_allowed; + 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 dd1f52ad..82a8396d 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h @@ -15,8 +15,7 @@ */ /* - * This module provides a helper class for storage, recall, and updating of - * calibration data using the ASH (Android Sensor Hub) API for dynamic runtime + * This module provides a containing class (NanoSensorCal) for dynamic runtime * calibration algorithms that affect the following sensors: * - Accelerometer (offset) * - Gyroscope (offset, with over-temperature compensation) @@ -28,21 +27,16 @@ * - Magnetometer [micro Tesla, uT] * - Temperature [Celsius]. * - * INPUTS: - * This module uses pointers to runtime calibration algorithm objects. - * These must be constructed and initialized outside of this class. The owner - * bears the burden of managing the lifetime of these objects with respect to - * the NanoSensorCal class which depends on these objects and handles their - * interaction with the Android ASH/CHRE system. This arrangement makes it - * convenient to abstract the specific algorithm implementations (i.e., choice - * of calibration algorithm, parameter tuning, etc.) at the nanoapp level - * without the need to specialize the standard functionality implemented here. + * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging. * - * OnlineCalibration<CalibrationDataThreeAxis> *online_cal - * Pointer to the sensor calibration algorithm that provides calibration - * updates. - * - * NOTE: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging. + * NOTE2: This module uses pointers to runtime calibration algorithm objects. + * These must be constructed and initialized outside of this class. The owner + * bares the burden of managing the lifetime of these objects with respect to + * the NanoSensorCal class which depends on these objects and handles their + * interaction with the Android ASH/CHRE system. This arrangement makes it + * convenient to modify the specific algorithm implementations (i.e., choice of + * calibration algorithm, parameter tuning, etc.) at the nanoapp level without + * the need to specialize the standard functionality implemented here. */ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ @@ -55,6 +49,7 @@ #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" @@ -64,10 +59,10 @@ namespace nano_calibration { /* - * NanoSensorCal is a helper class for dynamic runtime calibration sensor + * NanoSensorCal is a container class for dynamic runtime calibration sensor * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class - * is to manage sensor calibration data persistence (storage & recall), and to - * provide calibration updates to CHRE using the ASH API. + * is to transfer sensor data to the sensor calibration algorithms and provide + * calibration updates to CHRE using the ASH API. */ class NanoSensorCal { public: @@ -79,56 +74,94 @@ class NanoSensorCal { NanoSensorCal() = default; - // Provides ASH calibration updates using the sensor calibration associated - // with the 'online_cal' algorithm. The input bit mask 'new_cal_flags' - // describe the new calibration elements to update. - void UpdateCalibration(online_calibration::CalibrationTypeFlags new_cal_flags, - const OnlineCalibrationThreeAxis &online_cal); + // Sets the sensor calibration object pointers and initializes the algorithms + // using runtime values recalled using Android Sensor Hub (ASH). A nullptr may + // be passed in to disable a particular sensor calibration. + void Initialize(OnlineCalibrationThreeAxis *accel_cal, + OnlineCalibrationThreeAxis *gyro_cal, + OnlineCalibrationThreeAxis *mag_cal); + + // Sends new sensor samples to the calibration algorithms. + void HandleSensorSamples(uint16_t event_type, + const chreSensorThreeAxisData *event_data); - // Loads runtime calibration data from the system registry using ASH. This is - // usually called once whenever the owning runtime calibration algorithm is - // initialized. - void LoadAshCalibration(OnlineCalibrationThreeAxis *online_cal); + // Provides temperature updates to the calibration algorithms. + void HandleTemperatureSamples(uint16_t event_type, + const chreSensorFloatData *event_data); - // Sets the pointer to a calibration result logger. 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); + + // Loads runtime calibration data using the Android Sensor Hub API. Returns + // 'true' when runtime calibration values were successfully recalled and used + // for algorithm initialization. 'sensor_tag' is a string that identifies a + // sensor-specific identifier for log messages. Updates 'flags' to indicate + // which runtime calibration parameters were recalled. + bool LoadAshCalibration(uint8_t chreSensorType, + OnlineCalibrationThreeAxis *online_cal, + online_calibration::CalibrationTypeFlags *flags, + const char *sensor_tag); + // Provides sensor calibration updates using the ASH API for the specified // sensor type. 'cal_data' contains the new calibration data. 'flags' is used // to indicate all of the valid calibration values that should be provided // with the update. Returns 'true' with a successful ASH update. bool NotifyAshCalibration( - uint8_t chre_sensor_type, uint8_t sensor_index, uint8_t calibration_index, + uint8_t chreSensorType, const online_calibration::CalibrationDataThreeAxis &cal_data, - online_calibration::CalibrationTypeFlags flags, char const *sensor_tag); + online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); // Checks whether 'ash_cal_parameters' is a valid set of runtime calibration // data and can be used for algorithm initialization. Updates 'flags' to - // indicate which runtime calibration parameters were detected. Returns true - // if valid runtime calibration data is detected and may be used. - bool DetectRuntimeCalibration( - uint8_t chre_sensor_type, const char *sensor_tag, uint8_t sensor_index, - uint8_t calibration_index, const ashCalParams &ash_cal_parameters, - online_calibration::CalibrationTypeFlags &flags); + // indicate which runtime calibration parameters were detected. + bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag, + online_calibration::CalibrationTypeFlags *flags, + ashCalParams *ash_cal_parameters); // Helper functions for logging calibration information. + void PrintAshCalParams(const ashCalParams &cal_params, + const char *sensor_tag); + void PrintCalibration( const online_calibration::CalibrationDataThreeAxis &cal_data, - uint8_t sensor_index, uint8_t calibration_index, online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); - bool IsGyroLogUpdateAllowed(uint64_t timestamp_nanos); + bool HandleGyroLogMessage(uint64_t timestamp_nanos); + + // Pointer to the accelerometer runtime calibration object. + OnlineCalibrationThreeAxis *accel_cal_ = nullptr; + + // Pointer to the gyroscope runtime calibration object. + OnlineCalibrationThreeAxis *gyro_cal_ = nullptr; // 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 initial_gyro_cal_time_nanos_ = 0; - - // Pointer to a calibration result logger (e.g., telemetry). + uint64_t initialization_start_time_nanos_ = 0; + + // Pointer to the magnetometer runtime calibration object. + OnlineCalibrationThreeAxis *mag_cal_ = nullptr; + + // Flags that determine which calibration elements are updated with the ASH + // API. These are reset during initialization, and latched when a particular + // calibration update is detected upon a valid recall of parameters and/or + // during runtime. The latching behavior is used to start sending calibration + // values of a given type (e.g., bias, over-temp model, etc.) once they are + // detected and thereafter. + online_calibration::CalibrationTypeFlags accel_cal_update_flags_ = + online_calibration::CalibrationTypeFlags::NONE; + online_calibration::CalibrationTypeFlags gyro_cal_update_flags_ = + 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; }; 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 11cf292e..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 @@ -63,11 +63,6 @@ class AccelOffsetCal final bool SetInitialCalibration( const CalibrationDataThreeAxis& input_cal_data) final; - // Indicates which values are modified by this calibration algorithm. - CalibrationTypeFlags which_calibration_flags() const final { - return CalibrationTypeFlags::BIAS; - } - // Returns the calibration sensor type. SensorType get_sensor_type() const final { return SensorType::kAccelerometerMps2; diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h index d9e5bd7e..d6475c78 100644 --- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h @@ -64,8 +64,8 @@ enum class CalibrationQualityLevel : uint8_t { // Sets the calibration quality value when this metric is either not // implemented, or has not yet been determined (e.g., a calibration hasn't -// occurred). Represented with an arbitrarily large value. -constexpr float kUndeterminedCalibrationQuality = 1.0e9f; +// occurred). +constexpr float kUndeterminedCalibrationQuality = -1.0f; /* * Calibration quality structure that contains a quantitative (float) and diff --git a/firmware/os/algos/calibration/online_calibration/common_data/config_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/config_callback.h deleted file mode 100644 index de122801..00000000 --- a/firmware/os/algos/calibration/online_calibration/common_data/config_callback.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CONFIG_CALLBACK_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CONFIG_CALLBACK_H_ - -#include <cstdint> - -namespace online_calibration { - -// Context-dependent configuration change types. -enum class ConfigChangeType : uint8_t { - kNoConfigChange = 0, - kMagGyroActiveMode, - kMagGyroPassiveMode, - kGyroActiveMode, - kGyroPassiveMode, - kNumConfigChangeTypes, -}; - -// Callback interface for changing an algorithm specific configuration (e.g., -// sensor subscription properties, etc.). -class ConfigCallback { - protected: - // Protected destructor. The implementation can destroy itself, it can't be - // destroyed through this interface. - virtual ~ConfigCallback() = default; - - public: - /* - * Override this method to allow calibration objects to trigger changes in - * sensor configurations or operational states (e.g., upon device motion the - * MagGyroCal may switch to higher-rate gyroscope and magnetometer sampling - * rates to produce a new calibration result). - * - * config_type: This enumerator indicates what configuration change must be - * made, the owner of the calibration object will make the - * appropriate platform dependent changes. - * - * sensor_index: The calibration algorithm will provide a sensor index to help - * uniquely identify the sensor it calibrates. This can be used - * to disambiguate what platform specific configuration response - * should be taken. - */ - virtual void UpdateConfiguration(ConfigChangeType config_type, - uint8_t sensor_index) = 0; -}; - -} // namespace online_calibration - -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CONFIG_CALLBACK_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h index 710c442c..59e26bae 100644 --- a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h +++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h @@ -17,8 +17,7 @@ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ -#include <cstdint> -#include <cstring> +#include <string.h> #include "calibration/online_calibration/common_data/calibration_callback.h" #include "calibration/online_calibration/common_data/calibration_data.h" @@ -26,14 +25,6 @@ namespace online_calibration { -// Device physical state change types. -enum class PhysicalStateType : uint8_t { - kUnknownPhysicalState = 0, - kFoldableOpen, - kFoldableClosed, - kNumPhysicalStateTypes, -}; - /* * This abstract base class provides a set of general interface functions for * calibration algorithms. The data structures used are intended to be lean and @@ -71,29 +62,10 @@ class OnlineCalibration { // of the calibration update flags, 'cal_update_polling_flags_'. virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0; - // In a multisensor context, 'sensor_index' is used to disambiguate the origin - // of the input sensor data (e.g., useful for separating multiple magnetometer - // data streams). The default implementation resorts to the above - // SetMeasurement implementation provided by each calibration algorithm. - // SetMultiSensorMeasurement can be overridden to do the special multisensor - // handling when applicable. - virtual CalibrationTypeFlags SetMultiSensorMeasurement( - const SensorData& sample, uint8_t sensor_index) { - return SetMeasurement(sample); - } - // Sets the initial calibration data of the calibration algorithm. Returns // "true" if set successfully. virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0; - // Indicates which values are modified by this calibration algorithm. - virtual CalibrationTypeFlags which_calibration_flags() const = 0; - - // Optional function used by calibration algorithms to maintain awareness of - // of sensor enable states. - virtual void UpdateSensorEnableState(SensorType sensor_type, - uint8_t sensor_index, bool is_enabled) {} - // Polling Updates: New calibration updates are generated during // SetMeasurement and the 'cal_update_polling_flags_' are set according to // which calibration values have changed. To prevent missing updates in @@ -119,32 +91,9 @@ class OnlineCalibration { calibration_callback_ = calibration_callback; } - // Sets a platform-dependent sensor index that can be used to associate - // calibration data with a particular sensor. - void set_sensor_index(uint8_t sensor_index) { sensor_index_ = sensor_index; } - - // Returns the platform-dependent sensor index. - uint8_t get_sensor_index() const { return sensor_index_; } - - // Sets a platform-dependent calibration index that can be used to - // associate more than one distinct calibration data with a particular sensor. - void set_calibration_index(uint8_t calibration_index) { - calibration_index_ = calibration_index; - } - - // Returns the platform-dependent sensor index. - uint8_t get_calibration_index() const { return calibration_index_; } - // Returns the sensor-type this calibration algorithm provides updates for. virtual SensorType get_sensor_type() const = 0; - // Tells the calibrator that the device's physical state has changed. This is - // useful, for example, if there is a need for the calibration algorithm to be - // aware of and take some sort of internal action in response to a physical - // state change (e.g., for foldable devices, MagCal may adjust internal states - // to implement specific transition behavior between open/closed states). - virtual void UpdatePhysicalState(PhysicalStateType physical_state) {} - protected: // Helper function that activates the registered callback. void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const { @@ -163,10 +112,6 @@ class OnlineCalibration { // Stores the sensor calibration data. CalibrationType cal_data_; - // Associated sensor and calibration indices. - uint8_t sensor_index_ = 0; - uint8_t calibration_index_ = 0; - // Tracks the most recent sensor temperature value. float temperature_celsius_ = kInvalidTemperatureCelsius; 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 index 585f67fd..ca54f2fb 100644 --- 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 @@ -20,19 +20,11 @@ class ResultCallbackInterface { // 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. - // sensor_index: Platform-dependent index that identifies the sensor (useful - // for devices with more than one sensor type). - // calibration_index: Platform-dependent index that identifies the calibration - // value that is being applied (distinct calibrations may - // be utilized according to physical state [e.g., different - // magnetometer biases may be required for the open/closed - // states of a foldable device]). // 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, - uint8_t sensor_index, uint8_t calibration_index, - CalibrationTypeFlags flags, const CalibrationDataThreeAxis &cal_data) = 0; + virtual void SetCalibrationEvent(uint64_t event_timestamp_nanos, + SensorType sensor_type, + CalibrationTypeFlags flags) = 0; }; } // namespace online_calibration 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 b6d6eb37..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 @@ -48,8 +48,6 @@ enum class SensorType : int8_t { kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal). kWifiM = 6, // 3-axis sensor (units = meter). kProximity = 7, // 1-axis sensor (units = ?). - kHallEffect = 8, // 1-axis sensor (units = ?). - kHingeAngle = 9, // 1-axis sensor (units = degrees). }; // 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 f373ce41..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 @@ -28,7 +28,6 @@ constexpr float GyroOffsetOtcCal::kHighQualityRps; void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters, const OverTempCalParameters& otc_parameters) { - gyro_is_enabled_ = true; gyroCalInit(&gyro_cal_, &gyro_cal_parameters); overTempCalInit(&over_temp_cal_, &otc_parameters); InitializeCalData(); @@ -36,17 +35,6 @@ void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters, CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( const SensorData& sample) { - // Bypass calibration data process and updates when the gyro sensor is not - // enabled. - if (!gyro_is_enabled_) { - // Tracks any updates in temperature. - if (sample.type == SensorType::kTemperatureCelsius) { - temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; - } - - return CalibrationTypeFlags::NONE; - } - // Routes the input sensor sample to the calibration algorithm. switch (sample.type) { case SensorType::kAccelerometerMps2: @@ -90,12 +78,9 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( uint64_t calibration_time_nanos = 0; gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2], &temperature_celsius, &calibration_time_nanos); - - if (temperature_celsius != kInvalidTemperatureCelsius) { - overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos, - offset, temperature_celsius); - cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS; - } + 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. @@ -193,13 +178,4 @@ bool GyroOffsetOtcCal::SetInitialCalibration( return true; } -void GyroOffsetOtcCal::UpdateSensorEnableState(SensorType sensor_type, - uint8_t sensor_index, - bool is_enabled) { - if (sensor_type == SensorType::kGyroscopeRps && - sensor_index_ == sensor_index) { - gyro_is_enabled_ = is_enabled; - } -} - } // namespace online_calibration 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 5142a151..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 @@ -69,12 +69,6 @@ class GyroOffsetOtcCal final bool SetInitialCalibration( const CalibrationDataThreeAxis& input_cal_data) final; - // Indicates which values are modified by this calibration algorithm. - CalibrationTypeFlags which_calibration_flags() const final { - return CalibrationTypeFlags::BIAS | CalibrationTypeFlags::OVER_TEMP | - CalibrationTypeFlags::OTC_STILL_BIAS; - } - // Returns the calibration sensor type. SensorType get_sensor_type() const final { return SensorType::kGyroscopeRps; @@ -84,20 +78,12 @@ class GyroOffsetOtcCal final const GyroCal& get_gyro_cal() const { return gyro_cal_; } const OverTempCal& get_over_temp_cal() const { return over_temp_cal_; } - // Optional function used by calibration algorithms to maintain awareness of - // of sensor enable states. - void UpdateSensorEnableState(SensorType sensor_type, uint8_t sensor_index, - bool is_enabled) final; - private: // GyroCal algorithm data structure. GyroCal gyro_cal_; // Over-temperature offset compensation algorithm data structure. OverTempCal over_temp_cal_; - - // Tracks the gyro sensor enable state. - bool gyro_is_enabled_; }; } // namespace online_calibration 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 1b6f416f..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 @@ -66,11 +66,6 @@ class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> { bool SetInitialCalibration( const CalibrationDataThreeAxis& input_cal_data) final; - // Indicates which values are modified by this calibration algorithm. - CalibrationTypeFlags which_calibration_flags() const final { - return CalibrationTypeFlags::BIAS; - } - // Returns the calibration sensor type. SensorType get_sensor_type() const final { return SensorType::kMagnetometerUt; |