/* * Copyright (C) 2017 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "calibration/nano_calibration/nano_calibration.h" #include #include #include "common/techeng_log_util.h" 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]"}; // 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 // appear at a rate set by 'rapid_message_interval_sec'. Afterwards, log // messages will be produced at a rate determined by // 'slow_message_interval_min'. struct LogMessageRegimen { uint8_t rapid_message_interval_sec; // Assists device verification. uint8_t slow_message_interval_min; // Avoids long-term log spam. uint8_t duration_of_rapid_messages_min; }; constexpr LogMessageRegimen kGyroscopeMessagePlan = { /*rapid_message_interval_sec*/ 20, /*slow_message_interval_min*/ 5, /*duration_of_rapid_messages_min*/ 3}; using ::online_calibration::CalibrationDataThreeAxis; using ::online_calibration::CalibrationTypeFlags; using ::online_calibration::SensorData; using ::online_calibration::SensorIndex; using ::online_calibration::SensorType; // NanoSensorCal logging macros. #ifndef LOG_TAG #define LOG_TAG "[ImuCal]" #endif #ifdef NANO_SENSOR_CAL_DBG_ENABLED #define NANO_CAL_LOGD(tag, format, ...) \ TECHENG_LOGD("%s " format, tag, ##__VA_ARGS__) #define NANO_CAL_LOGW(tag, format, ...) \ TECHENG_LOGW("%s " format, tag, ##__VA_ARGS__) #define NANO_CAL_LOGE(tag, format, ...) \ TECHENG_LOGE("%s " format, tag, ##__VA_ARGS__) #else #define NANO_CAL_LOGD(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) #define NANO_CAL_LOGW(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) #define NANO_CAL_LOGE(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) #endif // NANO_SENSOR_CAL_DBG_ENABLED // NOTE: LOGI is defined to ensure calibration updates are always logged for // field diagnosis and verification. #define NANO_CAL_LOGI(tag, format, ...) \ 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; break; case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA: sample.type = SensorType::kGyroscopeRps; break; case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: sample.type = SensorType::kMagnetometerUt; break; default: // This sensor type is not used. NANO_CAL_LOGW("[NanoSensorCal]", "Unexpected 3-axis sensor type received."); return; } // Sends the sensor payload to the calibration algorithms and checks for // calibration updates. const auto &header = event_data->header; const auto *data = event_data->readings; sample.timestamp_nanos = header.baseTimestamp; for (size_t i = 0; i < header.readingCount; i++) { sample.timestamp_nanos += data[i].timestampDelta; memcpy(sample.data, data[i].v, sizeof(sample.data)); ProcessSample(sample); } // 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::HandleTemperatureSamples( uint16_t event_type, const chreSensorFloatData *event_data) { // Computes the mean of the batched temperature samples and delivers it to the // calibration algorithms. Note, the temperature sensor batch size determines // its minimum update interval. if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA && event_data->header.readingCount > 0) { const auto header = event_data->header; const auto *data = event_data->readings; SensorData sample; sample.type = SensorType::kTemperatureCelsius; sample.timestamp_nanos = header.baseTimestamp; float accum_temperature_celsius = 0.0f; for (size_t i = 0; i < header.readingCount; i++) { sample.timestamp_nanos += data[i].timestampDelta; accum_temperature_celsius += data[i].value; } sample.data[SensorIndex::kSingleAxis] = accum_temperature_celsius / header.readingCount; ProcessSample(sample); } else { NANO_CAL_LOGW("[NanoSensorCal]", "Unexpected single-axis sensor type received."); } } void NanoSensorCal::ProcessSample(const SensorData &sample) { // Sends a new sensor sample to each active calibration algorithm and sends // out notifications for new calibration updates. if (accel_cal_ != nullptr) { const CalibrationTypeFlags 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_); } } } } bool NanoSensorCal::NotifyAshCalibration( uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data, CalibrationTypeFlags flags, const char *sensor_tag) { // Updates the sensor offset calibration using the ASH API. ashCalInfo ash_cal_info; memset(&ash_cal_info, 0, sizeof(ashCalInfo)); ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor). ash_cal_info.compMatrix[4] = 1.0f; ash_cal_info.compMatrix[8] = 1.0f; memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias)); // Maps CalibrationQualityLevel to ASH calibration accuracy. switch (cal_data.calibration_quality.level) { case online_calibration::CalibrationQualityLevel::HIGH_QUALITY: ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH; break; case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY: ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM; break; case online_calibration::CalibrationQualityLevel::LOW_QUALITY: ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW; break; default: ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE; break; } if (!ashSetCalibration(chreSensorType, &ash_cal_info)) { NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update."); return false; } // Uses the ASH API to store all calibration parameters relevant to a given // algorithm as indicated by the input calibration type flags. ashCalParams ash_cal_parameters; memset(&ash_cal_parameters, 0, sizeof(ashCalParams)); if (flags & CalibrationTypeFlags::BIAS) { ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius; memcpy(ash_cal_parameters.offset, cal_data.offset, sizeof(ash_cal_parameters.offset)); ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; } if (flags & CalibrationTypeFlags::OVER_TEMP) { memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity, sizeof(ash_cal_parameters.tempSensitivity)); memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept, sizeof(ash_cal_parameters.tempIntercept)); ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME; ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; } if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) { NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update."); return false; } return true; } bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, OnlineCalibrationThreeAxis *online_cal, 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 false; } 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. // 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 == ASH_CAL_PARAMS_SOURCE_RUNTIME) { runtime_cal_detected = true; *flags = CalibrationTypeFlags::BIAS; } if (ash_cal_parameters->tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME && ash_cal_parameters->tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { *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); } else { // This is a warning (not an error) since the runtime algorithms will // function correctly with no recalled calibration values. They will // eventually trigger and update the system with valid calibration data. NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found."); } return runtime_cal_detected; } // Helper functions for logging calibration information. void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params, const char *sensor_tag) { 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, CalibrationTypeFlags flags, const char *sensor_tag) { if (flags & CalibrationTypeFlags::BIAS) { NANO_CAL_LOGI(sensor_tag, "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f", cal_data.offset[0], cal_data.offset[1], cal_data.offset[2], cal_data.offset_temp_celsius); } if (flags & CalibrationTypeFlags::OVER_TEMP) { NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f", cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1], cal_data.temp_sensitivity[2]); NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f", cal_data.temp_intercept[0], cal_data.temp_intercept[1], cal_data.temp_intercept[2]); } } 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( 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 print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( timestamp_nanos, gyro_notification_time_nanos_, next_log_interval_nanos); if (print_gyro_log) { gyro_notification_time_nanos_ = timestamp_nanos; PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, kGyroTag); } return print_gyro_log; } } // namespace nano_calibration