1 /* 2 * Copyright (C) 2016 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/gyroscope/gyro_cal.h" 18 19 #include <float.h> 20 #include <math.h> 21 #include <string.h> 22 23 #include "calibration/util/cal_log.h" 24 #include "common/math/macros.h" 25 26 /////// DEFINITIONS AND MACROS /////////////////////////////////////// 27 28 // Maximum gyro bias correction (should be set based on expected max bias 29 // of the given sensor). 30 #define MAX_GYRO_BIAS (0.2f) // [rad/sec] 31 32 // Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and 33 // resets when exceeded. 34 #define GYRO_WATCHDOG_TIMEOUT_NANOS (SEC_TO_NANOS(5)) 35 36 #ifdef GYRO_CAL_DBG_ENABLED 37 // The time value used to throttle debug messaging. 38 #define GYROCAL_WAIT_TIME_NANOS (MSEC_TO_NANOS(100)) 39 40 // A debug version label to help with tracking results. 41 #define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]" 42 43 // Debug log tag string used to identify debug report output data. 44 #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]" 45 #endif // GYRO_CAL_DBG_ENABLED 46 47 /////// FORWARD DECLARATIONS ///////////////////////////////////////// 48 49 static void deviceStillnessCheck(struct GyroCal* gyro_cal, 50 uint64_t sample_time_nanos); 51 52 static void computeGyroCal(struct GyroCal* gyro_cal, 53 uint64_t calibration_time_nanos); 54 55 static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos); 56 57 // Data tracker command enumeration. 58 enum GyroCalTrackerCommand { 59 DO_RESET = 0, // Resets the local data used for data tracking. 60 DO_UPDATE_DATA, // Updates the local tracking data. 61 DO_STORE_DATA, // Stores intermediate results for later recall. 62 DO_EVALUATE // Computes and provides the results of the gate function. 63 }; 64 65 /* 66 * Updates the temperature min/max and mean during the stillness period. Returns 67 * 'true' if the min and max temperature values exceed the range set by 68 * 'temperature_delta_limit_celsius'. 69 * 70 * INPUTS: 71 * gyro_cal: Pointer to the GyroCal data structure. 72 * temperature_celsius: New temperature sample to include. 73 * do_this: Command enumerator that controls function behavior: 74 */ 75 static bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal, 76 float temperature_celsius, 77 enum GyroCalTrackerCommand do_this); 78 79 /* 80 * Tracks the minimum and maximum gyroscope stillness window means. 81 * Returns 'true' when the difference between gyroscope min and max window 82 * means are outside the range set by 'stillness_mean_delta_limit'. 83 * 84 * INPUTS: 85 * gyro_cal: Pointer to the GyroCal data structure. 86 * do_this: Command enumerator that controls function behavior. 87 */ 88 static bool gyroStillMeanTracker(struct GyroCal* gyro_cal, 89 enum GyroCalTrackerCommand do_this); 90 91 #ifdef GYRO_CAL_DBG_ENABLED 92 // Defines the type of debug data to print. 93 enum DebugPrintData { 94 OFFSET = 0, 95 STILLNESS_DATA, 96 SAMPLE_RATE_AND_TEMPERATURE, 97 GYRO_MINMAX_STILLNESS_MEAN, 98 ACCEL_STATS, 99 GYRO_STATS, 100 MAG_STATS, 101 ACCEL_STATS_TUNING, 102 GYRO_STATS_TUNING, 103 MAG_STATS_TUNING 104 }; 105 106 /* 107 * Updates the running calculation of the gyro's mean sampling rate. 108 * 109 * Behavior: 110 * 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local 111 * calculation of the sampling rate is copied, and the function returns. 112 * 2) Else, if 'reset_stats' is 'true' then the local estimate is reset and 113 * the function returns. 114 * 3) Otherwise, the local estimate of the mean sampling rates is updated. 115 * 116 * INPUTS: 117 * sample_rate_estimator: Pointer to the estimator data structure. 118 * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update. 119 * timestamp_nanos: Time stamp (nanoseconds). 120 * reset_stats: Flag that signals a reset of the sampling rate estimate. 121 */ 122 static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator, 123 float* debug_mean_sampling_rate_hz, 124 uint64_t timestamp_nanos, bool reset_stats); 125 126 // Updates the information used for debug printouts. 127 static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); 128 129 // Helper function for printing out common debug data. 130 static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, 131 char* debug_tag, 132 enum DebugPrintData print_data); 133 134 // This conversion function is necessary for Nanohub firmware compilation (i.e., 135 // can't cast a uint64_t to a float directly). This conversion function was 136 // copied from: /third_party/contexthub/firmware/src/floatRt.c 137 static float floatFromUint64(uint64_t v) 138 { 139 uint32_t hi = v >> 32, lo = v; 140 141 if (!hi) //this is very fast for cases where we fit into a uint32_t 142 return (float)lo; 143 else { 144 return ((float)hi) * 4294967296.0f + (float)lo; 145 } 146 } 147 #endif // GYRO_CAL_DBG_ENABLED 148 149 /////// FUNCTION DEFINITIONS ///////////////////////////////////////// 150 151 // Initialize the gyro calibration data structure. 152 void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, 153 uint64_t max_still_duration_nanos, float bias_x, float bias_y, 154 float bias_z, uint64_t calibration_time_nanos, 155 uint64_t window_time_duration_nanos, float gyro_var_threshold, 156 float gyro_confidence_delta, float accel_var_threshold, 157 float accel_confidence_delta, float mag_var_threshold, 158 float mag_confidence_delta, float stillness_threshold, 159 float stillness_mean_delta_limit, 160 float temperature_delta_limit_celsius, 161 bool gyro_calibration_enable) { 162 // Clear gyro_cal structure memory. 163 memset(gyro_cal, 0, sizeof(struct GyroCal)); 164 165 // Initialize the stillness detectors. 166 // Gyro parameter input units are [rad/sec]. 167 // Accel parameter input units are [m/sec^2]. 168 // Magnetometer parameter input units are [uT]. 169 gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold, 170 gyro_confidence_delta); 171 gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold, 172 accel_confidence_delta); 173 gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold, 174 mag_confidence_delta); 175 176 // Reset stillness flag and start timestamp. 177 gyro_cal->prev_still = false; 178 gyro_cal->start_still_time_nanos = 0; 179 180 // Set the min and max window stillness duration. 181 gyro_cal->min_still_duration_nanos = min_still_duration_nanos; 182 gyro_cal->max_still_duration_nanos = max_still_duration_nanos; 183 184 // Sets the duration of the stillness processing windows. 185 gyro_cal->window_time_duration_nanos = window_time_duration_nanos; 186 187 // Set the watchdog timeout duration. 188 gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS; 189 190 // Load the last valid cal from system memory. 191 gyro_cal->bias_x = bias_x; // [rad/sec] 192 gyro_cal->bias_y = bias_y; // [rad/sec] 193 gyro_cal->bias_z = bias_z; // [rad/sec] 194 gyro_cal->calibration_time_nanos = calibration_time_nanos; 195 196 // Set the stillness threshold required for gyro bias calibration. 197 gyro_cal->stillness_threshold = stillness_threshold; 198 199 // Current window end-time used to assist in keeping sensor data collection in 200 // sync. Setting this to zero signals that sensor data will be dropped until a 201 // valid end-time is set from the first gyro timestamp received. 202 gyro_cal->stillness_win_endtime_nanos = 0; 203 204 // Gyro calibrations will be applied (see, gyroCalRemoveBias()). 205 gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0); 206 207 // Sets the stability limit for the stillness window mean acceptable delta. 208 gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit; 209 210 // Sets the min/max temperature delta limit for the stillness period. 211 gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius; 212 213 // Ensures that the data tracking functionality is reset. 214 gyroStillMeanTracker(gyro_cal, DO_RESET); 215 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); 216 217 #ifdef GYRO_CAL_DBG_ENABLED 218 if (gyro_cal->gyro_calibration_enable) { 219 CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED."); 220 } else { 221 CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED."); 222 } 223 224 // Ensures that the gyro sampling rate estimate is reset. 225 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0, 226 /*reset_stats=*/true); 227 #endif // GYRO_CAL_DBG_ENABLED 228 } 229 230 // Void pointer in the gyro calibration data structure (doesn't do anything 231 // except prevent compiler warnings). 232 void gyroCalDestroy(struct GyroCal* gyro_cal) { 233 (void)gyro_cal; 234 } 235 236 // Get the most recent bias calibration value. 237 void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, 238 float* bias_z, float* temperature_celsius) { 239 *bias_x = gyro_cal->bias_x; 240 *bias_y = gyro_cal->bias_y; 241 *bias_z = gyro_cal->bias_z; 242 *temperature_celsius = gyro_cal->bias_temperature_celsius; 243 } 244 245 // Set an initial bias calibration value. 246 void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, 247 float bias_z, uint64_t calibration_time_nanos) { 248 gyro_cal->bias_x = bias_x; 249 gyro_cal->bias_y = bias_y; 250 gyro_cal->bias_z = bias_z; 251 gyro_cal->calibration_time_nanos = calibration_time_nanos; 252 253 #ifdef GYRO_CAL_DBG_ENABLED 254 CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]", 255 "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET, 256 CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3), 257 CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3), 258 CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3)); 259 #endif // GYRO_CAL_DBG_ENABLED 260 } 261 262 // Remove bias from a gyro measurement [rad/sec]. 263 void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, 264 float* xo, float* yo, float* zo) { 265 if (gyro_cal->gyro_calibration_enable) { 266 *xo = xi - gyro_cal->bias_x; 267 *yo = yi - gyro_cal->bias_y; 268 *zo = zi - gyro_cal->bias_z; 269 } 270 } 271 272 // Returns true when a new gyro calibration is available. 273 bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal) { 274 bool new_gyro_cal_available = 275 (gyro_cal->gyro_calibration_enable && gyro_cal->new_gyro_cal_available); 276 277 // Clear the flag. 278 gyro_cal->new_gyro_cal_available = false; 279 280 return new_gyro_cal_available; 281 } 282 283 // Update the gyro calibration with gyro data [rad/sec]. 284 void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 285 float x, float y, float z, float temperature_celsius) { 286 // Make sure that a valid window end-time is set, and start the watchdog 287 // timer. 288 if (gyro_cal->stillness_win_endtime_nanos <= 0) { 289 gyro_cal->stillness_win_endtime_nanos = 290 sample_time_nanos + gyro_cal->window_time_duration_nanos; 291 292 // Start the watchdog timer. 293 gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; 294 } 295 296 // Update the temperature statistics. 297 gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA); 298 299 #ifdef GYRO_CAL_DBG_ENABLED 300 // Update the gyro sampling rate estimate. 301 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 302 sample_time_nanos, /*reset_stats=*/false); 303 #endif // GYRO_CAL_DBG_ENABLED 304 305 // Pass gyro data to stillness detector 306 gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect, 307 gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, 308 x, y, z); 309 310 // Perform a device stillness check, set next window end-time, and 311 // possibly do a gyro bias calibration and stillness detector reset. 312 deviceStillnessCheck(gyro_cal, sample_time_nanos); 313 } 314 315 // Update the gyro calibration with mag data [micro Tesla]. 316 void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 317 float x, float y, float z) { 318 // Pass magnetometer data to stillness detector. 319 gyroStillDetUpdate(&gyro_cal->mag_stillness_detect, 320 gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, 321 x, y, z); 322 323 // Received a magnetometer sample; incorporate it into detection. 324 gyro_cal->using_mag_sensor = true; 325 326 // Perform a device stillness check, set next window end-time, and 327 // possibly do a gyro bias calibration and stillness detector reset. 328 deviceStillnessCheck(gyro_cal, sample_time_nanos); 329 } 330 331 // Update the gyro calibration with accel data [m/sec^2]. 332 void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, 333 float x, float y, float z) { 334 // Pass accelerometer data to stillnesss detector. 335 gyroStillDetUpdate(&gyro_cal->accel_stillness_detect, 336 gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, 337 x, y, z); 338 339 // Perform a device stillness check, set next window end-time, and 340 // possibly do a gyro bias calibration and stillness detector reset. 341 deviceStillnessCheck(gyro_cal, sample_time_nanos); 342 } 343 344 // TODO: Consider breaking this function up to improve readability. 345 // Checks the state of all stillness detectors to determine 346 // whether the device is "still". 347 void deviceStillnessCheck(struct GyroCal* gyro_cal, 348 uint64_t sample_time_nanos) { 349 bool stillness_duration_exceeded = false; 350 bool stillness_duration_too_short = false; 351 bool min_max_temp_exceeded = false; 352 bool mean_not_stable = false; 353 bool device_is_still = false; 354 float conf_not_rot = 0; 355 float conf_not_accel = 0; 356 float conf_still = 0; 357 358 // Check the watchdog timer. 359 checkWatchdog(gyro_cal, sample_time_nanos); 360 361 // Is there enough data to do a stillness calculation? 362 if ((!gyro_cal->mag_stillness_detect.stillness_window_ready && 363 gyro_cal->using_mag_sensor) || 364 !gyro_cal->accel_stillness_detect.stillness_window_ready || 365 !gyro_cal->gyro_stillness_detect.stillness_window_ready) { 366 return; // Not yet, wait for more data. 367 } 368 369 // Set the next window end-time for the stillness detectors. 370 gyro_cal->stillness_win_endtime_nanos = 371 sample_time_nanos + gyro_cal->window_time_duration_nanos; 372 373 // Update the confidence scores for all sensors. 374 gyroStillDetCompute(&gyro_cal->accel_stillness_detect); 375 gyroStillDetCompute(&gyro_cal->gyro_stillness_detect); 376 if (gyro_cal->using_mag_sensor) { 377 gyroStillDetCompute(&gyro_cal->mag_stillness_detect); 378 } else { 379 // Not using magnetometer, force stillness confidence to 100%. 380 gyro_cal->mag_stillness_detect.stillness_confidence = 1.0f; 381 } 382 383 // Updates the mean tracker data. 384 gyroStillMeanTracker(gyro_cal, DO_UPDATE_DATA); 385 386 // Determine motion confidence scores (rotation, accelerating, and stillness). 387 conf_not_rot = gyro_cal->gyro_stillness_detect.stillness_confidence * 388 gyro_cal->mag_stillness_detect.stillness_confidence; 389 conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence; 390 conf_still = conf_not_rot * conf_not_accel; 391 392 // Evaluate the mean and temperature gate functions. 393 mean_not_stable = gyroStillMeanTracker(gyro_cal, DO_EVALUATE); 394 min_max_temp_exceeded = 395 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_EVALUATE); 396 397 // Determines if the device is currently still. 398 device_is_still = (conf_still > gyro_cal->stillness_threshold) && 399 !mean_not_stable && !min_max_temp_exceeded; 400 401 if (device_is_still) { 402 // Device is "still" logic: 403 // If not previously still, then record the start time. 404 // If stillness period is too long, then do a calibration. 405 // Otherwise, continue collecting stillness data. 406 407 // If device was not previously still, set new start timestamp. 408 if (!gyro_cal->prev_still) { 409 // Record the starting timestamp of the current stillness window. 410 // This enables the calculation of total duration of the stillness period. 411 gyro_cal->start_still_time_nanos = 412 gyro_cal->gyro_stillness_detect.window_start_time; 413 } 414 415 // Check to see if current stillness period exceeds the desired limit. 416 stillness_duration_exceeded = 417 (gyro_cal->gyro_stillness_detect.last_sample_time >= 418 gyro_cal->start_still_time_nanos + gyro_cal->max_still_duration_nanos); 419 420 // Track the new stillness mean and temperature data. 421 gyroStillMeanTracker(gyro_cal, DO_STORE_DATA); 422 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_STORE_DATA); 423 424 if (stillness_duration_exceeded) { 425 // The current stillness has gone too long. Do a calibration with the 426 // current data and reset. 427 428 // Updates the gyro bias estimate with the current window data and 429 // resets the stats. 430 gyroStillDetReset(&gyro_cal->accel_stillness_detect, 431 /*reset_stats=*/true); 432 gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); 433 gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); 434 435 // Resets the local calculations because the stillness period is over. 436 gyroStillMeanTracker(gyro_cal, DO_RESET); 437 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); 438 439 // Computes a new gyro offset estimate. 440 computeGyroCal(gyro_cal, 441 gyro_cal->gyro_stillness_detect.last_sample_time); 442 443 #ifdef GYRO_CAL_DBG_ENABLED 444 // Resets the sampling rate estimate. 445 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 446 sample_time_nanos, /*reset_stats=*/true); 447 #endif // GYRO_CAL_DBG_ENABLED 448 449 // Update stillness flag. Force the start of a new stillness period. 450 gyro_cal->prev_still = false; 451 } else { 452 // Continue collecting stillness data. 453 454 // Extend the stillness period. 455 gyroStillDetReset(&gyro_cal->accel_stillness_detect, 456 /*reset_stats=*/false); 457 gyroStillDetReset(&gyro_cal->gyro_stillness_detect, 458 /*reset_stats=*/false); 459 gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false); 460 461 // Update the stillness flag. 462 gyro_cal->prev_still = true; 463 } 464 } else { 465 // Device is NOT still; motion detected. 466 467 // If device was previously still and the total stillness duration is not 468 // "too short", then do a calibration with the data accumulated thus far. 469 stillness_duration_too_short = 470 (gyro_cal->gyro_stillness_detect.window_start_time < 471 gyro_cal->start_still_time_nanos + gyro_cal->min_still_duration_nanos); 472 473 if (gyro_cal->prev_still && !stillness_duration_too_short) { 474 computeGyroCal(gyro_cal, 475 gyro_cal->gyro_stillness_detect.window_start_time); 476 } 477 478 // Reset the stillness detectors and the stats. 479 gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); 480 gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); 481 gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); 482 483 // Resets the temperature and sensor mean data. 484 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); 485 gyroStillMeanTracker(gyro_cal, DO_RESET); 486 487 #ifdef GYRO_CAL_DBG_ENABLED 488 // Resets the sampling rate estimate. 489 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 490 sample_time_nanos, /*reset_stats=*/true); 491 #endif // GYRO_CAL_DBG_ENABLED 492 493 // Update stillness flag. 494 gyro_cal->prev_still = false; 495 } 496 497 // Reset the watchdog timer after we have processed data. 498 gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; 499 } 500 501 // Calculates a new gyro bias offset calibration value. 502 void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { 503 // Check to see if new calibration values is within acceptable range. 504 if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS && 505 gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS && 506 gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS && 507 gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS && 508 gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS && 509 gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) { 510 #ifdef GYRO_CAL_DBG_ENABLED 511 CAL_DEBUG_LOG( 512 "[GYRO_CAL:REJECT]", 513 "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET 514 ", " CAL_FORMAT_3DIGITS ", %llu", 515 CAL_ENCODE_FLOAT( 516 gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3), 517 CAL_ENCODE_FLOAT( 518 gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MDEG, 3), 519 CAL_ENCODE_FLOAT( 520 gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3), 521 CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3), 522 (unsigned long long int)calibration_time_nanos); 523 #endif // GYRO_CAL_DBG_ENABLED 524 525 // Outside of range. Ignore, reset, and continue. 526 return; 527 } 528 529 // Record the new gyro bias offset calibration. 530 gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean_x; 531 gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean_y; 532 gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean_z; 533 534 // Store the calibration temperature (using the mean temperature over the 535 // "stillness" period). 536 gyro_cal->bias_temperature_celsius = gyro_cal->temperature_mean_celsius; 537 538 // Store the calibration time stamp. 539 gyro_cal->calibration_time_nanos = calibration_time_nanos; 540 541 // Record the final stillness confidence. 542 gyro_cal->stillness_confidence = 543 gyro_cal->gyro_stillness_detect.prev_stillness_confidence * 544 gyro_cal->accel_stillness_detect.prev_stillness_confidence * 545 gyro_cal->mag_stillness_detect.prev_stillness_confidence; 546 547 // Set flag to indicate a new gyro calibration value is available. 548 gyro_cal->new_gyro_cal_available = true; 549 550 #ifdef GYRO_CAL_DBG_ENABLED 551 // Increment the total count of calibration updates. 552 gyro_cal->debug_calibration_count++; 553 554 // Update the calibration debug information and trigger a printout. 555 gyroCalUpdateDebug(gyro_cal); 556 #endif 557 } 558 559 // Check for a watchdog timeout condition. 560 void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { 561 bool watchdog_timeout; 562 563 // Check for initialization of the watchdog time (=0). 564 if (gyro_cal->gyro_watchdog_start_nanos <= 0) { 565 return; 566 } 567 568 // Checks for the following watchdog timeout conditions: 569 // i. The current timestamp has exceeded the allowed watchdog duration. 570 // ii. A timestamp was received that has jumped backwards by more than the 571 // allowed watchdog duration (e.g., timestamp clock roll-over). 572 watchdog_timeout = 573 (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos + 574 gyro_cal->gyro_watchdog_start_nanos) || 575 (sample_time_nanos + gyro_cal->gyro_watchdog_timeout_duration_nanos < 576 gyro_cal->gyro_watchdog_start_nanos); 577 578 // If a timeout occurred then reset to known good state. 579 if (watchdog_timeout) { 580 #ifdef GYRO_CAL_DBG_ENABLED 581 gyro_cal->debug_watchdog_count++; 582 if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) { 583 CAL_DEBUG_LOG( 584 "[GYRO_CAL:WATCHDOG]", 585 "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu", 586 (unsigned long int)gyro_cal->debug_watchdog_count, 587 (unsigned long long int)sample_time_nanos, 588 (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos - 589 sample_time_nanos)); 590 } else { 591 CAL_DEBUG_LOG( 592 "[GYRO_CAL:WATCHDOG]", 593 "Total#, Timestamp | Delta [nsec]: %lu, %llu, %llu", 594 (unsigned long int)gyro_cal->debug_watchdog_count, 595 (unsigned long long int)sample_time_nanos, 596 (unsigned long long int)(sample_time_nanos - 597 gyro_cal->gyro_watchdog_start_nanos)); 598 } 599 #endif // GYRO_CAL_DBG_ENABLED 600 601 // Reset stillness detectors and restart data capture. 602 gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); 603 gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); 604 gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); 605 606 // Resets the temperature and sensor mean data. 607 gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); 608 gyroStillMeanTracker(gyro_cal, DO_RESET); 609 610 #ifdef GYRO_CAL_DBG_ENABLED 611 // Resets the sampling rate estimate. 612 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 613 sample_time_nanos, /*reset_stats=*/true); 614 #endif // GYRO_CAL_DBG_ENABLED 615 616 // Resets the stillness window end-time. 617 gyro_cal->stillness_win_endtime_nanos = 0; 618 619 // Force stillness confidence to zero. 620 gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0; 621 gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0; 622 gyro_cal->mag_stillness_detect.prev_stillness_confidence = 0; 623 gyro_cal->stillness_confidence = 0; 624 gyro_cal->prev_still = false; 625 626 // If there are no magnetometer samples being received then 627 // operate the calibration algorithm without this sensor. 628 if (!gyro_cal->mag_stillness_detect.stillness_window_ready && 629 gyro_cal->using_mag_sensor) { 630 gyro_cal->using_mag_sensor = false; 631 } 632 633 // Assert watchdog timeout flags. 634 gyro_cal->gyro_watchdog_timeout |= watchdog_timeout; 635 gyro_cal->gyro_watchdog_start_nanos = 0; 636 } 637 } 638 639 // TODO -- Combine the following two functions into one or consider 640 // implementing a separate helper module for tracking the temperature and mean 641 // statistics. 642 bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal, 643 float temperature_celsius, 644 enum GyroCalTrackerCommand do_this) { 645 bool min_max_temp_exceeded = false; 646 647 switch (do_this) { 648 case DO_RESET: 649 // Resets the mean accumulator. 650 gyro_cal->temperature_mean_tracker.num_points = 0; 651 gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f; 652 653 // Initializes the min/max temperatures values. 654 gyro_cal->temperature_mean_tracker.temperature_min_celsius = FLT_MAX; 655 gyro_cal->temperature_mean_tracker.temperature_max_celsius = -FLT_MAX; 656 break; 657 658 case DO_UPDATE_DATA: 659 // Does the mean accumulation. 660 gyro_cal->temperature_mean_tracker.mean_accumulator += 661 temperature_celsius; 662 gyro_cal->temperature_mean_tracker.num_points++; 663 664 // Tracks the min, max, and latest temperature values. 665 gyro_cal->temperature_mean_tracker.latest_temperature_celsius = 666 temperature_celsius; 667 if (gyro_cal->temperature_mean_tracker.temperature_min_celsius > 668 temperature_celsius) { 669 gyro_cal->temperature_mean_tracker.temperature_min_celsius = 670 temperature_celsius; 671 } 672 if (gyro_cal->temperature_mean_tracker.temperature_max_celsius < 673 temperature_celsius) { 674 gyro_cal->temperature_mean_tracker.temperature_max_celsius = 675 temperature_celsius; 676 } 677 break; 678 679 case DO_STORE_DATA: 680 // Store the most recent temperature statistics data to the GyroCal data 681 // structure. This functionality allows previous results to be recalled 682 // when the device suddenly becomes "not still". 683 if (gyro_cal->temperature_mean_tracker.num_points > 0) { 684 gyro_cal->temperature_mean_celsius = 685 gyro_cal->temperature_mean_tracker.mean_accumulator / 686 gyro_cal->temperature_mean_tracker.num_points; 687 } else { 688 gyro_cal->temperature_mean_celsius = 689 gyro_cal->temperature_mean_tracker.latest_temperature_celsius; 690 #ifdef GYRO_CAL_DBG_ENABLED 691 CAL_DEBUG_LOG("[GYRO_CAL:TEMP_GATE]", 692 "Insufficient statistics (num_points = 0), using latest " 693 "measured temperature as the mean value."); 694 #endif // GYRO_CAL_DBG_ENABLED 695 } 696 #ifdef GYRO_CAL_DBG_ENABLED 697 // Records the min/max and mean temperature values for debug purposes. 698 gyro_cal->debug_gyro_cal.temperature_mean_celsius = 699 gyro_cal->temperature_mean_celsius; 700 gyro_cal->debug_gyro_cal.temperature_min_celsius = 701 gyro_cal->temperature_mean_tracker.temperature_min_celsius; 702 gyro_cal->debug_gyro_cal.temperature_max_celsius = 703 gyro_cal->temperature_mean_tracker.temperature_max_celsius; 704 #endif 705 break; 706 707 case DO_EVALUATE: 708 // Determines if the min/max delta exceeded the set limit. 709 if (gyro_cal->temperature_mean_tracker.num_points > 0) { 710 min_max_temp_exceeded = 711 (gyro_cal->temperature_mean_tracker.temperature_max_celsius - 712 gyro_cal->temperature_mean_tracker.temperature_min_celsius) > 713 gyro_cal->temperature_delta_limit_celsius; 714 715 #ifdef GYRO_CAL_DBG_ENABLED 716 if (min_max_temp_exceeded) { 717 CAL_DEBUG_LOG( 718 "[GYRO_CAL:TEMP_GATE]", 719 "Exceeded the max temperature variation during stillness."); 720 } 721 #endif // GYRO_CAL_DBG_ENABLED 722 } 723 break; 724 725 default: 726 break; 727 } 728 729 return min_max_temp_exceeded; 730 } 731 732 bool gyroStillMeanTracker(struct GyroCal* gyro_cal, 733 enum GyroCalTrackerCommand do_this) { 734 bool mean_not_stable = false; 735 736 switch (do_this) { 737 case DO_RESET: 738 // Resets the min/max window mean values to a default value. 739 for (size_t i = 0; i < 3; i++) { 740 gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX; 741 gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX; 742 } 743 break; 744 745 case DO_UPDATE_DATA: 746 // Computes the min/max window mean values. 747 if (gyro_cal->window_mean_tracker.gyro_winmean_min[0] > 748 gyro_cal->gyro_stillness_detect.win_mean_x) { 749 gyro_cal->window_mean_tracker.gyro_winmean_min[0] = 750 gyro_cal->gyro_stillness_detect.win_mean_x; 751 } 752 if (gyro_cal->window_mean_tracker.gyro_winmean_max[0] < 753 gyro_cal->gyro_stillness_detect.win_mean_x) { 754 gyro_cal->window_mean_tracker.gyro_winmean_max[0] = 755 gyro_cal->gyro_stillness_detect.win_mean_x; 756 } 757 758 if (gyro_cal->window_mean_tracker.gyro_winmean_min[1] > 759 gyro_cal->gyro_stillness_detect.win_mean_y) { 760 gyro_cal->window_mean_tracker.gyro_winmean_min[1] = 761 gyro_cal->gyro_stillness_detect.win_mean_y; 762 } 763 if (gyro_cal->window_mean_tracker.gyro_winmean_max[1] < 764 gyro_cal->gyro_stillness_detect.win_mean_y) { 765 gyro_cal->window_mean_tracker.gyro_winmean_max[1] = 766 gyro_cal->gyro_stillness_detect.win_mean_y; 767 } 768 769 if (gyro_cal->window_mean_tracker.gyro_winmean_min[2] > 770 gyro_cal->gyro_stillness_detect.win_mean_z) { 771 gyro_cal->window_mean_tracker.gyro_winmean_min[2] = 772 gyro_cal->gyro_stillness_detect.win_mean_z; 773 } 774 if (gyro_cal->window_mean_tracker.gyro_winmean_max[2] < 775 gyro_cal->gyro_stillness_detect.win_mean_z) { 776 gyro_cal->window_mean_tracker.gyro_winmean_max[2] = 777 gyro_cal->gyro_stillness_detect.win_mean_z; 778 } 779 break; 780 781 case DO_STORE_DATA: 782 // Store the most recent "stillness" mean data to the GyroCal data 783 // structure. This functionality allows previous results to be recalled 784 // when the device suddenly becomes "not still". 785 memcpy(gyro_cal->gyro_winmean_min, 786 gyro_cal->window_mean_tracker.gyro_winmean_min, 787 sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min)); 788 memcpy(gyro_cal->gyro_winmean_max, 789 gyro_cal->window_mean_tracker.gyro_winmean_max, 790 sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max)); 791 break; 792 793 case DO_EVALUATE: 794 // Performs the stability check and returns the 'true' if the difference 795 // between min/max window mean value is outside the stable range. 796 for (size_t i = 0; i < 3; i++) { 797 mean_not_stable |= (gyro_cal->window_mean_tracker.gyro_winmean_max[i] - 798 gyro_cal->window_mean_tracker.gyro_winmean_min[i]) > 799 gyro_cal->stillness_mean_delta_limit; 800 } 801 #ifdef GYRO_CAL_DBG_ENABLED 802 if (mean_not_stable) { 803 CAL_DEBUG_LOG( 804 "[GYRO_CAL:MEAN_STABILITY_GATE]", 805 "Variation Limit|Delta [mDPS]: " CAL_FORMAT_3DIGITS 806 " | " CAL_FORMAT_3DIGITS_TRIPLET, 807 CAL_ENCODE_FLOAT(gyro_cal->stillness_mean_delta_limit * RAD_TO_MDEG, 808 3), 809 CAL_ENCODE_FLOAT( 810 (gyro_cal->window_mean_tracker.gyro_winmean_max[0] - 811 gyro_cal->window_mean_tracker.gyro_winmean_min[0]) * 812 RAD_TO_MDEG, 813 3), 814 CAL_ENCODE_FLOAT( 815 (gyro_cal->window_mean_tracker.gyro_winmean_max[1] - 816 gyro_cal->window_mean_tracker.gyro_winmean_min[1]) * 817 RAD_TO_MDEG, 818 3), 819 CAL_ENCODE_FLOAT( 820 (gyro_cal->window_mean_tracker.gyro_winmean_max[2] - 821 gyro_cal->window_mean_tracker.gyro_winmean_min[2]) * 822 RAD_TO_MDEG, 823 3)); 824 } 825 #endif // GYRO_CAL_DBG_ENABLED 826 break; 827 828 default: 829 break; 830 } 831 832 return mean_not_stable; 833 } 834 835 #ifdef GYRO_CAL_DBG_ENABLED 836 void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator, 837 float* debug_mean_sampling_rate_hz, 838 uint64_t timestamp_nanos, bool reset_stats) { 839 // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads 840 // out the estimate of the sampling rate. 841 if (debug_mean_sampling_rate_hz) { 842 if (sample_rate_estimator->num_samples > 1 && 843 sample_rate_estimator->time_delta_accumulator > 0) { 844 // Computes the final mean calculation. 845 *debug_mean_sampling_rate_hz = 846 sample_rate_estimator->num_samples / 847 (floatFromUint64(sample_rate_estimator->time_delta_accumulator) * 848 NANOS_TO_SEC); 849 } else { 850 // Not enough samples to compute a valid sample rate estimate. Indicate 851 // this with a -1 value. 852 *debug_mean_sampling_rate_hz = -1.0f; 853 } 854 reset_stats = true; 855 } 856 857 // Resets the sampling rate mean estimator data. 858 if (reset_stats) { 859 sample_rate_estimator->last_timestamp_nanos = 0; 860 sample_rate_estimator->time_delta_accumulator = 0; 861 sample_rate_estimator->num_samples = 0; 862 return; 863 } 864 865 // Skip adding this data to the accumulator if: 866 // 1. A bad timestamp was received (i.e., time not monotonic). 867 // 2. 'last_timestamp_nanos' is zero. 868 if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos || 869 sample_rate_estimator->last_timestamp_nanos == 0) { 870 sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; 871 return; 872 } 873 874 // Increments the number of samples. 875 sample_rate_estimator->num_samples++; 876 877 // Accumulate the time steps. 878 sample_rate_estimator->time_delta_accumulator += 879 timestamp_nanos - sample_rate_estimator->last_timestamp_nanos; 880 sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; 881 } 882 883 void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { 884 // Only update this data if debug printing is not currently in progress 885 // (i.e., don't want to risk overwriting debug information that is actively 886 // being reported). 887 if (gyro_cal->debug_state != GYRO_IDLE) { 888 return; 889 } 890 891 // Probability of stillness (acc, rot, still), duration, timestamp. 892 gyro_cal->debug_gyro_cal.accel_stillness_conf = 893 gyro_cal->accel_stillness_detect.prev_stillness_confidence; 894 gyro_cal->debug_gyro_cal.gyro_stillness_conf = 895 gyro_cal->gyro_stillness_detect.prev_stillness_confidence; 896 gyro_cal->debug_gyro_cal.mag_stillness_conf = 897 gyro_cal->mag_stillness_detect.prev_stillness_confidence; 898 899 // Magnetometer usage. 900 gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor; 901 902 // Stillness start, stop, and duration times. 903 gyro_cal->debug_gyro_cal.start_still_time_nanos = 904 gyro_cal->start_still_time_nanos; 905 gyro_cal->debug_gyro_cal.end_still_time_nanos = 906 gyro_cal->calibration_time_nanos; 907 gyro_cal->debug_gyro_cal.stillness_duration_nanos = 908 gyro_cal->calibration_time_nanos - gyro_cal->start_still_time_nanos; 909 910 // Records the current calibration values. 911 gyro_cal->debug_gyro_cal.calibration[0] = gyro_cal->bias_x; 912 gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y; 913 gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z; 914 915 // Records the mean gyroscope sampling rate. 916 gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, 917 &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0, 918 /*reset_stats=*/true); 919 920 // Records the min/max gyroscope window stillness mean values. 921 memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min, 922 sizeof(gyro_cal->gyro_winmean_min)); 923 memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max, 924 sizeof(gyro_cal->gyro_winmean_max)); 925 926 // Records the previous stillness window means. 927 gyro_cal->debug_gyro_cal.accel_mean[0] = 928 gyro_cal->accel_stillness_detect.prev_mean_x; 929 gyro_cal->debug_gyro_cal.accel_mean[1] = 930 gyro_cal->accel_stillness_detect.prev_mean_y; 931 gyro_cal->debug_gyro_cal.accel_mean[2] = 932 gyro_cal->accel_stillness_detect.prev_mean_z; 933 934 gyro_cal->debug_gyro_cal.gyro_mean[0] = 935 gyro_cal->gyro_stillness_detect.prev_mean_x; 936 gyro_cal->debug_gyro_cal.gyro_mean[1] = 937 gyro_cal->gyro_stillness_detect.prev_mean_y; 938 gyro_cal->debug_gyro_cal.gyro_mean[2] = 939 gyro_cal->gyro_stillness_detect.prev_mean_z; 940 941 gyro_cal->debug_gyro_cal.mag_mean[0] = 942 gyro_cal->mag_stillness_detect.prev_mean_x; 943 gyro_cal->debug_gyro_cal.mag_mean[1] = 944 gyro_cal->mag_stillness_detect.prev_mean_y; 945 gyro_cal->debug_gyro_cal.mag_mean[2] = 946 gyro_cal->mag_stillness_detect.prev_mean_z; 947 948 // Records the variance data. 949 // NOTE: These statistics include the final captured window, which may be 950 // outside of the "stillness" period. Therefore, these values may exceed the 951 // stillness thresholds. 952 gyro_cal->debug_gyro_cal.accel_var[0] = 953 gyro_cal->accel_stillness_detect.win_var_x; 954 gyro_cal->debug_gyro_cal.accel_var[1] = 955 gyro_cal->accel_stillness_detect.win_var_y; 956 gyro_cal->debug_gyro_cal.accel_var[2] = 957 gyro_cal->accel_stillness_detect.win_var_z; 958 959 gyro_cal->debug_gyro_cal.gyro_var[0] = 960 gyro_cal->gyro_stillness_detect.win_var_x; 961 gyro_cal->debug_gyro_cal.gyro_var[1] = 962 gyro_cal->gyro_stillness_detect.win_var_y; 963 gyro_cal->debug_gyro_cal.gyro_var[2] = 964 gyro_cal->gyro_stillness_detect.win_var_z; 965 966 gyro_cal->debug_gyro_cal.mag_var[0] = 967 gyro_cal->mag_stillness_detect.win_var_x; 968 gyro_cal->debug_gyro_cal.mag_var[1] = 969 gyro_cal->mag_stillness_detect.win_var_y; 970 gyro_cal->debug_gyro_cal.mag_var[2] = 971 gyro_cal->mag_stillness_detect.win_var_z; 972 973 // Trigger a printout of the debug information. 974 gyro_cal->debug_print_trigger = true; 975 } 976 977 void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 978 enum DebugPrintData print_data) { 979 // Prints out the desired debug data. 980 float mag_data; 981 switch (print_data) { 982 case OFFSET: 983 CAL_DEBUG_LOG( 984 debug_tag, 985 "Cal#|Offset|Temp|Time [mDPS|C|nsec]: " 986 "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu", 987 (unsigned long int)gyro_cal->debug_calibration_count, 988 CAL_ENCODE_FLOAT( 989 gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3), 990 CAL_ENCODE_FLOAT( 991 gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MDEG, 3), 992 CAL_ENCODE_FLOAT( 993 gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3), 994 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 995 3), 996 (unsigned long long int) 997 gyro_cal->debug_gyro_cal.end_still_time_nanos); 998 break; 999 1000 case STILLNESS_DATA: 1001 mag_data = (gyro_cal->debug_gyro_cal.using_mag_sensor) 1002 ? gyro_cal->debug_gyro_cal.mag_stillness_conf 1003 : -1.0f; // Signals that magnetometer was not used. 1004 CAL_DEBUG_LOG( 1005 debug_tag, 1006 "Cal#|Stillness|Confidence [nsec]: %lu, " 1007 "%llu, " CAL_FORMAT_3DIGITS_TRIPLET, 1008 (unsigned long int)gyro_cal->debug_calibration_count, 1009 (unsigned long long int)(gyro_cal->debug_gyro_cal 1010 .end_still_time_nanos - 1011 gyro_cal->debug_gyro_cal 1012 .start_still_time_nanos), 1013 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3), 1014 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3), 1015 CAL_ENCODE_FLOAT(mag_data, 3)); 1016 break; 1017 1018 case SAMPLE_RATE_AND_TEMPERATURE: 1019 CAL_DEBUG_LOG( 1020 debug_tag, 1021 "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: " 1022 "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS 1023 ", " CAL_FORMAT_3DIGITS, 1024 (unsigned long int)gyro_cal->debug_calibration_count, 1025 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 1026 3), 1027 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3), 1028 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius, 3), 1029 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius - 1030 gyro_cal->debug_gyro_cal.temperature_min_celsius, 1031 3), 1032 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3)); 1033 break; 1034 1035 case GYRO_MINMAX_STILLNESS_MEAN: 1036 CAL_DEBUG_LOG( 1037 debug_tag, 1038 "Cal#|Gyro Peak Stillness Variation [mDPS]: " 1039 "%lu, " CAL_FORMAT_3DIGITS_TRIPLET, 1040 (unsigned long int)gyro_cal->debug_calibration_count, 1041 CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] - 1042 gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) * 1043 RAD_TO_MDEG, 1044 3), 1045 CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] - 1046 gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) * 1047 RAD_TO_MDEG, 1048 3), 1049 CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] - 1050 gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) * 1051 RAD_TO_MDEG, 1052 3)); 1053 break; 1054 1055 case ACCEL_STATS: 1056 CAL_DEBUG_LOG(debug_tag, 1057 "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: " 1058 "%lu, " CAL_FORMAT_3DIGITS_TRIPLET 1059 ", " CAL_FORMAT_6DIGITS_TRIPLET, 1060 (unsigned long int)gyro_cal->debug_calibration_count, 1061 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3), 1062 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3), 1063 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3), 1064 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6), 1065 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6), 1066 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6)); 1067 break; 1068 1069 case GYRO_STATS: 1070 CAL_DEBUG_LOG( 1071 debug_tag, 1072 "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET 1073 ", " CAL_FORMAT_3DIGITS_TRIPLET, 1074 (unsigned long int)gyro_cal->debug_calibration_count, 1075 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG, 1076 3), 1077 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG, 1078 3), 1079 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MDEG, 1080 3), 1081 CAL_ENCODE_FLOAT( 1082 gyro_cal->debug_gyro_cal.gyro_var[0] * RAD_TO_MDEG * RAD_TO_MDEG, 1083 3), 1084 CAL_ENCODE_FLOAT( 1085 gyro_cal->debug_gyro_cal.gyro_var[1] * RAD_TO_MDEG * RAD_TO_MDEG, 1086 3), 1087 CAL_ENCODE_FLOAT( 1088 gyro_cal->debug_gyro_cal.gyro_var[2] * RAD_TO_MDEG * RAD_TO_MDEG, 1089 3)); 1090 break; 1091 1092 case MAG_STATS: 1093 if (gyro_cal->debug_gyro_cal.using_mag_sensor) { 1094 CAL_DEBUG_LOG( 1095 debug_tag, 1096 "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET 1097 ", " CAL_FORMAT_6DIGITS_TRIPLET, 1098 (unsigned long int)gyro_cal->debug_calibration_count, 1099 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3), 1100 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3), 1101 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3), 1102 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6), 1103 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6), 1104 CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6)); 1105 } else { 1106 CAL_DEBUG_LOG(debug_tag, 1107 "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, " 1108 "-1.0", 1109 (unsigned long int)gyro_cal->debug_calibration_count); 1110 } 1111 break; 1112 1113 default: 1114 break; 1115 } 1116 } 1117 1118 void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) { 1119 // This is a state machine that controls the reporting out of debug data. 1120 switch (gyro_cal->debug_state) { 1121 case GYRO_IDLE: 1122 // Wait for a trigger and start the debug printout sequence. 1123 if (gyro_cal->debug_print_trigger) { 1124 CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, ""); 1125 CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, "Debug Version: %s", 1126 GYROCAL_DEBUG_VERSION_STRING); 1127 gyro_cal->debug_print_trigger = false; // Resets trigger. 1128 gyro_cal->debug_state = GYRO_PRINT_OFFSET; 1129 } else { 1130 gyro_cal->debug_state = GYRO_IDLE; 1131 } 1132 break; 1133 1134 case GYRO_WAIT_STATE: 1135 // This helps throttle the print statements. 1136 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, 1137 gyro_cal->wait_timer_nanos, 1138 GYROCAL_WAIT_TIME_NANOS)) { 1139 gyro_cal->debug_state = gyro_cal->next_state; 1140 } 1141 break; 1142 1143 case GYRO_PRINT_OFFSET: 1144 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET); 1145 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1146 gyro_cal->next_state = GYRO_PRINT_STILLNESS_DATA; // Sets the next state. 1147 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1148 break; 1149 1150 case GYRO_PRINT_STILLNESS_DATA: 1151 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA); 1152 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1153 gyro_cal->next_state = 1154 GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE; // Sets next state. 1155 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1156 break; 1157 1158 case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE: 1159 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, 1160 SAMPLE_RATE_AND_TEMPERATURE); 1161 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1162 gyro_cal->next_state = 1163 GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN; // Sets next state. 1164 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1165 break; 1166 1167 case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN: 1168 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, 1169 GYRO_MINMAX_STILLNESS_MEAN); 1170 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1171 gyro_cal->next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state. 1172 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1173 break; 1174 1175 case GYRO_PRINT_ACCEL_STATS: 1176 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS); 1177 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1178 gyro_cal->next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state. 1179 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1180 break; 1181 1182 case GYRO_PRINT_GYRO_STATS: 1183 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS); 1184 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1185 gyro_cal->next_state = GYRO_PRINT_MAG_STATS; // Sets the next state. 1186 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1187 break; 1188 1189 case GYRO_PRINT_MAG_STATS: 1190 gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS); 1191 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1192 gyro_cal->next_state = GYRO_IDLE; // Sets the next state. 1193 gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. 1194 break; 1195 1196 default: 1197 // Sends this state machine to its idle state. 1198 gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. 1199 gyro_cal->debug_state = GYRO_IDLE; // Go to idle state. 1200 } 1201 } 1202 #endif // GYRO_CAL_DBG_ENABLED 1203
