[automerger skipped] Merge Android 12 am: e681c66b42 -s ours am: 95b1801fa2 -s ours am: 2e37a4dbda -s ours am: b34923fb47 -s ours
am skip reason: Merged-In Id5f58e8cb24bcba19ec2253a9ca5d0703a54b43b with SHA-1 8c87763c0a is already in history
Original change: https://android-review.googlesource.com/c/device/google/contexthub/+/1847157
Change-Id: I28e1ee90edc68790017712d41ffbcd74ac67ae47
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index 122af48..9d8e04d 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 {
// Common log message sensor-specific identifiers.
-constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
-constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
-constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+char const *kAccelTag = "[ACCEL_MPS2]";
+char const *kGyroTag = "[GYRO_RPS]";
+char const *kMagTag = "[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,8 +47,6 @@
using ::online_calibration::CalibrationDataThreeAxis;
using ::online_calibration::CalibrationTypeFlags;
-using ::online_calibration::SensorData;
-using ::online_calibration::SensorIndex;
using ::online_calibration::SensorType;
// NanoSensorCal logging macros.
@@ -56,6 +54,24 @@
#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__)
@@ -74,190 +90,83 @@
#define NANO_CAL_LOGI(tag, format, ...) \
TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__)
-} // namespace
+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();
-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;
+ switch (online_cal.get_sensor_type()) {
+ case SensorType::kAccelerometerMps2:
+ *chre_sensor_type = CHRE_SENSOR_TYPE_ACCELEROMETER;
+ *sensor_tag = kAccelTag;
break;
- case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
- sample.type = SensorType::kGyroscopeRps;
+ case SensorType::kGyroscopeRps:
+ *chre_sensor_type = CHRE_SENSOR_TYPE_GYROSCOPE;
+ *sensor_tag = kGyroTag;
break;
- case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
- sample.type = SensorType::kMagnetometerUt;
+ case SensorType::kMagnetometerUt:
+ *chre_sensor_type = CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD;
+ *sensor_tag = kMagTag;
break;
default:
- // This sensor type is not used.
- NANO_CAL_LOGW("[NanoSensorCal]",
- "Unexpected 3-axis sensor type received.");
- return;
+ NANO_CAL_LOGW("[NanoSensorCal]", "Unexpected sensor calibration (%d).",
+ static_cast<int>(online_cal.get_sensor_type()));
+ return false;
}
-
- // 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;
- }
+ return true;
}
-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;
+} // namespace
- 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 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 (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 (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_);
- }
+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);
}
}
}
bool NanoSensorCal::NotifyAshCalibration(
- uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
- CalibrationTypeFlags flags, const char *sensor_tag) {
+ 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;
+ }
+
+ if (is_log_update_allowed) {
+ PrintCalibration(cal_data, sensor_index, calibration_index, flags,
+ sensor_tag);
+ }
+
+ 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);
+ }
+
// Updates the sensor offset calibration using the ASH API.
ashCalInfo ash_cal_info;
memset(&ash_cal_info, 0, sizeof(ashCalInfo));
@@ -285,7 +194,8 @@
break;
}
- if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
+ if (!ashSetMultiCalibration(chre_sensor_type, sensor_index, calibration_index,
+ &ash_cal_info)) {
NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
return false;
}
@@ -311,7 +221,8 @@
ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
}
- if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
+ if (!ashSaveMultiCalibrationParams(chre_sensor_type, sensor_index,
+ calibration_index, &ash_cal_parameters)) {
NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
return false;
}
@@ -319,91 +230,97 @@
return true;
}
-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();
+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 = {};
- // 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));
- }
+ // Resets the rate limiter for gyro calibration update messages.
+ initial_gyro_cal_time_nanos_ = 0;
- 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 {
+ 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)) {
// 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;
}
- return false;
+ // 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.");
+ }
+ }
}
-bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType,
- const char *sensor_tag,
- CalibrationTypeFlags *flags,
- ashCalParams *ash_cal_parameters) {
+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) {
// 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_LOGI(sensor_tag, "Runtime calibration data detected.");
- PrintAshCalParams(*ash_cal_parameters, sensor_tag);
+ NANO_CAL_LOGD(sensor_tag, "Runtime calibration data detected.");
} else {
// This is a warning (not an error) since the runtime algorithms will
// function correctly with no recalled calibration values. They will
@@ -414,82 +331,55 @@
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,
- "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
- cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
- cal_data.offset_temp_celsius);
+ 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));
}
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]);
+ 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]);
}
}
-bool NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) {
+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;
+ }
+
// 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_,
+ /*t1=*/timestamp_nanos, /*t2=*/initial_gyro_cal_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);
+ 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);
- if (print_gyro_log) {
+ if (is_log_update_allowed) {
gyro_notification_time_nanos_ = timestamp_nanos;
- PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_,
- kGyroTag);
}
- return print_gyro_log;
+ return is_log_update_allowed;
}
} // 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 82a8396..dd1f52a 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -15,7 +15,8 @@
*/
/*
- * This module provides a containing class (NanoSensorCal) for dynamic runtime
+ * This module provides a helper class for storage, recall, and updating of
+ * calibration data using the ASH (Android Sensor Hub) API for dynamic runtime
* calibration algorithms that affect the following sensors:
* - Accelerometer (offset)
* - Gyroscope (offset, with over-temperature compensation)
@@ -27,16 +28,21 @@
* - Magnetometer [micro Tesla, uT]
* - Temperature [Celsius].
*
- * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging.
+ * 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.
*
- * 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.
+ * 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.
*/
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
@@ -49,7 +55,6 @@
#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"
@@ -59,10 +64,10 @@
namespace nano_calibration {
/*
- * NanoSensorCal is a container class for dynamic runtime calibration sensor
+ * NanoSensorCal is a helper class for dynamic runtime calibration sensor
* algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
- * is to transfer sensor data to the sensor calibration algorithms and provide
- * calibration updates to CHRE using the ASH API.
+ * is to manage sensor calibration data persistence (storage & recall), and to
+ * provide calibration updates to CHRE using the ASH API.
*/
class NanoSensorCal {
public:
@@ -74,94 +79,56 @@
NanoSensorCal() = default;
- // 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);
+ // 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);
- // 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 chreSensorType,
+ uint8_t chre_sensor_type, uint8_t sensor_index, uint8_t calibration_index,
const online_calibration::CalibrationDataThreeAxis &cal_data,
- online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+ online_calibration::CalibrationTypeFlags flags, char const *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.
- bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag,
- online_calibration::CalibrationTypeFlags *flags,
- ashCalParams *ash_cal_parameters);
+ // 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);
// 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 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;
+ bool IsGyroLogUpdateAllowed(uint64_t timestamp_nanos);
// 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;
+ uint64_t initial_gyro_cal_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.
+ // Pointer to a calibration result logger (e.g., telemetry).
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 60b59df..11cf292 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,6 +63,11 @@
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 d6475c7..d9e5bd7 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 @@
// 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).
-constexpr float kUndeterminedCalibrationQuality = -1.0f;
+// occurred). Represented with an arbitrarily large value.
+constexpr float kUndeterminedCalibrationQuality = 1.0e9f;
/*
* 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
new file mode 100644
index 0000000..de12280
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/config_callback.h
@@ -0,0 +1,48 @@
+#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 59e26ba..710c442 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,7 +17,8 @@
#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 <string.h>
+#include <cstdint>
+#include <cstring>
#include "calibration/online_calibration/common_data/calibration_callback.h"
#include "calibration/online_calibration/common_data/calibration_data.h"
@@ -25,6 +26,14 @@
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
@@ -62,10 +71,29 @@
// 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
@@ -91,9 +119,32 @@
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 {
@@ -112,6 +163,10 @@
// 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 ca54f2f..585f67f 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,11 +20,19 @@
// 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,
- CalibrationTypeFlags flags) = 0;
+ 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;
};
} // 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 23b63c9..b6d6eb3 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,6 +48,8 @@
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 02c2670..f373ce4 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,6 +28,7 @@
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();
@@ -35,6 +36,17 @@
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:
@@ -78,9 +90,12 @@
uint64_t calibration_time_nanos = 0;
gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
&temperature_celsius, &calibration_time_nanos);
- overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
- offset, temperature_celsius);
- cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS;
+
+ if (temperature_celsius != kInvalidTemperatureCelsius) {
+ 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.
@@ -178,4 +193,13 @@
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 9b8962a..5142a15 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,6 +69,12 @@
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;
@@ -78,12 +84,20 @@
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 9b60a7f..1b6f416 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,6 +66,11 @@
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;
