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 <cstdint>
20 #include <cstring>
21
22 #include "chre/util/nanoapp/log.h"
23
24 namespace nano_calibration {
25 namespace {
26
27 // Common log message sensor-specific identifiers.
28 constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
29 constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
30 constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
31
32 // Defines a plan for limiting log messages so that upon initialization there
33 // begins a period set by 'duration_of_rapid_messages_min' where log messages
34 // appear at a rate set by 'rapid_message_interval_sec'. Afterwards, log
35 // messages will be produced at a rate determined by
36 // 'slow_message_interval_min'.
37 struct LogMessageRegimen {
38 uint8_t rapid_message_interval_sec; // Assists device verification.
39 uint8_t slow_message_interval_min; // Avoids long-term log spam.
40 uint8_t duration_of_rapid_messages_min;
41 };
42
43 constexpr LogMessageRegimen kGyroscopeMessagePlan = {
44 /*rapid_message_interval_sec*/ 20,
45 /*slow_message_interval_min*/ 5,
46 /*duration_of_rapid_messages_min*/ 3
47 };
48
49 using ::online_calibration::CalibrationDataThreeAxis;
50 using ::online_calibration::CalibrationTypeFlags;
51 using ::online_calibration::SensorData;
52 using ::online_calibration::SensorIndex;
53 using ::online_calibration::SensorType;
54
55 // NanoSensorCal logging macros.
56 #ifndef LOG_TAG
57 #define LOG_TAG "[ImuCal]"
58 #endif
59
60 #ifdef NANO_SENSOR_CAL_DBG_ENABLED
61 #define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
62 #define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__)
63 #define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__)
64 #else
65 #define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
66 #define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
67 #define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
68 #endif // NANO_SENSOR_CAL_DBG_ENABLED
69
70 // NOTE: LOGI is defined to ensure calibration updates are always logged for
71 // field diagnosis and verification.
72 #define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__)
73
74 } // namespace
75
Initialize(OnlineCalibrationThreeAxis * accel_cal,OnlineCalibrationThreeAxis * gyro_cal,OnlineCalibrationThreeAxis * mag_cal)76 void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
77 OnlineCalibrationThreeAxis *gyro_cal,
78 OnlineCalibrationThreeAxis *mag_cal) {
79 // Loads stored calibration data and initializes the calibration algorithms.
80 accel_cal_ = accel_cal;
81 if (accel_cal_ != nullptr) {
82 if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
83 LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
84 &accel_cal_update_flags_, kAccelTag);
85 NANO_CAL_LOGI(kAccelTag,
86 "Accelerometer runtime calibration initialized.");
87 } else {
88 accel_cal_ = nullptr;
89 NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type.");
90 }
91 }
92
93 gyro_cal_ = gyro_cal;
94 if (gyro_cal_ != nullptr) {
95 if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
96 LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_,
97 &gyro_cal_update_flags_, kGyroTag);
98 NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
99 } else {
100 gyro_cal_ = nullptr;
101 NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type.");
102 }
103 }
104
105 mag_cal_ = mag_cal;
106 if (mag_cal != nullptr) {
107 if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
108 LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
109 &mag_cal_update_flags_, kMagTag);
110 NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
111 } else {
112 mag_cal_ = nullptr;
113 NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
114 }
115 }
116
117 // Resets the initialization timestamp. Set below in HandleSensorSamples.
118 initialization_start_time_nanos_ = 0;
119 }
120
HandleSensorSamples(uint16_t event_type,const chreSensorThreeAxisData * event_data)121 void NanoSensorCal::HandleSensorSamples(
122 uint16_t event_type, const chreSensorThreeAxisData *event_data) {
123 // Converts CHRE Event -> SensorData::SensorType.
124 SensorData sample;
125 switch (event_type) {
126 case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
127 sample.type = SensorType::kAccelerometerMps2;
128 break;
129 case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
130 sample.type = SensorType::kGyroscopeRps;
131 break;
132 case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
133 sample.type = SensorType::kMagnetometerUt;
134 break;
135 default:
136 // This sensor type is not used.
137 NANO_CAL_LOGW("[NanoSensorCal]",
138 "Unexpected 3-axis sensor type received.");
139 return;
140 }
141
142 // Sends the sensor payload to the calibration algorithms and checks for
143 // calibration updates.
144 const auto &header = event_data->header;
145 const auto *data = event_data->readings;
146 sample.timestamp_nanos = header.baseTimestamp;
147 for (size_t i = 0; i < header.readingCount; i++) {
148 sample.timestamp_nanos += data[i].timestampDelta;
149 memcpy(sample.data, data[i].v, sizeof(sample.data));
150 ProcessSample(sample);
151 }
152
153 // Starts tracking the time after initialization to help rate limit gyro log
154 // messaging.
155 if (initialization_start_time_nanos_ == 0) {
156 initialization_start_time_nanos_ = header.baseTimestamp;
157 gyro_notification_time_nanos_ = 0;
158 }
159 }
160
HandleTemperatureSamples(uint16_t event_type,const chreSensorFloatData * event_data)161 void NanoSensorCal::HandleTemperatureSamples(
162 uint16_t event_type, const chreSensorFloatData *event_data) {
163 // Computes the mean of the batched temperature samples and delivers it to the
164 // calibration algorithms. Note, the temperature sensor batch size determines
165 // its minimum update interval.
166 if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
167 event_data->header.readingCount > 0) {
168 const auto header = event_data->header;
169 const auto *data = event_data->readings;
170
171 SensorData sample;
172 sample.type = SensorType::kTemperatureCelsius;
173 sample.timestamp_nanos = header.baseTimestamp;
174
175 float accum_temperature_celsius = 0.0f;
176 for (size_t i = 0; i < header.readingCount; i++) {
177 sample.timestamp_nanos += data[i].timestampDelta;
178 accum_temperature_celsius += data[i].value;
179 }
180 sample.data[SensorIndex::kSingleAxis] =
181 accum_temperature_celsius / header.readingCount;
182 ProcessSample(sample);
183 } else {
184 NANO_CAL_LOGW("[NanoSensorCal]",
185 "Unexpected single-axis sensor type received.");
186 }
187 }
188
ProcessSample(const SensorData & sample)189 void NanoSensorCal::ProcessSample(const SensorData &sample) {
190 // Sends a new sensor sample to each active calibration algorithm and sends
191 // out notifications for new calibration updates.
192 if (accel_cal_ != nullptr) {
193 const CalibrationTypeFlags new_cal_flags =
194 accel_cal_->SetMeasurement(sample);
195 if (new_cal_flags != CalibrationTypeFlags::NONE) {
196 accel_cal_update_flags_ |= new_cal_flags;
197 NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
198 accel_cal_->GetSensorCalibration(),
199 accel_cal_update_flags_, kAccelTag);
200 PrintCalibration(accel_cal_->GetSensorCalibration(),
201 accel_cal_update_flags_, kAccelTag);
202 }
203 }
204
205 if (gyro_cal_ != nullptr) {
206 const CalibrationTypeFlags new_cal_flags =
207 gyro_cal_->SetMeasurement(sample);
208 if (new_cal_flags != CalibrationTypeFlags::NONE) {
209 gyro_cal_update_flags_ |= new_cal_flags;
210 if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
211 gyro_cal_->GetSensorCalibration(),
212 gyro_cal_update_flags_, kGyroTag)) {
213 HandleGyroLogMessage(sample.timestamp_nanos);
214 }
215 }
216 }
217
218 if (mag_cal_ != nullptr) {
219 const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample);
220 if (new_cal_flags != CalibrationTypeFlags::NONE) {
221 mag_cal_update_flags_ |= new_cal_flags;
222 NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
223 mag_cal_->GetSensorCalibration(),
224 mag_cal_update_flags_, kMagTag);
225 PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_,
226 kMagTag);
227 }
228 }
229 }
230
NotifyAshCalibration(uint8_t chreSensorType,const CalibrationDataThreeAxis & cal_data,CalibrationTypeFlags flags,const char * sensor_tag)231 bool NanoSensorCal::NotifyAshCalibration(
232 uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
233 CalibrationTypeFlags flags, const char *sensor_tag) {
234 // Updates the sensor offset calibration using the ASH API.
235 ashCalInfo ash_cal_info;
236 memset(&ash_cal_info, 0, sizeof(ashCalInfo));
237 ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor).
238 ash_cal_info.compMatrix[4] = 1.0f;
239 ash_cal_info.compMatrix[8] = 1.0f;
240 memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias));
241
242 // Maps CalibrationQualityLevel to ASH calibration accuracy.
243 switch (cal_data.calibration_quality.level) {
244 case online_calibration::CalibrationQualityLevel::HIGH_QUALITY:
245 ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
246 break;
247
248 case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY:
249 ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM;
250 break;
251
252 case online_calibration::CalibrationQualityLevel::LOW_QUALITY:
253 ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW;
254 break;
255
256 default:
257 ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE;
258 break;
259 }
260
261 if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
262 NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
263 return false;
264 }
265
266 // Uses the ASH API to store all calibration parameters relevant to a given
267 // algorithm as indicated by the input calibration type flags.
268 ashCalParams ash_cal_parameters;
269 memset(&ash_cal_parameters, 0, sizeof(ashCalParams));
270 if (flags & CalibrationTypeFlags::BIAS) {
271 ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius;
272 memcpy(ash_cal_parameters.offset, cal_data.offset,
273 sizeof(ash_cal_parameters.offset));
274 ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
275 ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
276 }
277
278 if (flags & CalibrationTypeFlags::OVER_TEMP) {
279 memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity,
280 sizeof(ash_cal_parameters.tempSensitivity));
281 memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept,
282 sizeof(ash_cal_parameters.tempIntercept));
283 ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
284 ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
285 }
286
287 if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
288 NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
289 return false;
290 }
291
292 return true;
293 }
294
LoadAshCalibration(uint8_t chreSensorType,OnlineCalibrationThreeAxis * online_cal,CalibrationTypeFlags * flags,const char * sensor_tag)295 bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
296 OnlineCalibrationThreeAxis *online_cal,
297 CalibrationTypeFlags* flags,
298 const char *sensor_tag) {
299 ashCalParams recalled_ash_cal_parameters;
300 if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
301 &recalled_ash_cal_parameters)) {
302 // Checks whether a valid set of runtime calibration parameters was received
303 // and can be used for initialization.
304 if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags,
305 &recalled_ash_cal_parameters)) {
306 CalibrationDataThreeAxis cal_data;
307 cal_data.type = online_cal->get_sensor_type();
308 cal_data.cal_update_time_nanos = chreGetTime();
309
310 // Analyzes the calibration flags and sets only the runtime calibration
311 // values that were received.
312 if (*flags & CalibrationTypeFlags::BIAS) {
313 cal_data.offset_temp_celsius =
314 recalled_ash_cal_parameters.offsetTempCelsius;
315 memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
316 sizeof(cal_data.offset));
317 }
318
319 if (*flags & CalibrationTypeFlags::OVER_TEMP) {
320 memcpy(cal_data.temp_sensitivity,
321 recalled_ash_cal_parameters.tempSensitivity,
322 sizeof(cal_data.temp_sensitivity));
323 memcpy(cal_data.temp_intercept,
324 recalled_ash_cal_parameters.tempIntercept,
325 sizeof(cal_data.temp_intercept));
326 }
327
328 // Sets the algorithm's initial calibration data and notifies ASH to apply
329 // the recalled calibration data.
330 if (online_cal->SetInitialCalibration(cal_data)) {
331 return NotifyAshCalibration(chreSensorType,
332 online_cal->GetSensorCalibration(), *flags,
333 sensor_tag);
334 } else {
335 NANO_CAL_LOGE(sensor_tag,
336 "Calibration data failed to initialize algorithm.");
337 }
338 }
339 } else {
340 // This is not necessarily an error since there may not be any previously
341 // stored runtime calibration data to load yet (e.g., first device boot).
342 NANO_CAL_LOGW(sensor_tag, "ASH did not recall calibration data.");
343 }
344
345 return false;
346 }
347
DetectRuntimeCalibration(uint8_t chreSensorType,const char * sensor_tag,CalibrationTypeFlags * flags,ashCalParams * ash_cal_parameters)348 bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType,
349 const char *sensor_tag,
350 CalibrationTypeFlags *flags,
351 ashCalParams *ash_cal_parameters) {
352 // Analyzes calibration source flags to determine whether runtime
353 // calibration values have been loaded and may be used for initialization. A
354 // valid runtime calibration source will include at least an offset.
355 *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags.
356
357 // Uses the ASH calibration source flags to set the appropriate
358 // CalibrationTypeFlags. These will be used to determine which values to copy
359 // from 'ash_cal_parameters' and provide to the calibration algorithms for
360 // initialization.
361 bool runtime_cal_detected = false;
362 if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
363 ash_cal_parameters->offsetTempCelsiusSource ==
364 ASH_CAL_PARAMS_SOURCE_RUNTIME) {
365 runtime_cal_detected = true;
366 *flags = CalibrationTypeFlags::BIAS;
367 }
368
369 if (ash_cal_parameters->tempSensitivitySource ==
370 ASH_CAL_PARAMS_SOURCE_RUNTIME &&
371 ash_cal_parameters->tempInterceptSource ==
372 ASH_CAL_PARAMS_SOURCE_RUNTIME) {
373 *flags |= CalibrationTypeFlags::OVER_TEMP;
374 }
375
376 if (runtime_cal_detected) {
377 // Prints the retrieved runtime calibration data.
378 NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected.");
379 PrintAshCalParams(*ash_cal_parameters, sensor_tag);
380 } else {
381 // This is a warning (not an error) since the runtime algorithms will
382 // function correctly with no recalled calibration values. They will
383 // eventually trigger and update the system with valid calibration data.
384 NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
385 }
386
387 return runtime_cal_detected;
388 }
389
390 // Helper functions for logging calibration information.
PrintAshCalParams(const ashCalParams & cal_params,const char * sensor_tag)391 void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params,
392 const char *sensor_tag) {
393 if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
394 NANO_CAL_LOGI(sensor_tag,
395 "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
396 cal_params.offset[0], cal_params.offset[1],
397 cal_params.offset[2], cal_params.offsetTempCelsius);
398 }
399
400 if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
401 NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
402 cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
403 cal_params.tempSensitivity[2]);
404 }
405
406 if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
407 NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
408 cal_params.tempIntercept[0], cal_params.tempIntercept[1],
409 cal_params.tempIntercept[2]);
410 }
411
412 if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
413 NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
414 cal_params.scaleFactor[0], cal_params.scaleFactor[1],
415 cal_params.scaleFactor[2]);
416 }
417
418 if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
419 NANO_CAL_LOGI(sensor_tag,
420 "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
421 cal_params.crossAxis[0], cal_params.crossAxis[1],
422 cal_params.crossAxis[2]);
423 }
424 }
425
PrintCalibration(const CalibrationDataThreeAxis & cal_data,CalibrationTypeFlags flags,const char * sensor_tag)426 void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
427 CalibrationTypeFlags flags,
428 const char *sensor_tag) {
429 if (flags & CalibrationTypeFlags::BIAS) {
430 NANO_CAL_LOGI(sensor_tag,
431 "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
432 cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
433 cal_data.offset_temp_celsius);
434 }
435
436 if (flags & CalibrationTypeFlags::OVER_TEMP) {
437 NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f",
438 cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1],
439 cal_data.temp_sensitivity[2]);
440 NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f",
441 cal_data.temp_intercept[0], cal_data.temp_intercept[1],
442 cal_data.temp_intercept[2]);
443 }
444 }
445
HandleGyroLogMessage(uint64_t timestamp_nanos)446 void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) {
447 // Limits the log messaging update rate for the gyro calibrations since
448 // these can occur frequently with rapid temperature changes.
449 const int64_t next_log_interval_nanos =
450 (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
451 timestamp_nanos, initialization_start_time_nanos_,
452 MIN_TO_NANOS(kGyroscopeMessagePlan.duration_of_rapid_messages_min)))
453 ? MIN_TO_NANOS(kGyroscopeMessagePlan.slow_message_interval_min)
454 : SEC_TO_NANOS(kGyroscopeMessagePlan.rapid_message_interval_sec);
455
456 const bool print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
457 timestamp_nanos, gyro_notification_time_nanos_,
458 next_log_interval_nanos);
459
460 if (print_gyro_log) {
461 gyro_notification_time_nanos_ = timestamp_nanos;
462 PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_,
463 kGyroTag);
464 }
465 }
466
467 } // namespace nano_calibration
468