/* * Copyright (C) 2016 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "calibration/gyroscope/gyro_cal.h" #include #include #include #include "calibration/util/cal_log.h" #include "common/math/vec.h" /////// DEFINITIONS AND MACROS /////////////////////////////////////// // Maximum gyro bias correction (should be set based on expected max bias // of the given sensor). #define MAX_GYRO_BIAS (0.1f) // [rad/sec] // Converts units of radians to milli-degrees. #define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI) #ifdef GYRO_CAL_DBG_ENABLED // The time value used to throttle debug messaging. #define GYROCAL_WAIT_TIME_NANOS (300000000) // Unit conversion: nanoseconds to seconds. #define NANOS_TO_SEC (1.0e-9f) // A debug version label to help with tracking results. #define GYROCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]" // Debug log tag string used to identify debug report output data. #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]" // Debug log tag string used to identify debug tuning output data. #define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]" #endif // GYRO_CAL_DBG_ENABLED /////// FORWARD DECLARATIONS ///////////////////////////////////////// static void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time_nanos); static void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos); static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos); // Data tracker command enumeration. enum GyroCalTrackerCommand { DO_RESET = 0, // Resets the local data used for data tracking. DO_UPDATE_DATA, // Updates the local tracking data. DO_STORE_DATA, // Stores intermediate results for later recall. DO_EVALUATE // Computes and provides the results of the gate function. }; /* * Updates the temperature min/max and mean during the stillness period. Returns * 'true' if the min and max temperature values exceed the range set by * 'temperature_delta_limit_celsius'. * * INPUTS: * gyro_cal: Pointer to the GyroCal data structure. * temperature_celsius: New temperature sample to include. * do_this: Command enumerator that controls function behavior: */ static bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal, float temperature_celsius, enum GyroCalTrackerCommand do_this); /* * Tracks the minimum and maximum gyroscope stillness window means. * Returns 'true' when the difference between gyroscope min and max window * means are outside the range set by 'stillness_mean_delta_limit'. * * INPUTS: * gyro_cal: Pointer to the GyroCal data structure. * do_this: Command enumerator that controls function behavior. */ static bool gyroStillMeanTracker(struct GyroCal* gyro_cal, enum GyroCalTrackerCommand do_this); #ifdef GYRO_CAL_DBG_ENABLED // Defines the type of debug data to print. enum DebugPrintData { OFFSET = 0, STILLNESS_DATA, SAMPLE_RATE_AND_TEMPERATURE, GYRO_MINMAX_STILLNESS_MEAN, ACCEL_STATS, GYRO_STATS, MAG_STATS, ACCEL_STATS_TUNING, GYRO_STATS_TUNING, MAG_STATS_TUNING }; /* * Updates running calculation of the gyro's mean sampling rate. * * Behavior: * 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local * calculation of the sampling rate is copied, and the function returns. * 2) Else, if 'reset_stats' is 'true' then the local estimate is reset and * the function returns. * 3) Otherwise, the local estimate of the mean sampling rates is updated. * * INPUTS: * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update. * timestamp_nanos: Time stamp (nanoseconds). * reset_stats: Flag that signals a reset of the sampling rate estimate. */ static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz, uint64_t timestamp_nanos, bool reset_stats); // Updates the information used for debug printouts. static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); // Helper function for printing out common debug data. static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, enum DebugPrintData print_data); // This conversion function is necessary for Nanohub firmware compilation (i.e., // can't cast a uint64_t to a float directly). This conversion function was // copied from: /third_party/contexthub/firmware/src/floatRt.c static float floatFromUint64(uint64_t v) { uint32_t hi = v >> 32, lo = v; if (!hi) //this is very fast for cases where we fit into a uint32_t return(float)lo; else { return ((float)hi) * 4294967296.0f + (float)lo; } } #ifdef GYRO_CAL_DBG_TUNE_ENABLED // Prints debug information useful for tuning the GyroCal parameters. static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal, uint64_t timestamp_nanos); #endif // GYRO_CAL_DBG_TUNE_ENABLED #endif // GYRO_CAL_DBG_ENABLED /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the gyro calibration data structure. void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, uint64_t max_still_duration_nanos, float bias_x, float bias_y, float bias_z, uint64_t calibration_time_nanos, uint64_t window_time_duration_nanos, float gyro_var_threshold, float gyro_confidence_delta, float accel_var_threshold, float accel_confidence_delta, float mag_var_threshold, float mag_confidence_delta, float stillness_threshold, float stillness_mean_delta_limit, float temperature_delta_limit_celsius, bool gyro_calibration_enable) { // Clear gyro_cal structure memory. memset(gyro_cal, 0, sizeof(struct GyroCal)); // Initialize the stillness detectors. // Gyro parameter input units are [rad/sec]. // Accel parameter input units are [m/sec^2]. // Magnetometer parameter input units are [uT]. gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold, gyro_confidence_delta); gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold, accel_confidence_delta); gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold, mag_confidence_delta); // Reset stillness flag and start timestamp. gyro_cal->prev_still = false; gyro_cal->start_still_time_nanos = 0; // Set the min and max window stillness duration. gyro_cal->min_still_duration_nanos = min_still_duration_nanos; gyro_cal->max_still_duration_nanos = max_still_duration_nanos; // Sets the duration of the stillness processing windows. gyro_cal->window_time_duration_nanos = window_time_duration_nanos; // Set the watchdog timeout duration. gyro_cal->gyro_watchdog_timeout_duration_nanos = 2 * window_time_duration_nanos; // Load the last valid cal from system memory. gyro_cal->bias_x = bias_x; // [rad/sec] gyro_cal->bias_y = bias_y; // [rad/sec] gyro_cal->bias_z = bias_z; // [rad/sec] gyro_cal->calibration_time_nanos = calibration_time_nanos; // Set the stillness threshold required for gyro bias calibration. gyro_cal->stillness_threshold = stillness_threshold; // Current window end-time used to assist in keeping sensor data collection in // sync. Setting this to zero signals that sensor data will be dropped until a // valid end-time is set from the first gyro timestamp received. gyro_cal->stillness_win_endtime_nanos = 0; // Gyro calibrations will be applied (see, gyroCalRemoveBias()). gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0); // Sets the stability limit for the stillness window mean acceptable delta. gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit; // Sets the min/max temperature delta limit for the stillness period. gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius; // Ensures that the data tracking functionality is reset. gyroStillMeanTracker(gyro_cal, DO_RESET); gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:MEMORY]", "sizeof(struct GyroCal): %lu", (unsigned long int)sizeof(struct GyroCal)); if (gyro_cal->gyro_calibration_enable) { CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED."); } else { CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED."); } // Ensures that the gyro sampling rate estimate is reset. gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true); #endif // GYRO_CAL_DBG_ENABLED } // Void pointer in the gyro calibration data structure (doesn't do anything // except prevent compiler warnings). void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; } // Get the most recent bias calibration value. void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, float* bias_z, float* temperature_celsius) { *bias_x = gyro_cal->bias_x; *bias_y = gyro_cal->bias_y; *bias_z = gyro_cal->bias_z; *temperature_celsius = gyro_cal->bias_temperature_celsius; } // Set an initial bias calibration value. void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, float bias_z, uint64_t calibration_time_nanos) { gyro_cal->bias_x = bias_x; gyro_cal->bias_y = bias_y; gyro_cal->bias_z = bias_z; gyro_cal->calibration_time_nanos = calibration_time_nanos; #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:RECALL]", "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d", CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); #endif // GYRO_CAL_DBG_ENABLED } // Remove bias from a gyro measurement [rad/sec]. void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, float* xo, float* yo, float* zo) { if (gyro_cal->gyro_calibration_enable) { *xo = xi - gyro_cal->bias_x; *yo = yi - gyro_cal->bias_y; *zo = zi - gyro_cal->bias_z; } } // Returns true when a new gyro calibration is available. bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal) { bool new_gyro_cal_available = (gyro_cal->gyro_calibration_enable && gyro_cal->new_gyro_cal_available); // Clear the flag. gyro_cal->new_gyro_cal_available = false; return new_gyro_cal_available; } // Update the gyro calibration with gyro data [rad/sec]. void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, float x, float y, float z, float temperature_celsius) { static float latest_temperature_celsius = 0.0f; // Make sure that a valid window end-time is set, and start the watchdog // timer. if (gyro_cal->stillness_win_endtime_nanos <= 0) { gyro_cal->stillness_win_endtime_nanos = sample_time_nanos + gyro_cal->window_time_duration_nanos; // Start the watchdog timer. gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; } // Update the temperature statistics (only on a temperature change). if (NANO_ABS(temperature_celsius - latest_temperature_celsius) > FLT_MIN) { gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA); } #ifdef GYRO_CAL_DBG_ENABLED // Update the gyro sampling rate estimate. gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false); #endif // GYRO_CAL_DBG_ENABLED // Pass gyro data to stillness detector gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect, gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, x, y, z); // Perform a device stillness check, set next window end-time, and // possibly do a gyro bias calibration and stillness detector reset. deviceStillnessCheck(gyro_cal, sample_time_nanos); } // Update the gyro calibration with mag data [micro Tesla]. void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, float x, float y, float z) { // Pass magnetometer data to stillness detector. gyroStillDetUpdate(&gyro_cal->mag_stillness_detect, gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, x, y, z); // Received a magnetometer sample; incorporate it into detection. gyro_cal->using_mag_sensor = true; // Perform a device stillness check, set next window end-time, and // possibly do a gyro bias calibration and stillness detector reset. deviceStillnessCheck(gyro_cal, sample_time_nanos); } // Update the gyro calibration with accel data [m/sec^2]. void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, float x, float y, float z) { // Pass accelerometer data to stillnesss detector. gyroStillDetUpdate(&gyro_cal->accel_stillness_detect, gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, x, y, z); // Perform a device stillness check, set next window end-time, and // possibly do a gyro bias calibration and stillness detector reset. deviceStillnessCheck(gyro_cal, sample_time_nanos); } // TODO(davejacobs): Consider breaking this function up to improve readability. // Checks the state of all stillness detectors to determine // whether the device is "still". void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { bool stillness_duration_exceeded = false; bool stillness_duration_too_short = false; bool min_max_temp_exceeded = false; bool mean_not_stable = false; bool device_is_still = false; float conf_not_rot = 0; float conf_not_accel = 0; float conf_still = 0; // Check the watchdog timer. checkWatchdog(gyro_cal, sample_time_nanos); // Is there enough data to do a stillness calculation? if ((!gyro_cal->mag_stillness_detect.stillness_window_ready && gyro_cal->using_mag_sensor) || !gyro_cal->accel_stillness_detect.stillness_window_ready || !gyro_cal->gyro_stillness_detect.stillness_window_ready) { return; // Not yet, wait for more data. } // Set the next window end-time for the stillness detectors. gyro_cal->stillness_win_endtime_nanos = sample_time_nanos + gyro_cal->window_time_duration_nanos; // Update the confidence scores for all sensors. gyroStillDetCompute(&gyro_cal->accel_stillness_detect); gyroStillDetCompute(&gyro_cal->gyro_stillness_detect); if (gyro_cal->using_mag_sensor) { gyroStillDetCompute(&gyro_cal->mag_stillness_detect); } else { // Not using magnetometer, force stillness confidence to 100%. gyro_cal->mag_stillness_detect.stillness_confidence = 1.0f; } // Updates the mean tracker data. gyroStillMeanTracker(gyro_cal, DO_UPDATE_DATA); // Determine motion confidence scores (rotation, accelerating, and stillness). conf_not_rot = gyro_cal->gyro_stillness_detect.stillness_confidence * gyro_cal->mag_stillness_detect.stillness_confidence; conf_not_accel = gyro_cal->accel_stillness_detect.stillness_confidence; conf_still = conf_not_rot * conf_not_accel; // Evaluate the mean and temperature gate functions. mean_not_stable = gyroStillMeanTracker(gyro_cal, DO_EVALUATE); min_max_temp_exceeded = gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_EVALUATE); // Determines if the device is currently still. device_is_still = (conf_still > gyro_cal->stillness_threshold) && !mean_not_stable && !min_max_temp_exceeded ; if (device_is_still) { // Device is "still" logic: // If not previously still, then record the start time. // If stillness period is too long, then do a calibration. // Otherwise, continue collecting stillness data. // If device was not previously still, set new start timestamp. if (!gyro_cal->prev_still) { // Record the starting timestamp of the current stillness window. // This enables the calculation of total duration of the stillness period. gyro_cal->start_still_time_nanos = gyro_cal->gyro_stillness_detect.window_start_time; } // Check to see if current stillness period exceeds the desired limit. stillness_duration_exceeded = ((gyro_cal->gyro_stillness_detect.last_sample_time - gyro_cal->start_still_time_nanos) > gyro_cal->max_still_duration_nanos); // Track the new stillness mean and temperature data. gyroStillMeanTracker(gyro_cal, DO_STORE_DATA); gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_STORE_DATA); if (stillness_duration_exceeded) { // The current stillness has gone too long. Do a calibration with the // current data and reset. // Updates the gyro bias estimate with the current window data and // resets the stats. gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); // Resets the local calculations because the stillness period is over. gyroStillMeanTracker(gyro_cal, DO_RESET); gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); // Computes a new gyro offset estimate. computeGyroCal(gyro_cal, gyro_cal->gyro_stillness_detect.last_sample_time); #ifdef GYRO_CAL_DBG_ENABLED // Resets the sampling rate estimate. gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true); #endif // GYRO_CAL_DBG_ENABLED // Update stillness flag. Force the start of a new stillness period. gyro_cal->prev_still = false; } else { // Continue collecting stillness data. // Extend the stillness period. gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/false); gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/false); gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false); // Update the stillness flag. gyro_cal->prev_still = true; } } else { // Device is NOT still; motion detected. // If device was previously still and the total stillness duration is not // "too short", then do a calibration with the data accumulated thus far. stillness_duration_too_short = ((gyro_cal->gyro_stillness_detect.window_start_time - gyro_cal->start_still_time_nanos) < gyro_cal->min_still_duration_nanos); if (gyro_cal->prev_still && !stillness_duration_too_short) { computeGyroCal(gyro_cal, gyro_cal->gyro_stillness_detect.window_start_time); } // Reset the stillness detectors and the stats. gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); // Resets the temperature and sensor mean data. gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); gyroStillMeanTracker(gyro_cal, DO_RESET); #ifdef GYRO_CAL_DBG_ENABLED // Resets the sampling rate estimate. gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true); #endif // GYRO_CAL_DBG_ENABLED // Update stillness flag. gyro_cal->prev_still = false; } // Reset the watchdog timer after we have processed data. gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; } // Calculates a new gyro bias offset calibration value. void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { // Check to see if new calibration values is within acceptable range. if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) { #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:REJECT]", "Offset|Temp|Time [mdps|C|nsec]: %s%d.%06d, %s%d.%06d, " "%s%d.%06d, %s%d.%06d, %llu", CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 6), (unsigned long long int)calibration_time_nanos); #endif // GYRO_CAL_DBG_ENABLED // Outside of range. Ignore, reset, and continue. return; } // Record the new gyro bias offset calibration. gyro_cal->bias_x = gyro_cal->gyro_stillness_detect.prev_mean_x; gyro_cal->bias_y = gyro_cal->gyro_stillness_detect.prev_mean_y; gyro_cal->bias_z = gyro_cal->gyro_stillness_detect.prev_mean_z; // Store the calibration temperature (using the mean temperature over the // "stillness" period). gyro_cal->bias_temperature_celsius = gyro_cal->temperature_mean_celsius; // Store the calibration time stamp. gyro_cal->calibration_time_nanos = calibration_time_nanos; // Record the final stillness confidence. gyro_cal->stillness_confidence = gyro_cal->gyro_stillness_detect.prev_stillness_confidence * gyro_cal->accel_stillness_detect.prev_stillness_confidence * gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Set flag to indicate a new gyro calibration value is available. gyro_cal->new_gyro_cal_available = true; #ifdef GYRO_CAL_DBG_ENABLED // Increment the total count of calibration updates. gyro_cal->debug_calibration_count++; // Update the calibration debug information and trigger a printout. gyroCalUpdateDebug(gyro_cal); #endif } // Check for a watchdog timeout condition. void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { bool watchdog_timeout; // Check for initialization of the watchdog time (=0). if (gyro_cal->gyro_watchdog_start_nanos <= 0) { return; } // Check for the watchdog timeout condition (i.e., the time elapsed since the // last received sample has exceeded the allowed watchdog duration). watchdog_timeout = (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos + gyro_cal->gyro_watchdog_start_nanos); // If a timeout occurred then reset to known good state. if (watchdog_timeout) { // Reset stillness detectors and restart data capture. gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); // Resets the temperature and sensor mean data. gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); gyroStillMeanTracker(gyro_cal, DO_RESET); #ifdef GYRO_CAL_DBG_ENABLED // Resets the sampling rate estimate. gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true); #endif // GYRO_CAL_DBG_ENABLED // Resets the stillness window end-time. gyro_cal->stillness_win_endtime_nanos = 0; // Force stillness confidence to zero. gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0; gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0; gyro_cal->mag_stillness_detect.prev_stillness_confidence = 0; gyro_cal->stillness_confidence = 0; gyro_cal->prev_still = false; // If there are no magnetometer samples being received then // operate the calibration algorithm without this sensor. if (!gyro_cal->mag_stillness_detect.stillness_window_ready && gyro_cal->using_mag_sensor) { gyro_cal->using_mag_sensor = false; } // Assert watchdog timeout flags. gyro_cal->gyro_watchdog_timeout |= watchdog_timeout; gyro_cal->gyro_watchdog_start_nanos = 0; #ifdef GYRO_CAL_DBG_ENABLED gyro_cal->debug_watchdog_count++; CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp [nsec]: %lu, %llu", (unsigned long int)gyro_cal->debug_watchdog_count, (unsigned long long int)sample_time_nanos); #endif // GYRO_CAL_DBG_ENABLED } } // TODO(davejacobs) -- Combine the following two functions into one or consider // implementing a separate helper module for tracking the temperature and mean // statistics. bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal, float temperature_celsius, enum GyroCalTrackerCommand do_this) { // This is used for local calculations of the running mean. static float mean_accumulator = 0.0f; static float temperature_min_max_celsius[2] = {0.0f, 0.0f}; static size_t num_points = 0; bool min_max_temp_exceeded = false; switch (do_this) { case DO_RESET: // Resets the mean accumulator. num_points = 0; mean_accumulator = 0.0f; // Initializes the min/max temperatures values. temperature_min_max_celsius[0] = FLT_MAX; temperature_min_max_celsius[1] = -1.0f * (FLT_MAX - 1.0f); break; case DO_UPDATE_DATA: // Does the mean accumulation. mean_accumulator += temperature_celsius; num_points++; // Tracks the min and max temperature values. if (temperature_min_max_celsius[0] > temperature_celsius) { temperature_min_max_celsius[0] = temperature_celsius; } if (temperature_min_max_celsius[1] < temperature_celsius) { temperature_min_max_celsius[1] = temperature_celsius; } break; case DO_STORE_DATA: // Store the most recent "stillness" mean data to the GyroCal data // structure. This functionality allows previous results to be recalled // when the device suddenly becomes "not still". if (num_points > 0) { memcpy(gyro_cal->temperature_min_max_celsius, temperature_min_max_celsius, 2 * sizeof(float)); gyro_cal->temperature_mean_celsius = mean_accumulator / num_points; } break; case DO_EVALUATE: // Determines if the min/max delta exceeded the set limit. if (num_points > 0) { min_max_temp_exceeded = (temperature_min_max_celsius[1] - temperature_min_max_celsius[0]) > gyro_cal->temperature_delta_limit_celsius; #ifdef GYRO_CAL_DBG_ENABLED if (min_max_temp_exceeded) { CAL_DEBUG_LOG( "[GYRO_CAL:TEMP_GATE]", "Exceeded the max temperature variation during stillness."); } #endif // GYRO_CAL_DBG_ENABLED } break; default: break; } return min_max_temp_exceeded; } bool gyroStillMeanTracker(struct GyroCal* gyro_cal, enum GyroCalTrackerCommand do_this) { static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f}; static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f}; bool mean_not_stable = false; size_t i; switch (do_this) { case DO_RESET: // Resets the min/max window mean values to a default value. for (i = 0; i < 3; i++) { gyro_winmean_min[i] = FLT_MAX; gyro_winmean_max[i] = -1.0f * (FLT_MAX - 1.0f); } break; case DO_UPDATE_DATA: // Computes the min/max window mean values. if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) { gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x; } if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) { gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x; } if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) { gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y; } if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) { gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y; } if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) { gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z; } if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) { gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z; } break; case DO_STORE_DATA: // Store the most recent "stillness" mean data to the GyroCal data // structure. This functionality allows previous results to be recalled // when the device suddenly becomes "not still". memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float)); memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float)); break; case DO_EVALUATE: // Performs the stability check and returns the 'true' if the difference // between min/max window mean value is outside the stable range. for (i = 0; i < 3; i++) { mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) > gyro_cal->stillness_mean_delta_limit; } #ifdef GYRO_CAL_DBG_ENABLED if (mean_not_stable) { CAL_DEBUG_LOG("[GYRO_CAL:MEAN_STABILITY_GATE]", "Exceeded the max variation in the gyro's stillness " "window mean values."); } #endif // GYRO_CAL_DBG_ENABLED break; default: break; } return mean_not_stable; } #ifdef GYRO_CAL_DBG_ENABLED void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz, uint64_t timestamp_nanos, bool reset_stats) { // This is used for local calculations of average sampling rate. static uint64_t last_timestamp_nanos = 0; static uint64_t time_delta_accumulator = 0; static size_t num_samples = 0; // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads // out the estimate of the sampling rate. if (debug_mean_sampling_rate_hz) { if (num_samples > 1 && time_delta_accumulator > 0) { // Computes the final mean calculation. *debug_mean_sampling_rate_hz = num_samples / (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC); } else { // Not enough samples to compute a valid sample rate estimate. Indicate // this with a -1 value. *debug_mean_sampling_rate_hz = -1.0f; } reset_stats = true; } // Resets the sampling rate mean estimator data. if (reset_stats) { last_timestamp_nanos = 0; time_delta_accumulator = 0; num_samples = 0; return; } // Skip adding this data to the accumulator if: // 1. A bad timestamp was received (i.e., time not monotonic). // 2. 'last_timestamp_nanos' is zero. if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) { last_timestamp_nanos = timestamp_nanos; return; } // Increments the number of samples. num_samples++; // Accumulate the time steps. time_delta_accumulator += timestamp_nanos - last_timestamp_nanos; last_timestamp_nanos = timestamp_nanos; } void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { // Only update this data if debug printing is not currently in progress // (i.e., don't want to risk overwriting debug information that is actively // being reported). if (gyro_cal->debug_state != GYRO_IDLE) { return; } // Probability of stillness (acc, rot, still), duration, timestamp. gyro_cal->debug_gyro_cal.accel_stillness_conf = gyro_cal->accel_stillness_detect.prev_stillness_confidence; gyro_cal->debug_gyro_cal.gyro_stillness_conf = gyro_cal->gyro_stillness_detect.prev_stillness_confidence; gyro_cal->debug_gyro_cal.mag_stillness_conf = gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Magnetometer usage. gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor; // Stillness start, stop, and duration times. gyro_cal->debug_gyro_cal.start_still_time_nanos = gyro_cal->start_still_time_nanos; gyro_cal->debug_gyro_cal.end_still_time_nanos = gyro_cal->calibration_time_nanos; gyro_cal->debug_gyro_cal.stillness_duration_nanos = gyro_cal->calibration_time_nanos - gyro_cal->start_still_time_nanos; // Records the current calibration values. gyro_cal->debug_gyro_cal.calibration[0] = gyro_cal->bias_x; gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y; gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z; // Records the mean gyroscope sampling rate. gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0, /*reset_stats=*/true); // Records the min/max and mean temperature values. gyro_cal->debug_gyro_cal.temperature_mean_celsius = gyro_cal->temperature_mean_celsius; memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius, gyro_cal->temperature_min_max_celsius, 2 * sizeof(float)); // Records the min/max gyroscope window stillness mean values. memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min, 3 * sizeof(float)); memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max, 3 * sizeof(float)); // Records the previous stillness window means. gyro_cal->debug_gyro_cal.accel_mean[0] = gyro_cal->accel_stillness_detect.prev_mean_x; gyro_cal->debug_gyro_cal.accel_mean[1] = gyro_cal->accel_stillness_detect.prev_mean_y; gyro_cal->debug_gyro_cal.accel_mean[2] = gyro_cal->accel_stillness_detect.prev_mean_z; gyro_cal->debug_gyro_cal.gyro_mean[0] = gyro_cal->gyro_stillness_detect.prev_mean_x; gyro_cal->debug_gyro_cal.gyro_mean[1] = gyro_cal->gyro_stillness_detect.prev_mean_y; gyro_cal->debug_gyro_cal.gyro_mean[2] = gyro_cal->gyro_stillness_detect.prev_mean_z; gyro_cal->debug_gyro_cal.mag_mean[0] = gyro_cal->mag_stillness_detect.prev_mean_x; gyro_cal->debug_gyro_cal.mag_mean[1] = gyro_cal->mag_stillness_detect.prev_mean_y; gyro_cal->debug_gyro_cal.mag_mean[2] = gyro_cal->mag_stillness_detect.prev_mean_z; // Records the variance data. // NOTE: These statistics include the final captured window, which may be // outside of the "stillness" period. Therefore, these values may exceed the // stillness thresholds. gyro_cal->debug_gyro_cal.accel_var[0] = gyro_cal->accel_stillness_detect.win_var_x; gyro_cal->debug_gyro_cal.accel_var[1] = gyro_cal->accel_stillness_detect.win_var_y; gyro_cal->debug_gyro_cal.accel_var[2] = gyro_cal->accel_stillness_detect.win_var_z; gyro_cal->debug_gyro_cal.gyro_var[0] = gyro_cal->gyro_stillness_detect.win_var_x; gyro_cal->debug_gyro_cal.gyro_var[1] = gyro_cal->gyro_stillness_detect.win_var_y; gyro_cal->debug_gyro_cal.gyro_var[2] = gyro_cal->gyro_stillness_detect.win_var_z; gyro_cal->debug_gyro_cal.mag_var[0] = gyro_cal->mag_stillness_detect.win_var_x; gyro_cal->debug_gyro_cal.mag_var[1] = gyro_cal->mag_stillness_detect.win_var_y; gyro_cal->debug_gyro_cal.mag_var[2] = gyro_cal->mag_stillness_detect.win_var_z; // Trigger a printout of the debug information. gyro_cal->debug_print_trigger = true; } void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, enum DebugPrintData print_data) { // Prints out the desired debug data. float mag_data; switch (print_data) { case OFFSET: CAL_DEBUG_LOG(debug_tag, "Cal#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, " "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3), (unsigned long long int) gyro_cal->debug_gyro_cal.end_still_time_nanos); break; case STILLNESS_DATA: mag_data = (gyro_cal->debug_gyro_cal.using_mag_sensor) ? gyro_cal->debug_gyro_cal.mag_stillness_conf : -1.0f; // Signals that magnetometer was not used. CAL_DEBUG_LOG( debug_tag, "Cal#|Start|End|Confidence [nsec]: %lu, %llu, %llu, " "%s%d.%03d, %s%d.%03d, %s%d.%03d", (unsigned long int)gyro_cal->debug_calibration_count, (unsigned long long int) gyro_cal->debug_gyro_cal.start_still_time_nanos, (unsigned long long int)gyro_cal->debug_gyro_cal.end_still_time_nanos, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3), CAL_ENCODE_FLOAT(mag_data, 3)); break; case SAMPLE_RATE_AND_TEMPERATURE: CAL_DEBUG_LOG( debug_tag, "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, " "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] - gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 4), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3)); break; case GYRO_MINMAX_STILLNESS_MEAN: CAL_DEBUG_LOG( debug_tag, "Cal#|Gyro Peak Stillness Variation [mdps]: %lu, %s%d.%06d, " "%s%d.%06d, %s%d.%06d", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] - gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] - gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] - gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) * RAD_TO_MILLI_DEGREES, 6)); break; case ACCEL_STATS: CAL_DEBUG_LOG( debug_tag, "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, " "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 8)); break; case GYRO_STATS: CAL_DEBUG_LOG( debug_tag, "Cal#|Gyro Mean|Var [mdps|(rad/sec)^2]: %lu, %s%d.%06d, " "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8)); break; case MAG_STATS: if (gyro_cal->debug_gyro_cal.using_mag_sensor) { CAL_DEBUG_LOG(debug_tag, "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%06d, " "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d", (unsigned long int)gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 8), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 8)); } else { CAL_DEBUG_LOG(debug_tag, "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, " "-1.0", (unsigned long int)gyro_cal->debug_calibration_count); } break; #ifdef GYRO_CAL_DBG_TUNE_ENABLED case ACCEL_STATS_TUNING: CAL_DEBUG_LOG( debug_tag, "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%06d, " "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d", CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 6), CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 6), CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 6), CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8), CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8), CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8)); break; case GYRO_STATS_TUNING: CAL_DEBUG_LOG( debug_tag, "Gyro Mean|Var [mdps|(rad/sec)^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, " "%s%d.%08d, %s%d.%08d, %s%d.%08d", CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MILLI_DEGREES, 6), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8), CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8)); break; case MAG_STATS_TUNING: if (gyro_cal->using_mag_sensor) { CAL_DEBUG_LOG( debug_tag, "Mag Mean|Var [uT|uT^2]: %s%d.%06d, %s%d.%06d, %s%d.%06d, " "%s%d.%08d, %s%d.%08d, %s%d.%08d", CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 6), CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 6), CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 6), CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8), CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8), CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8)); } else { CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Mag Mean|Var [uT|uT^2]: 0, 0, 0, -1.0, -1.0, -1.0"); } break; #endif // GYRO_CAL_DBG_TUNE_ENABLED default: break; } } void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) { static enum GyroCalDebugState next_state = GYRO_IDLE; static uint64_t wait_timer_nanos = 0; // This is a state machine that controls the reporting out of debug data. switch (gyro_cal->debug_state) { case GYRO_IDLE: // Wait for a trigger and start the debug printout sequence. if (gyro_cal->debug_print_trigger) { CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, ""); CAL_DEBUG_LOG(GYROCAL_REPORT_TAG, "Debug Version: %s", GYROCAL_DEBUG_VERSION_STRING); gyro_cal->debug_print_trigger = false; // Resets trigger. gyro_cal->debug_state = GYRO_PRINT_OFFSET; } else { gyro_cal->debug_state = GYRO_IDLE; } break; case GYRO_WAIT_STATE: // This helps throttle the print statements. if (timestamp_nanos >= GYROCAL_WAIT_TIME_NANOS + wait_timer_nanos) { gyro_cal->debug_state = next_state; } break; case GYRO_PRINT_OFFSET: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_STILLNESS_DATA; // Sets the next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_STILLNESS_DATA: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE; // Sets next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, SAMPLE_RATE_AND_TEMPERATURE); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN; // Sets next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_MINMAX_STILLNESS_MEAN); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_ACCEL_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_GYRO_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_MAG_STATS; // Sets the next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_MAG_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_IDLE; // Sets the next state. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; default: // Sends this state machine to its idle state. wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state. } #ifdef GYRO_CAL_DBG_TUNE_ENABLED if (gyro_cal->debug_state == GYRO_IDLE) { // This check keeps the tuning printout from interleaving with the above // debug print data. gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos); } #endif // GYRO_CAL_DBG_TUNE_ENABLED } #ifdef GYRO_CAL_DBG_TUNE_ENABLED void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal, uint64_t timestamp_nanos) { static enum GyroCalDebugState debug_state = GYRO_IDLE; static enum GyroCalDebugState next_state = GYRO_IDLE; static uint64_t wait_timer_nanos = 0; // Output sensor variance levels to assist with tuning thresholds. // i. Within the first 300 seconds of boot: output interval = 5 // seconds. // ii. Thereafter: output interval is 60 seconds. bool condition_i = ((timestamp_nanos <= 300000000000) && (timestamp_nanos > 5000000000 + wait_timer_nanos)); // nsec bool condition_ii = (timestamp_nanos > 60000000000 + wait_timer_nanos); // This is a state machine that controls the reporting out of tuning data. switch (debug_state) { case GYRO_IDLE: // Wait for a trigger and start the data tuning printout sequence. if (condition_i || condition_ii) { CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Temp [C]: %s%d.%03d", CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3)); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state. debug_state = GYRO_WAIT_STATE; // First, go to wait state. } else { debug_state = GYRO_IDLE; } break; case GYRO_WAIT_STATE: // This helps throttle the print statements. if (timestamp_nanos >= GYROCAL_WAIT_TIME_NANOS + wait_timer_nanos) { debug_state = next_state; } break; case GYRO_PRINT_ACCEL_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS_TUNING); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state. debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_GYRO_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS_TUNING); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_PRINT_MAG_STATS; // Sets the next state. debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; case GYRO_PRINT_MAG_STATS: gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS_TUNING); wait_timer_nanos = timestamp_nanos; // Starts the wait timer. next_state = GYRO_IDLE; // Sets the next state. debug_state = GYRO_WAIT_STATE; // First, go to wait state. break; default: // Sends this state machine to its idle state. wait_timer_nanos = timestamp_nanos; // Starts the wait timer. debug_state = GYRO_IDLE; } } #endif // GYRO_CAL_DBG_TUNE_ENABLED #endif // GYRO_CAL_DBG_ENABLED