/* * 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 #ifdef GYRO_CAL_DBG_ENABLED #include "calibration/util/cal_log.h" #endif // GYRO_CAL_DBG_ENABLED #include "common/math/macros.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.2f) // [rad/sec] // Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and // resets when exceeded. #define GYRO_WATCHDOG_TIMEOUT_NANOS (SEC_TO_NANOS(5)) #ifdef GYRO_CAL_DBG_ENABLED // The time value used to throttle debug messaging. #define GYROCAL_WAIT_TIME_NANOS (MSEC_TO_NANOS(100)) // A debug version label to help with tracking results. #define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]" // Parameters used for sample rate estimation. #define GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS (100) #define GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC (1.0f) // Debug log tag string used to identify debug report output data. #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]" #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 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); #endif // GYRO_CAL_DBG_ENABLED /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the gyro calibration data structure. void gyroCalInit(struct GyroCal* gyro_cal, const struct GyroCalParameters* parameters) { // 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, parameters->gyro_var_threshold, parameters->gyro_confidence_delta); gyroStillDetInit(&gyro_cal->accel_stillness_detect, parameters->accel_var_threshold, parameters->accel_confidence_delta); gyroStillDetInit(&gyro_cal->mag_stillness_detect, parameters->mag_var_threshold, parameters->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 = parameters->min_still_duration_nanos; gyro_cal->max_still_duration_nanos = parameters->max_still_duration_nanos; // Sets the duration of the stillness processing windows. gyro_cal->window_time_duration_nanos = parameters->window_time_duration_nanos; // Set the watchdog timeout duration. gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS; // Load the last valid cal from system memory. gyro_cal->bias_x = parameters->bias_x; // [rad/sec] gyro_cal->bias_y = parameters->bias_y; // [rad/sec] gyro_cal->bias_z = parameters->bias_z; // [rad/sec] gyro_cal->calibration_time_nanos = parameters->calibration_time_nanos; // Set the stillness threshold required for gyro bias calibration. gyro_cal->stillness_threshold = parameters->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 = (parameters->gyro_calibration_enable > 0); // Sets the stability limit for the stillness window mean acceptable delta. gyro_cal->stillness_mean_delta_limit = parameters->stillness_mean_delta_limit; // Sets the min/max temperature delta limit for the stillness period. gyro_cal->temperature_delta_limit_celsius = parameters->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 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."); } // Initializes the gyro sampling rate estimator. sampleRateEstimatorInit(&gyro_cal->debug_gyro_cal.sample_rate_estimator, GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS, GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC); #endif // GYRO_CAL_DBG_ENABLED } // 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, uint64_t* calibration_time_nanos) { *bias_x = gyro_cal->bias_x; *bias_y = gyro_cal->bias_y; *bias_z = gyro_cal->bias_z; *calibration_time_nanos = gyro_cal->calibration_time_nanos; *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, float temperature_celsius, 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; gyro_cal->bias_temperature_celsius = temperature_celsius; #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]", "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %" PRIu64, CAL_ENCODE_FLOAT(bias_x * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(bias_y * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(bias_z * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(temperature_celsius, 3), calibration_time_nanos); #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) { // 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. gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA); #ifdef GYRO_CAL_DBG_ENABLED // Update the gyro sampling rate estimate. sampleRateEstimatorUpdate(&gyro_cal->debug_gyro_cal.sample_rate_estimator, sample_time_nanos); #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: 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); // 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); // 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]: " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %" PRIu64, CAL_ENCODE_FLOAT( gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3), 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; } // Checks for the following watchdog timeout conditions: // i. The current timestamp has exceeded the allowed watchdog duration. // ii. A timestamp was received that has jumped backwards by more than the // allowed watchdog duration (e.g., timestamp clock roll-over). watchdog_timeout = (sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos + gyro_cal->gyro_watchdog_start_nanos) || (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) { #ifdef GYRO_CAL_DBG_ENABLED gyro_cal->debug_watchdog_count++; if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) { CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64 ", -%" PRIu64, gyro_cal->debug_watchdog_count, sample_time_nanos, gyro_cal->gyro_watchdog_start_nanos - sample_time_nanos); } else { CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64 ", %" PRIu64, gyro_cal->debug_watchdog_count, sample_time_nanos, sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos); } #endif // GYRO_CAL_DBG_ENABLED // 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); // 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_start_nanos = 0; } } // TODO -- 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) { bool min_max_temp_exceeded = false; switch (do_this) { case DO_RESET: // Resets the mean accumulator. gyro_cal->temperature_mean_tracker.num_points = 0; gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f; // Initializes the min/max temperatures values. gyro_cal->temperature_mean_tracker.temperature_min_celsius = FLT_MAX; gyro_cal->temperature_mean_tracker.temperature_max_celsius = -FLT_MAX; break; case DO_UPDATE_DATA: // Does the mean accumulation. gyro_cal->temperature_mean_tracker.mean_accumulator += temperature_celsius; gyro_cal->temperature_mean_tracker.num_points++; // Tracks the min, max, and latest temperature values. gyro_cal->temperature_mean_tracker.latest_temperature_celsius = temperature_celsius; if (gyro_cal->temperature_mean_tracker.temperature_min_celsius > temperature_celsius) { gyro_cal->temperature_mean_tracker.temperature_min_celsius = temperature_celsius; } if (gyro_cal->temperature_mean_tracker.temperature_max_celsius < temperature_celsius) { gyro_cal->temperature_mean_tracker.temperature_max_celsius = temperature_celsius; } break; case DO_STORE_DATA: // Store the most recent temperature statistics data to the GyroCal data // structure. This functionality allows previous results to be recalled // when the device suddenly becomes "not still". if (gyro_cal->temperature_mean_tracker.num_points > 0) { gyro_cal->temperature_mean_celsius = gyro_cal->temperature_mean_tracker.mean_accumulator / gyro_cal->temperature_mean_tracker.num_points; } else { gyro_cal->temperature_mean_celsius = gyro_cal->temperature_mean_tracker.latest_temperature_celsius; #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:TEMP_GATE]", "Insufficient statistics (num_points = 0), using latest " "measured temperature as the mean value."); #endif // GYRO_CAL_DBG_ENABLED } #ifdef GYRO_CAL_DBG_ENABLED // Records the min/max and mean temperature values for debug purposes. gyro_cal->debug_gyro_cal.temperature_mean_celsius = gyro_cal->temperature_mean_celsius; gyro_cal->debug_gyro_cal.temperature_min_celsius = gyro_cal->temperature_mean_tracker.temperature_min_celsius; gyro_cal->debug_gyro_cal.temperature_max_celsius = gyro_cal->temperature_mean_tracker.temperature_max_celsius; #endif break; case DO_EVALUATE: // Determines if the min/max delta exceeded the set limit. if (gyro_cal->temperature_mean_tracker.num_points > 0) { min_max_temp_exceeded = (gyro_cal->temperature_mean_tracker.temperature_max_celsius - gyro_cal->temperature_mean_tracker.temperature_min_celsius) > 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) { bool mean_not_stable = false; switch (do_this) { case DO_RESET: // Resets the min/max window mean values to a default value. for (size_t i = 0; i < 3; i++) { gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX; gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX; } break; case DO_UPDATE_DATA: // Computes the min/max window mean values. if (gyro_cal->window_mean_tracker.gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) { gyro_cal->window_mean_tracker.gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x; } if (gyro_cal->window_mean_tracker.gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) { gyro_cal->window_mean_tracker.gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x; } if (gyro_cal->window_mean_tracker.gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) { gyro_cal->window_mean_tracker.gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y; } if (gyro_cal->window_mean_tracker.gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) { gyro_cal->window_mean_tracker.gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y; } if (gyro_cal->window_mean_tracker.gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) { gyro_cal->window_mean_tracker.gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z; } if (gyro_cal->window_mean_tracker.gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) { gyro_cal->window_mean_tracker.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_cal->window_mean_tracker.gyro_winmean_min, sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min)); memcpy(gyro_cal->gyro_winmean_max, gyro_cal->window_mean_tracker.gyro_winmean_max, sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max)); 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 (size_t i = 0; i < 3; i++) { mean_not_stable |= (gyro_cal->window_mean_tracker.gyro_winmean_max[i] - gyro_cal->window_mean_tracker.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]", "Variation Limit|Delta [mDPS]: " CAL_FORMAT_3DIGITS " | " CAL_FORMAT_3DIGITS_TRIPLET, CAL_ENCODE_FLOAT(gyro_cal->stillness_mean_delta_limit * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( (gyro_cal->window_mean_tracker.gyro_winmean_max[0] - gyro_cal->window_mean_tracker.gyro_winmean_min[0]) * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( (gyro_cal->window_mean_tracker.gyro_winmean_max[1] - gyro_cal->window_mean_tracker.gyro_winmean_min[1]) * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( (gyro_cal->window_mean_tracker.gyro_winmean_max[2] - gyro_cal->window_mean_tracker.gyro_winmean_min[2]) * RAD_TO_MDEG, 3)); } #endif // GYRO_CAL_DBG_ENABLED break; default: break; } return mean_not_stable; } #ifdef GYRO_CAL_DBG_ENABLED 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 min/max gyroscope window stillness mean values. memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min, sizeof(gyro_cal->gyro_winmean_min)); memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max, sizeof(gyro_cal->gyro_winmean_max)); // 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]: " "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %" PRIu64, gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3), 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#|Stillness|Confidence [nsec]: %zu, " "%" PRIu64 ", " CAL_FORMAT_3DIGITS_TRIPLET, gyro_cal->debug_calibration_count, gyro_cal->debug_gyro_cal.end_still_time_nanos - gyro_cal->debug_gyro_cal.start_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]: " "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", " CAL_FORMAT_3DIGITS, 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_celsius, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius - gyro_cal->debug_gyro_cal.temperature_min_celsius, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.sample_rate_estimator .mean_sampling_rate_estimate_hz, 3)); break; case GYRO_MINMAX_STILLNESS_MEAN: CAL_DEBUG_LOG( debug_tag, "Cal#|Gyro Peak Stillness Variation [mDPS]: " "%zu, " CAL_FORMAT_3DIGITS_TRIPLET, 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_MDEG, 3), CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] - gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] - gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) * RAD_TO_MDEG, 3)); break; case ACCEL_STATS: CAL_DEBUG_LOG(debug_tag, "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: " "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS_TRIPLET, gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6)); break; case GYRO_STATS: CAL_DEBUG_LOG( debug_tag, "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS_TRIPLET, gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_var[0] * RAD_TO_MDEG * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_var[1] * RAD_TO_MDEG * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.gyro_var[2] * RAD_TO_MDEG * RAD_TO_MDEG, 3)); 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]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS_TRIPLET, gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6)); } else { CAL_DEBUG_LOG(debug_tag, "Cal#|Mag Mean|Var [uT|uT^2]: %zu, 0, 0, 0, -1.0, -1.0, " "-1.0", gyro_cal->debug_calibration_count); } break; default: break; } } void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) { // 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 (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, gyro_cal->wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) { gyro_cal->debug_state = gyro_cal->next_state; } break; case GYRO_PRINT_OFFSET: gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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); gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->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. gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. gyro_cal->debug_state = GYRO_IDLE; // Go to idle state. } } #endif // GYRO_CAL_DBG_ENABLED