diff options
Diffstat (limited to 'firmware/os/algos/calibration/nano_calibration/nano_calibration.cc')
| -rw-r--r-- | firmware/os/algos/calibration/nano_calibration/nano_calibration.cc | 448 |
1 files changed, 160 insertions, 288 deletions
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc index 122af489..024a031d 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. -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 @@ 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. @@ -74,190 +72,83 @@ using ::online_calibration::SensorType; #define NANO_CAL_LOGI(tag, format, ...) \ TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__) -} // 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; +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; 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; - } - - // 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; + NANO_CAL_LOGW("[NanoSensorCal]", "Unexpected sensor calibration (%d).", + static_cast<int>(online_cal.get_sensor_type())); + return false; } + 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; - - 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; +} // namespace + +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); } - 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_); - } - } +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; } - 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 (is_log_update_allowed) { + PrintCalibration(cal_data, sensor_index, calibration_index, flags, + sensor_tag); } - 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_); - } - } + if (result_callback_ != nullptr && send_results_callback) { + result_callback_->SetCalibrationEvent(cal_data.cal_update_time_nanos, + cal_data.type, sensor_index, + calibration_index, 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)); @@ -285,7 +176,8 @@ bool NanoSensorCal::NotifyAshCalibration( 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 +203,8 @@ bool NanoSensorCal::NotifyAshCalibration( 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 +212,97 @@ bool NanoSensorCal::NotifyAshCalibration( 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(); - - // 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 { +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)) { // 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 +313,55 @@ bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, 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 |