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