| /* |
| * 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 <algos/gyro_cal.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_t* gyro_cal, |
| uint64_t sample_time); |
| |
| static void computeGyroCal(struct gyroCal_t* gyro_cal, |
| uint64_t calibration_time); |
| |
| static void checkWatchdog(struct gyroCal_t* gyro_cal, |
| uint64_t sample_time); |
| |
| #ifdef GYRO_CAL_DBG_ENABLED |
| static void gyroCalUpdateDebug(struct gyroCal_t* gyro_cal, |
| struct debugGyroCal_t* debug_gyro_cal); |
| |
| static void gyroCalTuneDebugPrint(struct gyroCal_t* gyro_cal, |
| uint64_t sample_time); |
| |
| static void gyroCalDebugPrintData(int count, |
| struct debugGyroCal_t *debug_data); |
| |
| // Information print macro. |
| #define GYRO_INFO_PRINT(fmt, ...) do { \ |
| osLog(LOG_INFO, "%s " fmt, "[GYRO CAL]", ##__VA_ARGS__); \ |
| } while (0); |
| |
| // Macro used to print out floating point numbers. |
| #define GYRO_ENCODE_FLOAT(x, num_digits) ((x < 0) ? "-" : ""), \ |
| (int)floorf(fabsf(x)), \ |
| (int)((fabsf(x) - floorf(fabsf(x))) * powf(10,num_digits)) |
| #endif |
| |
| /////// FUNCTION DEFINITIONS ///////////////////////////////////////// |
| |
| // Initialize the gyro calibration data structure. |
| void gyroCalInit(struct gyroCal_t* 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_t)); |
| |
| // 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 |
| GYRO_INFO_PRINT("Gyro Cal: Initialized."); |
| #endif |
| } |
| |
| // Void all pointers in the gyro calibration data structure |
| // (prevents compiler warnings). |
| void gyroCalDestroy(struct gyroCal_t* gyro_cal) { |
| (void)gyro_cal; |
| } |
| |
| // Get the most recent bias calibration value. |
| void gyroCalGetBias(struct gyroCal_t* 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_t* 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; |
| } |
| |
| // Remove bias from a gyro measurement [rad/sec]. |
| void gyroCalRemoveBias(struct gyroCal_t* 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_t* 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_t* 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_t* 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_t* 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_t* 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_t* 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_t* 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_t* gyro_cal, |
| struct debugGyroCal_t* 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_t *debug_data) { |
| GYRO_INFO_PRINT( |
| "#%d Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n", |
| count, |
| GYRO_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MDEG, 6), |
| GYRO_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MDEG, 6), |
| GYRO_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MDEG, 6) |
| ); |
| |
| if (debug_data->used_mag_sensor) { |
| GYRO_INFO_PRINT(" Using Magnetometer.\n"); |
| } else { |
| GYRO_INFO_PRINT(" Not Using Magnetometer.\n"); |
| } |
| |
| GYRO_INFO_PRINT(" Temperature = %s%d.%06d [C]\n", |
| GYRO_ENCODE_FLOAT(debug_data->temperature_celcius, 6)); |
| |
| GYRO_INFO_PRINT(" Calibration Timestamp = %llu [nsec]\n", |
| debug_data->calibration_time); |
| |
| GYRO_INFO_PRINT(" Stillness Duration = %llu [nsec]\n", |
| debug_data->calibration_time_duration); |
| |
| GYRO_INFO_PRINT(" Accel Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [g]\n", |
| GYRO_ENCODE_FLOAT(debug_data->accel_mean[0] * |
| GRAVITY_TO_G, 6), |
| GYRO_ENCODE_FLOAT(debug_data->accel_mean[1] * |
| GRAVITY_TO_G, 6), |
| GYRO_ENCODE_FLOAT(debug_data->accel_mean[2] * |
| GRAVITY_TO_G, 6)); |
| |
| GYRO_INFO_PRINT(" Mag Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]\n", |
| GYRO_ENCODE_FLOAT(debug_data->mag_mean[0], 6), |
| GYRO_ENCODE_FLOAT(debug_data->mag_mean[1], 6), |
| GYRO_ENCODE_FLOAT(debug_data->mag_mean[2], 6) |
| ); |
| } |
| |
| // Print Debug data useful for tuning the stillness detectors. |
| void gyroCalTuneDebugPrint(struct gyroCal_t* gyro_cal, uint64_t sample_time) { |
| |
| static uint64_t counter = 0; |
| |
| // Output sensor variance levels to assist with tuning thresholds |
| // every N seconds. |
| if ((sample_time - counter) > 10000000000) { //nsec |
| GYRO_INFO_PRINT(""); |
| GYRO_INFO_PRINT( |
| " Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n", |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->gyro_stillness_detect.win_var_x, 8), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->gyro_stillness_detect.win_var_y, 8), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->gyro_stillness_detect.win_var_z, 8)); |
| GYRO_INFO_PRINT( |
| " Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [m/sec^2]^2\n", |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->accel_stillness_detect.win_var_x, 8), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->accel_stillness_detect.win_var_y, 8), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->accel_stillness_detect.win_var_z, 8)); |
| GYRO_INFO_PRINT( |
| " Mag Variance = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]^2\n", |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->mag_stillness_detect.win_var_x, 6), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->mag_stillness_detect.win_var_y, 6), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->mag_stillness_detect.win_var_z, 6)); |
| GYRO_INFO_PRINT( |
| " Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n", |
| GYRO_ENCODE_FLOAT( |
| gyro_cal-> |
| gyro_stillness_detect.stillness_confidence, 6), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal-> |
| accel_stillness_detect.stillness_confidence, 6), |
| GYRO_ENCODE_FLOAT( |
| gyro_cal-> |
| mag_stillness_detect.stillness_confidence, 6)); |
| GYRO_INFO_PRINT( |
| " Temperature = %s%d.%06d [C]\n", |
| GYRO_ENCODE_FLOAT( |
| gyro_cal->latest_temperature_celcius, 6)); |
| GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n", |
| gyro_cal->debug_calibration_count); |
| counter = sample_time; //reset |
| } |
| } |
| |
| // Print Debug data report. |
| void gyroCalDebugPrint(struct gyroCal_t* gyro_cal, |
| int* state, |
| uint64_t sample_time) { |
| |
| static int head_index; |
| static int i, j; |
| |
| if (gyro_cal->debug_num_cals == 0) { |
| *state = -1; // idle state. |
| } |
| |
| // State machine to control reporting out debug print data. |
| switch (*state) { |
| |
| case 0: |
| // STATE 0 : Print out header |
| GYRO_INFO_PRINT(""); |
| GYRO_INFO_PRINT( |
| "---DEBUG REPORT-------------------------------------\n"); |
| GYRO_INFO_PRINT("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 |
| *state = 1; // set next 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. |
| j = 0; |
| *state = 2; |
| |
| } else { |
| // Print out the footer data. |
| *state = 3; |
| } |
| break; |
| |
| case 2: |
| // STATE 2 : Wait state. |
| // Wait for N cycles of gyro samples before printing out more data. |
| // This helps throttle the print statements. |
| if (j <= 50) { |
| j++; |
| } else { |
| *state = 1; |
| } |
| break; |
| |
| case 3: |
| // STATE 3 : Print the end of the debug report. |
| GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n", |
| gyro_cal->debug_calibration_count); |
| |
| GYRO_INFO_PRINT("Total Watchdog Timeouts: %lu\n", |
| gyro_cal->debug_watchdog_count); |
| |
| GYRO_INFO_PRINT( |
| "---END DEBUG REPORT---------------------------------\n"); |
| *state = 4; |
| break; |
| |
| case 4: |
| // STATE 4 : Print out header (historic data) |
| GYRO_INFO_PRINT(""); |
| GYRO_INFO_PRINT( |
| "---DEBUG REPORT (HISTORY)---------------------------\n"); |
| GYRO_INFO_PRINT("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 |
| *state = 5; // set next 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. |
| j = 0; |
| *state = 6; |
| |
| } else { |
| // Print out the footer data. |
| *state = 7; |
| } |
| break; |
| |
| case 6: |
| // STATE 6 : Wait state. |
| // Wait for N cycles of gyro samples before printing out more data. |
| // This helps throttle the print statements. |
| if (j <= 50) { |
| j++; |
| } else { |
| *state = 5; |
| } |
| break; |
| |
| case 7: |
| // STATE 3 : Print the end of the debug report. |
| GYRO_INFO_PRINT("Total Gyro Calibrations: %lu\n", |
| gyro_cal->debug_calibration_count); |
| |
| GYRO_INFO_PRINT("Total Watchdog Timeouts: %lu\n", |
| gyro_cal->debug_watchdog_count); |
| |
| GYRO_INFO_PRINT( |
| "---END DEBUG REPORT---------------------------------\n"); |
| *state = -1; |
| break; |
| |
| default : |
| // Idle state. |
| *state = -1; |
| |
| // Report periodic data useful for tuning the stillness detectors. |
| //gyroCalTuneDebugPrint(gyro_cal, sample_time); |
| } |
| } |
| #endif |
