Google Git
Sign in
android / device / google / contexthub / d1942657e169dcf8beee6c617b13972fdf83548b / . / firmware / os / algos / calibration / gyroscope / gyro_cal.c
blob: adc9efc80cb91e9fcda8515cffd39b4815d9b46d [file] [log] [blame]
/*
* 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 <float.h>
#include <inttypes.h>
#include <math.h>
#include <string.h>
#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