path: root/firmware/os/algos/calibration/gyroscope/gyro_cal.c

/*
 * 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 "calibration/util/cal_log.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.096f  // [rad/sec]

// Helper constants for converting units.
#define RAD_TO_MDEG (float)(1e3 * 180.0 / M_PI)
#define GRAVITY_TO_G (float)(1.0 / 9.80665)

/////// FORWARD DECLARATIONS /////////////////////////////////////////

static void deviceStillnessCheck(struct GyroCal* gyro_cal,
                                 uint64_t sample_time);

static void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time);

static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time);

#ifdef GYRO_CAL_DBG_ENABLED
static void gyroCalUpdateDebug(struct GyroCal* gyro_cal,
                               struct DebugGyroCal* debug_gyro_cal);

static void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal,
                                  uint64_t sample_time);

static void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data);
#endif

/////// FUNCTION DEFINITIONS /////////////////////////////////////////

// Initialize the gyro calibration data structure.
void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
                 uint64_t max_still_duration, float bias_x, float bias_y,
                 float bias_z, uint64_t calibration_time,
                 uint64_t window_time_duration, 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,
                 int remove_bias_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 = 0;

  // Set the min and max window stillness duration.
  gyro_cal->min_still_duration = min_still_duration;
  gyro_cal->max_still_duration = max_still_duration;

  // Sets the duration of the stillness processing windows.
  gyro_cal->window_time_duration = window_time_duration;

  // Set the watchdog timeout duration.
  gyro_cal->gyro_watchdog_timeout_duration = 2 * window_time_duration;

  // 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 = calibration_time;

  // 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 = 0;

  // Gyro calibrations will be applied (see, gyroCalRemoveBias()).
  gyro_cal->gyro_calibration_enable = (remove_bias_enable > 0);

#ifdef GYRO_CAL_DBG_ENABLED
  CAL_DEBUG_LOG("[GYRO_CAL]", "Gyro Cal: Initialized.");
  CAL_DEBUG_LOG("[GYRO_CAL]",
                "GyroCalInit = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));

  // Initialize the debug report state machine.
  gyro_cal->gyro_debug_state = -1;
#endif
}

// Void all pointers in the gyro calibration data structure
// (prevents 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) {
  if (gyro_cal->gyro_calibration_enable) {
    *bias_x = gyro_cal->bias_x;
    *bias_y = gyro_cal->bias_y;
    *bias_z = gyro_cal->bias_z;
  }
}

// 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) {
  gyro_cal->bias_x = bias_x;
  gyro_cal->bias_y = bias_y;
  gyro_cal->bias_z = bias_z;
  gyro_cal->calibration_time = calibration_time;

#ifdef GYRO_CAL_DBG_ENABLED
  CAL_DEBUG_LOG("[GYRO_CAL]",
                "GyroCalSetBias = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
                CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
                CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
                CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
#endif
}

// 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, float x,
                       float y, float z, float temperature) {
  // Make sure that a valid window end time is set,
  // and start the watchdog timer.
  if (gyro_cal->stillness_win_endtime <= 0) {
    gyro_cal->stillness_win_endtime =
        sample_time + gyro_cal->window_time_duration;

    // Start the watchdog timer.
    gyro_cal->gyro_watchdog_start = sample_time;
  }

  // Record the latest temperture sample.
  gyro_cal->latest_temperature_celcius = temperature;

  // Pass gyro data to stillness detector
  gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect,
                     gyro_cal->stillness_win_endtime, sample_time, 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);
}

// Update the gyro calibration with mag data [micro Tesla].
void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
                      float y, float z) {
  // Pass magnetometer data to stillness detector.
  gyroStillDetUpdate(&gyro_cal->mag_stillness_detect,
                     gyro_cal->stillness_win_endtime, sample_time, 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);
}

// Update the gyro calibration with accel data [m/sec^2].
void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
                        float y, float z) {
  // Pass accelerometer data to stillnesss detector.
  gyroStillDetUpdate(&gyro_cal->accel_stillness_detect,
                     gyro_cal->stillness_win_endtime, sample_time, 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);
}

// Checks the state of all stillness detectors to determine
// whether the device is "still".
void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) {
  bool stillness_duration_exceeded = false;
  bool stillness_duration_too_short = 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);

  // 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 =
      sample_time + gyro_cal->window_time_duration;

  // 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;
  }

  // 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;

  // determine if the device is currently still.
  device_is_still = (conf_still > gyro_cal->stillness_threshold);

  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 =
          gyro_cal->gyro_stillness_detect.window_start_time;
    }

    // Check to see if current stillness period exceeds the desired limit
    // to avoid corrupting the .
    stillness_duration_exceeded =
        ((gyro_cal->gyro_stillness_detect.last_sample_time -
          gyro_cal->start_still_time) > gyro_cal->max_still_duration);

    if (stillness_duration_exceeded) {
      // The current stillness has gone too long. Do a calibration with the
      // current data and reset.

      // Update the gyro bias estimate with the current window data and
      // reset the stats.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);

      // Perform calibration.
      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.

      // Reset stillness detectors, and extend stillness period.
      gyroStillDetReset(&gyro_cal->accel_stillness_detect, false);
      gyroStillDetReset(&gyro_cal->gyro_stillness_detect, false);
      gyroStillDetReset(&gyro_cal->mag_stillness_detect, false);

      // Update 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) < gyro_cal->min_still_duration);

    if (gyro_cal->prev_still && !stillness_duration_too_short) {
      computeGyroCal(gyro_cal,
                     gyro_cal->gyro_stillness_detect.window_start_time);
    }

    // Reset stillness detectors and the stats.
    gyroStillDetReset(&gyro_cal->accel_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);

    // Update stillness flag.
    gyro_cal->prev_still = false;
  }

  // Reset the watchdog timer after we have processed data.
  gyro_cal->gyro_watchdog_start = sample_time;

#ifdef GYRO_CAL_DBG_ENABLED
  // Debug information available.
  gyro_cal->debug_processed_data_available = true;
  gyro_cal->debug_processed_data_time = sample_time;
#endif
}

// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) {
  // Current calibration duration.
  uint64_t cur_cal_dur = calibration_time - gyro_cal->start_still_time;

  // 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)) {
    // Outside of range. Ignore, reset, and continue.
    return;
  }

  // Record 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;

  // Record 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;

  // Store calibration stillness duration.
  gyro_cal->calibration_time_duration = cur_cal_dur;

  // Store calibration time stamp.
  gyro_cal->calibration_time = calibration_time;

  // 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++;

  // Store the last 'DEBUG_GYRO_SHORTTERM_NUM_CAL' calibration debug data
  // in a circular buffer, 'debug_cal_data[]'. 'debug_head' is the index
  // of the last valid calibration.
  gyro_cal->debug_head++;
  if (gyro_cal->debug_head >= DEBUG_GYRO_SHORTTERM_NUM_CAL) {
    gyro_cal->debug_head = 0;
  }
  if (gyro_cal->debug_num_cals < DEBUG_GYRO_SHORTTERM_NUM_CAL) {
    gyro_cal->debug_num_cals++;
  }

  // Update the calibration debug information.
  gyroCalUpdateDebug(gyro_cal, &gyro_cal->debug_cal_data[gyro_cal->debug_head]);

  // Improve the collection of historic calibrations. Limit frequency to
  // every N hours.
  if ((gyro_cal->debug_num_cals_hist <= 0) ||
      (calibration_time -
       gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]
           .calibration_time) >= DEBUG_GYRO_CAL_LIMIT) {
    gyro_cal->debug_head_hist++;
    if (gyro_cal->debug_head_hist >= DEBUG_GYRO_LONGTERM_NUM_CAL) {
      gyro_cal->debug_head_hist = 0;
    }
    if (gyro_cal->debug_num_cals_hist < DEBUG_GYRO_LONGTERM_NUM_CAL) {
      gyro_cal->debug_num_cals_hist++;
    }

    // Update the calibration debug information.
    gyroCalUpdateDebug(
        gyro_cal, &gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]);
  }
#endif
}

// Check for a watchdog timeout condition.
void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) {
  bool watchdog_timeout;

  // Check for initialization of the watchdog time (=0).
  if (gyro_cal->gyro_watchdog_start <= 0) {
    return;
  }

  // Check for timeout condition of watchdog.
  watchdog_timeout = ((sample_time - gyro_cal->gyro_watchdog_start) >
                      gyro_cal->gyro_watchdog_timeout_duration);

  // 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, true);
    gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true);
    gyroStillDetReset(&gyro_cal->mag_stillness_detect, true);
    gyro_cal->mag_stillness_detect.stillness_confidence = 0;
    gyro_cal->stillness_win_endtime = 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 = 0;
#ifdef GYRO_CAL_DBG_ENABLED
    gyro_cal->debug_watchdog_count++;
#endif
  }
}

#ifdef GYRO_CAL_DBG_ENABLED
// Update the calibration debug information.
void gyroCalUpdateDebug(struct GyroCal* gyro_cal,
                        struct DebugGyroCal* debug_gyro_cal) {
  // Probability of stillness (acc, rot, still), duration, timestamp.
  debug_gyro_cal->accel_stillness_conf =
      gyro_cal->accel_stillness_detect.prev_stillness_confidence;
  debug_gyro_cal->gyro_stillness_conf =
      gyro_cal->gyro_stillness_detect.prev_stillness_confidence;
  debug_gyro_cal->mag_stillness_conf =
      gyro_cal->mag_stillness_detect.prev_stillness_confidence;

  // Magnetometer usage.
  debug_gyro_cal->used_mag_sensor = gyro_cal->using_mag_sensor;

  // Temperature at calibration time.
  debug_gyro_cal->temperature_celcius = gyro_cal->latest_temperature_celcius;

  // Calibration time and stillness duration.
  debug_gyro_cal->calibration_time_duration =
      gyro_cal->calibration_time_duration;
  debug_gyro_cal->calibration_time = gyro_cal->calibration_time;

  // Record the current calibration values
  debug_gyro_cal->calibration[0] = gyro_cal->bias_x;
  debug_gyro_cal->calibration[1] = gyro_cal->bias_y;
  debug_gyro_cal->calibration[2] = gyro_cal->bias_z;

  // Record the previous window means
  debug_gyro_cal->accel_mean[0] = gyro_cal->accel_stillness_detect.prev_mean_x;
  debug_gyro_cal->accel_mean[1] = gyro_cal->accel_stillness_detect.prev_mean_y;
  debug_gyro_cal->accel_mean[2] = gyro_cal->accel_stillness_detect.prev_mean_z;

  debug_gyro_cal->gyro_mean[0] = gyro_cal->gyro_stillness_detect.prev_mean_x;
  debug_gyro_cal->gyro_mean[1] = gyro_cal->gyro_stillness_detect.prev_mean_y;
  debug_gyro_cal->gyro_mean[2] = gyro_cal->gyro_stillness_detect.prev_mean_z;

  debug_gyro_cal->mag_mean[0] = gyro_cal->mag_stillness_detect.prev_mean_x;
  debug_gyro_cal->mag_mean[1] = gyro_cal->mag_stillness_detect.prev_mean_y;
  debug_gyro_cal->mag_mean[2] = gyro_cal->mag_stillness_detect.prev_mean_z;

  // Record the variance data.
  debug_gyro_cal->accel_var[0] = gyro_cal->accel_stillness_detect.win_var_x;
  debug_gyro_cal->accel_var[1] = gyro_cal->accel_stillness_detect.win_var_y;
  debug_gyro_cal->accel_var[2] = gyro_cal->accel_stillness_detect.win_var_z;

  debug_gyro_cal->gyro_var[0] = gyro_cal->gyro_stillness_detect.win_var_x;
  debug_gyro_cal->gyro_var[1] = gyro_cal->gyro_stillness_detect.win_var_y;
  debug_gyro_cal->gyro_var[2] = gyro_cal->gyro_stillness_detect.win_var_z;

  debug_gyro_cal->mag_var[0] = gyro_cal->mag_stillness_detect.win_var_x;
  debug_gyro_cal->mag_var[1] = gyro_cal->mag_stillness_detect.win_var_y;
  debug_gyro_cal->mag_var[2] = gyro_cal->mag_stillness_detect.win_var_z;
}

// Helper function to print debug statements.
void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data) {
  CAL_DEBUG_LOG(
      "[GYRO_CAL]",
      "#%d Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n",
      count, CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MDEG, 6),
      CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MDEG, 6),
      CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MDEG, 6));

  if (debug_data->used_mag_sensor) {
    CAL_DEBUG_LOG("[GYRO_CAL]", "   Using Magnetometer.\n");
  } else {
    CAL_DEBUG_LOG("[GYRO_CAL]", "   Not Using Magnetometer.\n");
  }

  CAL_DEBUG_LOG("[GYRO_CAL]", "   Temperature = %s%d.%06d [C]\n",
                CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 6));

  CAL_DEBUG_LOG("[GYRO_CAL]", "   Calibration Timestamp = %llu [nsec]\n",
                debug_data->calibration_time);

  CAL_DEBUG_LOG("[GYRO_CAL]", "   Stillness Duration = %llu [nsec]\n",
                debug_data->calibration_time_duration);

  CAL_DEBUG_LOG("[GYRO_CAL]",
                "   Accel Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [g]\n",
                CAL_ENCODE_FLOAT(debug_data->accel_mean[0] * GRAVITY_TO_G, 6),
                CAL_ENCODE_FLOAT(debug_data->accel_mean[1] * GRAVITY_TO_G, 6),
                CAL_ENCODE_FLOAT(debug_data->accel_mean[2] * GRAVITY_TO_G, 6));

  CAL_DEBUG_LOG("[GYRO_CAL]",
                "   Mag Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]\n",
                CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 6),
                CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 6),
                CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 6));
}

// Print Debug data useful for tuning the stillness detectors.
void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) {
  static uint64_t start_time = 0;

  // Output sensor variance levels to assist with tuning thresholds.
  //   i.  Within the first 60 seconds of boot: output interval = 5 seconds.
  //   ii. Thereafter: output interval is set by DEBUG_GYRO_TUNE_UPDATES.
  bool condition_i = ((sample_time <= 60000000000) &&
                      ((sample_time - start_time) > 5000000000));  // nsec
  bool condition_ii = ((sample_time > 60000000000) &&
                       ((sample_time - start_time) > DEBUG_GYRO_TUNE_UPDATES));

  if (condition_i || condition_ii) {
    CAL_DEBUG_LOG("[GYRO_CAL]", "");
    CAL_DEBUG_LOG(
        "[GYRO_CAL]",
        "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
        "[mdps]\n",
        gyro_cal->debug_calibration_count,
        CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
        CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
        CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
    CAL_DEBUG_LOG(
        "[GYRO_CAL]",
        "   Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n",
        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));
    CAL_DEBUG_LOG(
        "[GYRO_CAL]",
        "   Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [m/sec^2]^2\n",
        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));
    if (gyro_cal->using_mag_sensor) {
      CAL_DEBUG_LOG(
          "[GYRO_CAL]",
          "   Mag Variance = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]^2\n",
          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
      CAL_DEBUG_LOG(
          "[GYRO_CAL]", "   Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n",
          CAL_ENCODE_FLOAT(
              gyro_cal->gyro_stillness_detect.stillness_confidence, 6),
          CAL_ENCODE_FLOAT(
              gyro_cal->accel_stillness_detect.stillness_confidence, 6),
          CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.stillness_confidence,
                            6));
    } else {
      CAL_DEBUG_LOG("[GYRO_CAL]", "   Mag Variance = {---, ---, ---} [uT]^2\n");
      CAL_DEBUG_LOG(
          "[GYRO_CAL]", "   Stillness = {G%s%d.%06d, A%s%d.%06d, M---}\n",
          CAL_ENCODE_FLOAT(
              gyro_cal->gyro_stillness_detect.stillness_confidence, 6),
          CAL_ENCODE_FLOAT(
              gyro_cal->accel_stillness_detect.stillness_confidence, 6));
    }

    CAL_DEBUG_LOG("[GYRO_CAL]", "   Temperature = %s%d.%06d [C]\n",
                  CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6));
    CAL_DEBUG_LOG("[GYRO_CAL]", "   Timestamp = %llu [nsec]\n", sample_time);
    CAL_DEBUG_LOG("[GYRO_CAL]", "   Total Gyro Calibrations: %lu\n",
                  gyro_cal->debug_calibration_count);
    start_time = sample_time;  // reset
  }
}

// Start the debug data report state machine.
void gyroCalDebugPrintStart(struct GyroCal* gyro_cal) {
  if (gyro_cal->gyro_debug_state < 0) {  // if currently in idle state.
    gyro_cal->gyro_debug_state = 0;      // start printing.
  }
}

// Print Debug data report.
void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) {
  static int head_index = 0;
  static uint64_t wait_timer = 0;
  static int i = 0;
  static int next_state = 0;

  // State machine to control reporting out debug print data.
  switch (gyro_cal->gyro_debug_state) {
    case 0:
      // STATE 0 : Print out header
      if (gyro_cal->debug_num_cals == 0) {
        CAL_DEBUG_LOG("[GYRO_CAL]", "");
        CAL_DEBUG_LOG("[GYRO_CAL]", "No Gyro Calibrations To Report.\n");
        CAL_DEBUG_LOG(
            "[GYRO_CAL]",
            "#0 Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
            "[mdps]\n",
            CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6),
            CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6),
            CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6));
        wait_timer = sample_time;        // start the wait timer.
        gyro_cal->gyro_debug_state = 7;  // go to next state.
      } else {
        CAL_DEBUG_LOG("[GYRO_CAL]", "");
        CAL_DEBUG_LOG("[GYRO_CAL]",
                      "---DEBUG REPORT-------------------------------------\n");
        CAL_DEBUG_LOG("[GYRO_CAL]",
                      "Gyro Calibrations To Report (newest to oldest): %d\n",
                      gyro_cal->debug_num_cals);
        head_index = gyro_cal->debug_head;
        i = 1;                           // initialize report count
        wait_timer = sample_time;        // start the wait timer.
        next_state = 1;                  // set the next state.
        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      }
      break;

    case 1:
      // STATE 1: Print out past N recent calibrations.
      if (i <= gyro_cal->debug_num_cals) {
        // Print the data.
        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data[head_index]);

        // Move to the previous calibration data set.
        head_index--;
        if (head_index < 0) {
          head_index = DEBUG_GYRO_SHORTTERM_NUM_CAL - 1;
        }

        // Increment the report count.
        i++;

        // Go to wait state.
        wait_timer = sample_time;        // start the wait timer.
        next_state = 1;                  // set the next state.
        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      } else {
        // Go to wait state.
        wait_timer = sample_time;        // start the wait timer.
        next_state = 3;                  // set the next state.
        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      }
      break;

    case 2:
      // STATE 2 : Wait state.
      // Wait for 500msec.
      // This helps throttle the print statements.
      if ((sample_time - wait_timer) >= 500000000) {
        gyro_cal->gyro_debug_state = next_state;
      }
      break;

    case 3:
      // STATE 3 : Print the end of the debug report.
      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n",
                    gyro_cal->debug_calibration_count);

      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n",
                    gyro_cal->debug_watchdog_count);

      CAL_DEBUG_LOG("[GYRO_CAL]",
                    "---END DEBUG REPORT---------------------------------\n");
      wait_timer = sample_time;        // start the wait timer.
      next_state = 4;                  // set the next state.
      gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      break;

    case 4:
      // STATE 4 : Print out header (historic data)
      CAL_DEBUG_LOG("[GYRO_CAL]", "");
      CAL_DEBUG_LOG("[GYRO_CAL]",
                    "---DEBUG REPORT (HISTORY)---------------------------\n");
      CAL_DEBUG_LOG("[GYRO_CAL]",
                    "Gyro Calibrations To Report (newest to oldest): %d\n",
                    gyro_cal->debug_num_cals_hist);
      head_index = gyro_cal->debug_head_hist;
      i = 1;                           // initialize report count
      wait_timer = sample_time;        // start the wait timer.
      next_state = 5;                  // set the next state.
      gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      break;

    case 5:
      // STATE 5: Print out past N historic calibrations.
      if (i <= gyro_cal->debug_num_cals_hist) {
        // Print the data.
        gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data_hist[head_index]);

        // Move to the previous calibration data set.
        head_index--;
        if (head_index < 0) {
          head_index = DEBUG_GYRO_LONGTERM_NUM_CAL - 1;
        }

        // Increment the report count.
        i++;

        // Go to wait state.
        wait_timer = sample_time;        // start the wait timer.
        next_state = 5;                  // set the next state.
        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      } else {
        // Go to wait state.
        wait_timer = sample_time;        // start the wait timer.
        next_state = 6;                  // set the next state.
        gyro_cal->gyro_debug_state = 2;  // but first, go to wait state.
      }
      break;

    case 6:
      // STATE 6 : Print the end of the debug report.
      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n",
                    gyro_cal->debug_calibration_count);

      CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n",
                    gyro_cal->debug_watchdog_count);

      CAL_DEBUG_LOG("[GYRO_CAL]",
                    "---END DEBUG REPORT---------------------------------\n");
      wait_timer = sample_time;        // start the wait timer.
      gyro_cal->gyro_debug_state = 7;  // go to next state.
      break;

    case 7:
      // STATE 7 : Final wait state.
      // Wait for 10 seconds.
      // This helps throttle the print statements.
      if ((sample_time - wait_timer) >= 10000000000) {
        gyro_cal->gyro_debug_state = -1;
      }
      break;

    default:
      // Idle state.
      gyro_cal->gyro_debug_state = -1;

      // Report periodic data useful for tuning the stillness detectors.
      gyroCalTuneDebugPrint(gyro_cal, sample_time);
  }
}
#endif