1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "calibration/nano_calibration/nano_calibration.h" 18 19 #include <cstring> 20 21 #include "chre/util/nanoapp/log.h" 22 23 namespace nano_calibration { 24 namespace { 25 26 using ::online_calibration::CalibrationDataThreeAxis; 27 using ::online_calibration::CalibrationTypeFlags; 28 using ::online_calibration::SensorData; 29 using ::online_calibration::SensorIndex; 30 using ::online_calibration::SensorType; 31 32 // NanoSensorCal logging macros. 33 #ifdef NANO_SENSOR_CAL_DBG_ENABLED 34 #ifndef LOG_TAG 35 #define LOG_TAG "[ImuCal]" 36 #endif 37 #define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__) 38 #define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__) 39 #define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__) 40 #define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__) 41 #else 42 #define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) 43 #define NANO_CAL_LOGI(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) 44 #define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) 45 #define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) 46 #endif // NANO_SENSOR_CAL_DBG_ENABLED 47 48 } // namespace 49 50 void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal, 51 OnlineCalibrationThreeAxis *gyro_cal, 52 OnlineCalibrationThreeAxis *mag_cal) { 53 // Loads stored calibration data and initializes the calibration algorithms. 54 accel_cal_ = accel_cal; 55 if (accel_cal_ != nullptr) { 56 if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) { 57 LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_, 58 &accel_cal_update_flags_, kAccelTag); 59 NANO_CAL_LOGI(kAccelTag, 60 "Accelerometer runtime calibration initialized."); 61 } else { 62 accel_cal_ = nullptr; 63 NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type."); 64 } 65 } 66 67 gyro_cal_ = gyro_cal; 68 if (gyro_cal_ != nullptr) { 69 if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) { 70 LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, 71 &gyro_cal_update_flags_, kGyroTag); 72 NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized."); 73 } else { 74 gyro_cal_ = nullptr; 75 NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type."); 76 } 77 } 78 79 mag_cal_ = mag_cal; 80 if (mag_cal != nullptr) { 81 if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) { 82 LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_, 83 &mag_cal_update_flags_, kMagTag); 84 NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized."); 85 } else { 86 mag_cal_ = nullptr; 87 NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type."); 88 } 89 } 90 } 91 92 void NanoSensorCal::HandleSensorSamples( 93 uint16_t event_type, const chreSensorThreeAxisData *event_data) { 94 // Converts CHRE Event -> SensorData::SensorType. 95 SensorData sample; 96 switch (event_type) { 97 case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA: 98 sample.type = SensorType::kAccelerometerMps2; 99 break; 100 case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA: 101 sample.type = SensorType::kGyroscopeRps; 102 break; 103 case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: 104 sample.type = SensorType::kMagnetometerUt; 105 break; 106 default: 107 // This sensor type is not used. 108 NANO_CAL_LOGW("[NanoSensorCal]", 109 "Unexpected 3-axis sensor type received."); 110 return; 111 } 112 113 // Sends the sensor payload to the calibration algorithms and checks for 114 // calibration updates. 115 const auto &header = event_data->header; 116 const auto *data = event_data->readings; 117 sample.timestamp_nanos = header.baseTimestamp; 118 for (size_t i = 0; i < header.readingCount; i++) { 119 sample.timestamp_nanos += data[i].timestampDelta; 120 memcpy(sample.data, data[i].v, sizeof(sample.data)); 121 ProcessSample(sample); 122 } 123 } 124 125 void NanoSensorCal::HandleTemperatureSamples( 126 uint16_t event_type, const chreSensorFloatData *event_data) { 127 // Computes the mean of the batched temperature samples and delivers it to the 128 // calibration algorithms. Note, the temperature sensor batch size determines 129 // its minimum update interval. 130 if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA && 131 event_data->header.readingCount > 0) { 132 const auto header = event_data->header; 133 const auto *data = event_data->readings; 134 135 SensorData sample; 136 sample.type = SensorType::kTemperatureCelsius; 137 sample.timestamp_nanos = header.baseTimestamp; 138 139 float accum_temperature_celsius = 0.0f; 140 for (size_t i = 0; i < header.readingCount; i++) { 141 sample.timestamp_nanos += data[i].timestampDelta; 142 accum_temperature_celsius += data[i].value; 143 } 144 sample.data[SensorIndex::kSingleAxis] = 145 accum_temperature_celsius / header.readingCount; 146 ProcessSample(sample); 147 } else { 148 NANO_CAL_LOGW("[NanoSensorCal]", 149 "Unexpected single-axis sensor type received."); 150 } 151 } 152 153 void NanoSensorCal::ProcessSample(const SensorData &sample) { 154 // Sends a new sensor sample to each active calibration algorithm and sends 155 // out notifications for new calibration updates. 156 if (accel_cal_ != nullptr) { 157 const CalibrationTypeFlags new_cal_flags = 158 accel_cal_->SetMeasurement(sample); 159 if (new_cal_flags != CalibrationTypeFlags::NONE) { 160 accel_cal_update_flags_ |= new_cal_flags; 161 NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, 162 accel_cal_->GetSensorCalibration(), 163 accel_cal_update_flags_, kAccelTag); 164 PrintCalibration(accel_cal_->GetSensorCalibration(), 165 accel_cal_update_flags_, kAccelTag); 166 } 167 } 168 169 if (gyro_cal_ != nullptr) { 170 const CalibrationTypeFlags new_cal_flags = 171 gyro_cal_->SetMeasurement(sample); 172 if (new_cal_flags != CalibrationTypeFlags::NONE) { 173 gyro_cal_update_flags_ |= new_cal_flags; 174 if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, 175 gyro_cal_->GetSensorCalibration(), 176 gyro_cal_update_flags_, kGyroTag)) { 177 // Limits the log messaging update rate for the gyro calibrations since 178 // these can occur frequently with rapid temperature changes. 179 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 180 sample.timestamp_nanos, gyro_notification_time_nanos_, 181 kNanoSensorCalMessageIntervalNanos)) { 182 gyro_notification_time_nanos_ = sample.timestamp_nanos; 183 PrintCalibration(gyro_cal_->GetSensorCalibration(), 184 gyro_cal_update_flags_, kGyroTag); 185 } 186 } 187 } 188 } 189 190 if (mag_cal_ != nullptr) { 191 const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample); 192 if (new_cal_flags != CalibrationTypeFlags::NONE) { 193 mag_cal_update_flags_ |= new_cal_flags; 194 NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, 195 mag_cal_->GetSensorCalibration(), 196 mag_cal_update_flags_, kMagTag); 197 PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_, 198 kMagTag); 199 } 200 } 201 } 202 203 bool NanoSensorCal::NotifyAshCalibration( 204 uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data, 205 CalibrationTypeFlags flags, const char *sensor_tag) { 206 // Updates the sensor offset calibration using the ASH API. 207 ashCalInfo ash_cal_info; 208 memset(&ash_cal_info, 0, sizeof(ashCalInfo)); 209 ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor). 210 ash_cal_info.compMatrix[4] = 1.0f; 211 ash_cal_info.compMatrix[8] = 1.0f; 212 memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias)); 213 214 // Maps CalibrationQualityLevel to ASH calibration accuracy. 215 switch (cal_data.calibration_quality.level) { 216 case online_calibration::CalibrationQualityLevel::HIGH_QUALITY: 217 ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH; 218 break; 219 220 case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY: 221 ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM; 222 break; 223 224 case online_calibration::CalibrationQualityLevel::LOW_QUALITY: 225 ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW; 226 break; 227 228 default: 229 ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE; 230 break; 231 } 232 233 if (!ashSetCalibration(chreSensorType, &ash_cal_info)) { 234 NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update."); 235 return false; 236 } 237 238 // Uses the ASH API to store all calibration parameters relevant to a given 239 // algorithm as indicated by the input calibration type flags. 240 ashCalParams ash_cal_parameters; 241 memset(&ash_cal_parameters, 0, sizeof(ashCalParams)); 242 if (flags & CalibrationTypeFlags::BIAS) { 243 ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius; 244 memcpy(ash_cal_parameters.offset, cal_data.offset, 245 sizeof(ash_cal_parameters.offset)); 246 ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; 247 ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; 248 } 249 250 if (flags & CalibrationTypeFlags::OVER_TEMP) { 251 memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity, 252 sizeof(ash_cal_parameters.tempSensitivity)); 253 memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept, 254 sizeof(ash_cal_parameters.tempIntercept)); 255 ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME; 256 ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; 257 } 258 259 if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) { 260 NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update."); 261 return false; 262 } 263 264 return true; 265 } 266 267 bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, 268 OnlineCalibrationThreeAxis *online_cal, 269 CalibrationTypeFlags* flags, 270 const char *sensor_tag) { 271 ashCalParams recalled_ash_cal_parameters; 272 if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH, 273 &recalled_ash_cal_parameters)) { 274 // Checks whether a valid set of runtime calibration parameters was received 275 // and can be used for initialization. 276 if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags, 277 &recalled_ash_cal_parameters)) { 278 CalibrationDataThreeAxis cal_data; 279 cal_data.type = online_cal->get_sensor_type(); 280 cal_data.cal_update_time_nanos = chreGetTime(); 281 282 // Analyzes the calibration flags and sets only the runtime calibration 283 // values that were received. 284 if (*flags & CalibrationTypeFlags::BIAS) { 285 cal_data.offset_temp_celsius = 286 recalled_ash_cal_parameters.offsetTempCelsius; 287 memcpy(cal_data.offset, recalled_ash_cal_parameters.offset, 288 sizeof(cal_data.offset)); 289 } 290 291 if (*flags & CalibrationTypeFlags::OVER_TEMP) { 292 memcpy(cal_data.temp_sensitivity, 293 recalled_ash_cal_parameters.tempSensitivity, 294 sizeof(cal_data.temp_sensitivity)); 295 memcpy(cal_data.temp_intercept, 296 recalled_ash_cal_parameters.tempIntercept, 297 sizeof(cal_data.temp_intercept)); 298 } 299 300 // Sets the algorithm's initial calibration data and notifies ASH to apply 301 // the recalled calibration data. 302 if (online_cal->SetInitialCalibration(cal_data)) { 303 return NotifyAshCalibration(chreSensorType, 304 online_cal->GetSensorCalibration(), *flags, 305 sensor_tag); 306 } else { 307 NANO_CAL_LOGE(sensor_tag, 308 "Calibration data failed to initialize algorithm."); 309 } 310 } 311 } else { 312 NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data."); 313 } 314 315 return false; 316 } 317 318 bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, 319 const char *sensor_tag, 320 CalibrationTypeFlags *flags, 321 ashCalParams *ash_cal_parameters) { 322 // Analyzes calibration source flags to determine whether runtime 323 // calibration values have been loaded and may be used for initialization. A 324 // valid runtime calibration source will include at least an offset. 325 *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags. 326 327 // Uses the ASH calibration source flags to set the appropriate 328 // CalibrationTypeFlags. These will be used to determine which values to copy 329 // from 'ash_cal_parameters' and provide to the calibration algorithms for 330 // initialization. 331 bool runtime_cal_detected = false; 332 if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME && 333 ash_cal_parameters->offsetTempCelsiusSource == 334 ASH_CAL_PARAMS_SOURCE_RUNTIME) { 335 runtime_cal_detected = true; 336 *flags = CalibrationTypeFlags::BIAS; 337 } 338 339 if (ash_cal_parameters->tempSensitivitySource == 340 ASH_CAL_PARAMS_SOURCE_RUNTIME && 341 ash_cal_parameters->tempInterceptSource == 342 ASH_CAL_PARAMS_SOURCE_RUNTIME) { 343 *flags |= CalibrationTypeFlags::OVER_TEMP; 344 } 345 346 if (runtime_cal_detected) { 347 // Prints the retrieved runtime calibration data. 348 NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected."); 349 PrintAshCalParams(*ash_cal_parameters, sensor_tag); 350 } else { 351 // This is a warning (not an error) since the runtime algorithms will 352 // function correctly with no recalled calibration values. They will 353 // eventually trigger and update the system with valid calibration data. 354 NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found."); 355 } 356 357 return runtime_cal_detected; 358 } 359 360 // Helper functions for logging calibration information. 361 void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params, 362 const char *sensor_tag) { 363 if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { 364 NANO_CAL_LOGI(sensor_tag, 365 "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f", 366 cal_params.offset[0], cal_params.offset[1], 367 cal_params.offset[2], cal_params.offsetTempCelsius); 368 } 369 370 if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { 371 NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f", 372 cal_params.tempSensitivity[0], cal_params.tempSensitivity[1], 373 cal_params.tempSensitivity[2]); 374 } 375 376 if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { 377 NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f", 378 cal_params.tempIntercept[0], cal_params.tempIntercept[1], 379 cal_params.tempIntercept[2]); 380 } 381 382 if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { 383 NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f", 384 cal_params.scaleFactor[0], cal_params.scaleFactor[1], 385 cal_params.scaleFactor[2]); 386 } 387 388 if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { 389 NANO_CAL_LOGI(sensor_tag, 390 "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f", 391 cal_params.crossAxis[0], cal_params.crossAxis[1], 392 cal_params.crossAxis[2]); 393 } 394 } 395 396 void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data, 397 CalibrationTypeFlags flags, 398 const char *sensor_tag) { 399 if (flags & CalibrationTypeFlags::BIAS) { 400 NANO_CAL_LOGI(sensor_tag, 401 "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f", 402 cal_data.offset[0], cal_data.offset[1], cal_data.offset[2], 403 cal_data.offset_temp_celsius); 404 } 405 406 if (flags & CalibrationTypeFlags::OVER_TEMP) { 407 NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f", 408 cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1], 409 cal_data.temp_sensitivity[2]); 410 NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f", 411 cal_data.temp_intercept[0], cal_data.temp_intercept[1], 412 cal_data.temp_intercept[2]); 413 } 414 } 415 416 } // namespace nano_calibration 417
