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