DO NOT MERGE Syncs google3 Calibration Code to Android.
This CL syncs the recent calibration updates made in the google3
directory: /location/lbs/contexthub/nanoapps/calibration/
Test:
- Firmware has been built and tested for the Nexus/Pixel devices.
Tip of G3 CL: 145593175
Change-Id: I37cb444c8974659b47752fa61ad1c3b2fdc3fdaa
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
index 8e5c3e2..312be69 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/common/diversity_checker.c
@@ -25,24 +25,37 @@
// Struct initialization.
void diversityCheckerInit(
struct DiversityChecker* diverse_data,
- float threshold,
- float max_distance,
size_t min_num_diverse_vectors,
size_t max_num_max_distance,
float var_threshold,
- float max_min_threshold) {
+ float max_min_threshold,
+ float local_field,
+ float threshold_tuning_param,
+ float max_distance_tuning_param) {
ASSERT_NOT_NULL(diverse_data);
+
// Initialize parameters.
- diverse_data->threshold = threshold;
- diverse_data->max_distance = max_distance;
+ diverse_data->threshold_tuning_param_sq =
+ (threshold_tuning_param * threshold_tuning_param);
+ diverse_data->max_distance_tuning_param_sq =
+ (max_distance_tuning_param * max_distance_tuning_param);
+
+ // Updating the threshold and max_distance using assumed local field.
+ // Testing for zero and negative local_field.
+ if (local_field <= 0) {
+ local_field = 1;
+ }
+ diversityCheckerLocalFieldUpdate(diverse_data, local_field);
diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
- // checking for min_num_diverse_vectors = 0
+
+ // Checking for min_num_diverse_vectors = 0.
if (min_num_diverse_vectors < 1) {
diverse_data->min_num_diverse_vectors = 1;
}
diverse_data->max_num_max_distance = max_num_max_distance;
diverse_data->var_threshold = var_threshold;
diverse_data->max_min_threshold = max_min_threshold;
+
// Setting the rest to zero.
diversityCheckerReset(diverse_data);
}
@@ -53,6 +66,7 @@
// Clear data memory.
memset(&diverse_data->diverse_data, 0,
sizeof(diverse_data->diverse_data));
+
// Resetting counters and data full bit.
diverse_data->num_points = 0;
diverse_data->num_max_dist_violations = 0;
@@ -62,10 +76,13 @@
void diversityCheckerUpdate(
struct DiversityChecker* diverse_data, float x, float y, float z) {
ASSERT_NOT_NULL(diverse_data);
+
// Converting three single inputs to a vector.
const float vec[3] = {x, y, z};
+
// Result vector for vector difference.
float vec_diff[3];
+
// normSquared result (k)
float norm_squared_result;
@@ -79,18 +96,22 @@
&diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
vec,
THREE_AXIS_DATA_DIM);
+
// k = |v|^2
norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
+
// if k < Threshold then leave the function.
if (norm_squared_result < diverse_data->threshold) {
return;
}
+
// if k > max_distance, count and leave the function.
if (norm_squared_result > diverse_data->max_distance) {
diverse_data->num_max_dist_violations++;
return;
}
}
+
// If none of the above caused to leave the function, data is diverse.
// Notice that the first data vector will be stored no matter what.
memcpy(&diverse_data->
@@ -99,6 +120,7 @@
sizeof(float) * THREE_AXIS_DATA_DIM);
// Count new data point.
diverse_data->num_points++;
+
// Setting data_full to 1, if memory is full.
if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
diverse_data->data_full = true;
@@ -163,3 +185,16 @@
float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
return (var < diverse_data->var_threshold);
}
+
+void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
+ float local_field) {
+ if ( local_field > 0 ) {
+ // Updating threshold based on the local field information.
+ diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
+ (local_field * local_field);
+
+ // Updating max distance based on the local field information.
+ diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
+ (local_field * local_field);
+ }
+}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/common/diversity_checker.h
index 30f53db..9581343 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.h
+++ b/firmware/os/algos/calibration/common/diversity_checker.h
@@ -53,7 +53,7 @@
#endif
#define THREE_AXIS_DATA_DIM (3) // data is three-dimensional.
-#define NUM_DIVERSE_VECTORS (10) // Storing 10 data points.
+#define NUM_DIVERSE_VECTORS (20) // Storing 20 data points.
// Main data struct.
struct DiversityChecker {
@@ -69,9 +69,17 @@
// Threshold value that is used to check k against.
float threshold;
+ // Threshold tuning paramter used to calculate threshold (k_algo):
+ // threshold = threshold_tuning_param_sq * (local_field)^2.
+ float threshold_tuning_param_sq;
+
// Maximum distance value.
float max_distance;
+ // Max Distance tuning parameter:
+ // max_distance = max_distance_tuning_param_sq * (local_field)^2.
+ float max_distance_tuning_param_sq;
+
// Data full bit.
bool data_full;
@@ -84,9 +92,6 @@
};
// Initialization of the function/struct, input:
-// threshold -> sets the threshold value, only distances k that are equal
-// or higher than that will be stored.
-// max_distance -> sets the maximum allowed distance of k.
// min_num_diverse_vectors -> sets the gate for a minimum number of data points
// in the memory
// max_num_max_distance -> sets the value for a max distance violation number
@@ -94,13 +99,17 @@
// var_threshold -> is a threshold value for a Norm variance gate.
// max_min_threshold -> is a value for a gate that rejects Norm variations
// that are larger than this number.
+// local_field -> is the assumed local_field (radius of the sphere).
+// threshold_tuning_param -> threshold tuning parameter used to calculate
+// threshold (k_algo).
+// max_distance_tuning_param -> Max distance tuning parameter used to calculate
+// max_distance.
void diversityCheckerInit(struct DiversityChecker* diverse_data,
- float threshold,
- float max_distance,
size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold);
+ size_t max_num_max_distance, float var_threshold,
+ float max_min_threshold, float local_field,
+ float threshold_tuning_param,
+ float max_distance_tuning_param);
// Resetting the memory and the counters, leaves threshold and max_distance
// as well as the setup variables for NormQuality check untouched.
@@ -126,6 +135,14 @@
float y_bias,
float z_bias);
+// This function updates the threshold value and max distance value based on the
+// local field. This ensures a local field independent operation of the
+// diversity checker.
+//
+// threshold = (threshold_tuning_param * local_field)^2
+// max_distance = (max_distance_tuning_param * local_field)^2
+void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
+ float local_field);
#ifdef __cplusplus
}
#endif
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 1c60955..e6bc275 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -29,18 +29,26 @@
// of the given sensor).
#define MAX_GYRO_BIAS (0.096f) // [rad/sec]
-// The time value used to throttle debug messaging.
-#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000)
-
// Converts units of radians to milli-degrees.
#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI)
-// Unit conversion: m/sec^2 to g's.
-#define GRAVITY_TO_G (float)(1e3f * 180.0f / M_PI)
+#ifdef GYRO_CAL_DBG_ENABLED
+// The time value used to throttle debug messaging.
+#define GYROCAL_WAIT_TIME_NANOS (300000000)
// Unit conversion: nanoseconds to seconds.
#define NANOS_TO_SEC (1.0e-9f)
+// A debug version label to help with tracking results.
+#define GYROCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
+
+// Debug log tag string used to identify debug report output data.
+#define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
+
+// Debug log tag string used to identify debug tuning output data.
+#define GYROCAL_TUNE_TAG "[GYRO_CAL:TUNE]"
+#endif // GYRO_CAL_DBG_ENABLED
+
/////// FORWARD DECLARATIONS /////////////////////////////////////////
static void deviceStillnessCheck(struct GyroCal* gyro_cal,
@@ -51,27 +59,51 @@
static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos);
-#ifdef GYRO_CAL_DBG_ENABLED
-static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
+// 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 running calculation of the temperature statistics.
- *
- * Behavior:
- * 1) If 'debug_temperature' pointer is not NULL then the local calculation
- * of the temperature statistics are copied, and the function returns.
- * 2) Else, if 'reset_stats' is 'true' then the local statistics are reset
- * and the function returns.
- * 3) Otherwise, the local temperature statistics are updated according to
- * the input value 'temperature'.
+ * 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:
- * debug_temperature: Pointer to the temperature stats sturcture to update.
- * temperature: Temperature value (Celsius).
- * reset_stats: Flag that determines if the local running stats are reset.
+ * gyro_cal: Pointer to the GyroCal data structure.
+ * temperature_celsius: New temperature sample to include.
+ * do_this: Command enumerator that controls function behavior:
*/
-static void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
- float temperature, bool reset_stats);
+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
+};
/*
* Updates running calculation of the gyro's mean sampling rate.
@@ -91,20 +123,12 @@
static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
uint64_t timestamp_nanos, bool reset_stats);
-// Defines the type of debug data to print.
-enum DebugPrintData {
- BIAS_CAL = 0,
- CAL_TIME,
- ACCEL_STATS,
- GYRO_STATS,
- MAG_STATS,
- TEMP_STATS,
- STILLNESS_DATA,
- SAMPLING_RATE
-};
+// 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 DebugGyroCal* debug_data,
+static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
+ char* debug_tag,
enum DebugPrintData print_data);
// This conversion function is necessary for Nanohub firmware compilation (i.e.,
@@ -138,7 +162,9 @@
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) {
+ float stillness_mean_delta_limit,
+ float temperature_delta_limit_celsius,
+ bool gyro_calibration_enable) {
// Clear gyro_cal structure memory.
memset(gyro_cal, 0, sizeof(struct GyroCal));
@@ -177,59 +203,52 @@
// 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.
+ // 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 = (remove_bias_enable > 0);
+ gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0);
+
+ // Sets the stability limit for the stillness window mean acceptable delta.
+ gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;
+
+ // Sets the min/max temperature delta limit for the stillness period.
+ gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;
+
+ // Ensures that the data tracking functionality is reset.
+ gyroStillMeanTracker(gyro_cal, DO_RESET);
+ gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[GYRO_CAL:MEMORY]", "sizeof(struct GyroCal): %lu",
(unsigned long int)sizeof(struct GyroCal));
- CAL_DEBUG_LOG("[GYRO_CAL:INIT]",
- "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
-
if (gyro_cal->gyro_calibration_enable) {
CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED.");
} else {
CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
}
- // Ensures that the running temperature statistics and gyro sampling rate
- // estimate are reset.
- gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+ // Ensures that the gyro sampling rate estimate is reset.
gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
}
-// Void all pointers in the gyro calibration data structure (doesn't do anything
+// Void pointer in the gyro calibration data structure (doesn't do anything
// except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
+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;
-
-#ifdef GYRO_CAL_DBG_ENABLED
- CAL_DEBUG_LOG(
- "[GYRO_CAL:STORED]",
- "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
-#endif
- }
+ float* bias_z, float* temperature_celsius) {
+ *bias_x = gyro_cal->bias_x;
+ *bias_y = gyro_cal->bias_y;
+ *bias_z = gyro_cal->bias_z;
+ *temperature_celsius = gyro_cal->bias_temperature_celsius;
}
// Set an initial bias calibration value.
@@ -246,7 +265,7 @@
CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
-#endif
+#endif // GYRO_CAL_DBG_ENABLED
}
// Remove bias from a gyro measurement [rad/sec].
@@ -272,9 +291,11 @@
// 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) {
- // Make sure that a valid window end time is set,
- // and start the watchdog timer.
+ float x, float y, float z, float temperature_celsius) {
+ static float latest_temperature_celsius = 0.0f;
+
+ // Make sure that a valid window end-time is set, and start the watchdog
+ // timer.
if (gyro_cal->stillness_win_endtime_nanos <= 0) {
gyro_cal->stillness_win_endtime_nanos =
sample_time_nanos + gyro_cal->window_time_duration_nanos;
@@ -283,25 +304,22 @@
gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos;
}
-#ifdef GYRO_CAL_DBG_ENABLED
- // Update the temperature statistics (on temperature change only).
- if (NANO_ABS(gyro_cal->latest_temperature_celcius - temperature) > FLT_MIN) {
- gyroTempUpdateStats(NULL, temperature, /*reset_stats=*/false);
+ // Update the temperature statistics (only on a temperature change).
+ if (NANO_ABS(temperature_celsius - latest_temperature_celsius) > FLT_MIN) {
+ gyroTemperatureStatsTracker(gyro_cal, temperature_celsius, DO_UPDATE_DATA);
}
+#ifdef GYRO_CAL_DBG_ENABLED
// Update the gyro sampling rate estimate.
gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
#endif // GYRO_CAL_DBG_ENABLED
- // 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_nanos, sample_time_nanos,
x, y, z);
- // Perform a device stillness check, set next window end time, and
+ // 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);
}
@@ -317,7 +335,7 @@
// 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
+ // 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);
}
@@ -330,17 +348,20 @@
gyro_cal->stillness_win_endtime_nanos, sample_time_nanos,
x, y, z);
- // Perform a device stillness check, set next window end time, and
+ // Perform a device stillness check, set next window end-time, and
// possibly do a gyro bias calibration and stillness detector reset.
deviceStillnessCheck(gyro_cal, sample_time_nanos);
}
+// TODO(davejacobs): Consider breaking this function up to improve readability.
// Checks the state of all stillness detectors to determine
// whether the device is "still".
void deviceStillnessCheck(struct GyroCal* gyro_cal,
uint64_t sample_time_nanos) {
bool stillness_duration_exceeded = false;
bool stillness_duration_too_short = false;
+ bool min_max_temp_exceeded = false;
+ bool mean_not_stable = false;
bool device_is_still = false;
float conf_not_rot = 0;
float conf_not_accel = 0;
@@ -357,7 +378,7 @@
return; // Not yet, wait for more data.
}
- // Set the next window end time for the stillness detectors.
+ // 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;
@@ -371,17 +392,26 @@
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;
- // determine if the device is currently still.
- device_is_still = (conf_still > gyro_cal->stillness_threshold);
+ // 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:
+ // 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.
@@ -400,24 +430,31 @@
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.
- // Update the gyro bias estimate with the current window data and
- // reset the stats.
+ // 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);
- // Perform calibration.
+ // Resets the local calculations because the stillness period is over.
+ gyroStillMeanTracker(gyro_cal, DO_RESET);
+ gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+
+ // Computes a new gyro offset estimate.
computeGyroCal(gyro_cal,
gyro_cal->gyro_stillness_detect.last_sample_time);
#ifdef GYRO_CAL_DBG_ENABLED
- // Reset the temperature statistics and sampling rate estimate.
- gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+ // Resets the sampling rate estimate.
gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
@@ -426,14 +463,14 @@
} else {
// Continue collecting stillness data.
- // Reset stillness detectors, and extend stillness period.
+ // 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 stillness flag.
+ // Update the stillness flag.
gyro_cal->prev_still = true;
}
} else {
@@ -451,14 +488,17 @@
gyro_cal->gyro_stillness_detect.window_start_time);
}
- // Reset stillness detectors and the stats.
+ // Reset the stillness detectors and the stats.
gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true);
gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true);
gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true);
+ // Resets the temperature and sensor mean data.
+ gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+ gyroStillMeanTracker(gyro_cal, DO_RESET);
+
#ifdef GYRO_CAL_DBG_ENABLED
- // Reset the temperature statistics and sampling rate estimate.
- gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+ // Resets the sampling rate estimate.
gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
@@ -472,10 +512,6 @@
// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
- // Current calibration duration.
- uint64_t cur_cal_dur_nanos =
- calibration_time_nanos - gyro_cal->start_still_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 &&
@@ -484,35 +520,44 @@
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:WARNING]",
- "Rejected Bias Update [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
+ CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
+ "Offset|Temp|Time [mdps|C|nsec]: %s%d.%06d, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %llu",
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 6),
+ (unsigned long long int)calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Outside of range. Ignore, reset, and continue.
return;
}
- // Record new gyro bias offset calibration.
+ // 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;
- // Record final stillness confidence.
+ // 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;
- // Store calibration stillness duration.
- gyro_cal->calibration_time_duration_nanos = cur_cal_dur_nanos;
-
- // Store calibration time stamp.
- gyro_cal->calibration_time_nanos = calibration_time_nanos;
-
// Set flag to indicate a new gyro calibration value is available.
gyro_cal->new_gyro_cal_available = true;
@@ -520,11 +565,8 @@
// Increment the total count of calibration updates.
gyro_cal->debug_calibration_count++;
- // Update the calibration debug information.
+ // Update the calibration debug information and trigger a printout.
gyroCalUpdateDebug(gyro_cal);
-
- // Trigger a printout of the debug information.
- gyro_cal->debug_print_trigger = true;
#endif
}
@@ -548,15 +590,19 @@
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);
- gyro_cal->mag_stillness_detect.stillness_confidence = 0;
- gyro_cal->stillness_win_endtime_nanos = 0;
+
+ // Resets the temperature and sensor mean data.
+ gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
+ gyroStillMeanTracker(gyro_cal, DO_RESET);
#ifdef GYRO_CAL_DBG_ENABLED
- // Reset the temperature statistics and sampling rate estimate.
- gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true);
+ // Resets the sampling rate estimate.
gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
+ // Resets the stillness window end-time.
+ gyro_cal->stillness_win_endtime_nanos = 0;
+
// Force stillness confidence to zero.
gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0;
gyro_cal->gyro_stillness_detect.prev_stillness_confidence = 0;
@@ -579,169 +625,155 @@
CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp [nsec]: %lu, %llu",
(unsigned long int)gyro_cal->debug_watchdog_count,
(unsigned long long int)sample_time_nanos);
-#endif
+#endif // GYRO_CAL_DBG_ENABLED
}
}
+// TODO(davejacobs) -- Combine the following two functions into one or consider
+// implementing a separate helper module for tracking the temperature and mean
+// statistics.
+bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal,
+ float temperature_celsius,
+ enum GyroCalTrackerCommand do_this) {
+ // This is used for local calculations of the running mean.
+ static float mean_accumulator = 0.0f;
+ static float temperature_min_max_celsius[2] = {0.0f, 0.0f};
+ static size_t num_points = 0;
+ bool min_max_temp_exceeded = false;
+
+ switch (do_this) {
+ case DO_RESET:
+ // Resets the mean accumulator.
+ num_points = 0;
+ mean_accumulator = 0.0f;
+
+ // Initializes the min/max temperatures values.
+ temperature_min_max_celsius[0] = FLT_MAX;
+ temperature_min_max_celsius[1] = -1.0f * (FLT_MAX - 1.0f);
+ break;
+
+ case DO_UPDATE_DATA:
+ // Does the mean accumulation.
+ mean_accumulator += temperature_celsius;
+ num_points++;
+
+ // Tracks the min and max temperature values.
+ if (temperature_min_max_celsius[0] > temperature_celsius) {
+ temperature_min_max_celsius[0] = temperature_celsius;
+ }
+ if (temperature_min_max_celsius[1] < temperature_celsius) {
+ temperature_min_max_celsius[1] = temperature_celsius;
+ }
+ break;
+
+ case DO_STORE_DATA:
+ // Store the most recent "stillness" mean data to the GyroCal data
+ // structure. This functionality allows previous results to be recalled
+ // when the device suddenly becomes "not still".
+ if (num_points > 0) {
+ memcpy(gyro_cal->temperature_min_max_celsius,
+ temperature_min_max_celsius, 2 * sizeof(float));
+ gyro_cal->temperature_mean_celsius = mean_accumulator / num_points;
+ }
+ break;
+
+ case DO_EVALUATE:
+ // Determines if the min/max delta exceeded the set limit.
+ if (num_points > 0) {
+ min_max_temp_exceeded =
+ (temperature_min_max_celsius[1] -
+ temperature_min_max_celsius[0]) >
+ gyro_cal->temperature_delta_limit_celsius;
+
+#ifdef GYRO_CAL_DBG_ENABLED
+ if (min_max_temp_exceeded) {
+ CAL_DEBUG_LOG(
+ "[GYRO_CAL:TEMP_GATE]",
+ "Exceeded the max temperature variation during stillness.");
+ }
+#endif // GYRO_CAL_DBG_ENABLED
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ return min_max_temp_exceeded;
+}
+
+bool gyroStillMeanTracker(struct GyroCal* gyro_cal,
+ enum GyroCalTrackerCommand do_this) {
+ static float gyro_winmean_min[3] = {0.0f, 0.0f, 0.0f};
+ static float gyro_winmean_max[3] = {0.0f, 0.0f, 0.0f};
+ bool mean_not_stable = false;
+ size_t i;
+
+ switch (do_this) {
+ case DO_RESET:
+ // Resets the min/max window mean values to a default value.
+ for (i = 0; i < 3; i++) {
+ gyro_winmean_min[i] = FLT_MAX;
+ gyro_winmean_max[i] = -1.0f * (FLT_MAX - 1.0f);
+ }
+ break;
+
+ case DO_UPDATE_DATA:
+ // Computes the min/max window mean values.
+ if (gyro_winmean_min[0] > gyro_cal->gyro_stillness_detect.win_mean_x) {
+ gyro_winmean_min[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+ }
+ if (gyro_winmean_max[0] < gyro_cal->gyro_stillness_detect.win_mean_x) {
+ gyro_winmean_max[0] = gyro_cal->gyro_stillness_detect.win_mean_x;
+ }
+
+ if (gyro_winmean_min[1] > gyro_cal->gyro_stillness_detect.win_mean_y) {
+ gyro_winmean_min[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+ }
+ if (gyro_winmean_max[1] < gyro_cal->gyro_stillness_detect.win_mean_y) {
+ gyro_winmean_max[1] = gyro_cal->gyro_stillness_detect.win_mean_y;
+ }
+
+ if (gyro_winmean_min[2] > gyro_cal->gyro_stillness_detect.win_mean_z) {
+ gyro_winmean_min[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+ }
+ if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
+ gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+ }
+ break;
+
+ case DO_STORE_DATA:
+ // Store the most recent "stillness" mean data to the GyroCal data
+ // structure. This functionality allows previous results to be recalled
+ // when the device suddenly becomes "not still".
+ memcpy(gyro_cal->gyro_winmean_min, gyro_winmean_min, 3 * sizeof(float));
+ memcpy(gyro_cal->gyro_winmean_max, gyro_winmean_max, 3 * sizeof(float));
+ break;
+
+ case DO_EVALUATE:
+ // Performs the stability check and returns the 'true' if the difference
+ // between min/max window mean value is outside the stable range.
+ for (i = 0; i < 3; i++) {
+ mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
+ gyro_cal->stillness_mean_delta_limit;
+ }
+#ifdef GYRO_CAL_DBG_ENABLED
+ if (mean_not_stable) {
+ CAL_DEBUG_LOG(
+ "[GYRO_CAL:MEAN_STABILITY_GATE]",
+ "Exceeded the max variation in the stillness window mean values.");
+ }
+#endif // GYRO_CAL_DBG_ENABLED
+ break;
+
+ default:
+ break;
+ }
+
+ return mean_not_stable;
+}
+
#ifdef GYRO_CAL_DBG_ENABLED
-void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
- // 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;
-
- // Temperature at calibration time.
- gyro_cal->debug_gyro_cal.temperature_celcius =
- gyro_cal->latest_temperature_celcius;
-
- // 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_duration_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 complete temperature statistics.
- gyroTempUpdateStats(&gyro_cal->debug_gyro_cal.debug_temperature, 0,
- /*reset_stats=*/true);
- gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
- /*reset_stats=*/true);
-
- // Records the previous 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.
- 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;
-}
-
-void gyroTempUpdateStats(struct DebugTemperature* debug_temperature,
- float temperature, bool reset_stats) {
- // Using the method of the assumed mean to preserve some numerical stability
- // while avoiding per-sample divisions that the more numerically stable
- // Welford method would afford.
-
- // Reference for the numerical method used below to compute the online mean
- // and variance statistics:
- // 1). en.wikipedia.org/wiki/assumed_mean
-
- // This is used for local calculations of temperature statistics.
- static struct DebugTemperature local_temperature_stats = {0};
- static bool set_assumed_mean = true;
-
- // If 'debug_temperature' is not NULL then this function just reads out the
- // current statistics, resets, and returns.
- if (debug_temperature) {
- if (local_temperature_stats.num_temperature_samples > 1) {
- // Computes the final calculation of temperature sensor mean and variance.
- float tmp = local_temperature_stats.temperature_mean_celsius;
- local_temperature_stats.temperature_mean_celsius /=
- local_temperature_stats.num_temperature_samples;
- local_temperature_stats.temperature_var_celsius =
- (local_temperature_stats.temperature_var_celsius -
- local_temperature_stats.temperature_mean_celsius * tmp) /
- (local_temperature_stats.num_temperature_samples - 1);
-
- // Adds the assumed mean value back to the total mean calculation.
- local_temperature_stats.temperature_mean_celsius +=
- local_temperature_stats.assumed_mean;
- } else {
- // Not enough samples to compute a valid variance. Indicate this with a -1
- // value.
- local_temperature_stats.temperature_var_celsius = -1.0f;
- }
-
- memcpy(debug_temperature, &local_temperature_stats,
- sizeof(struct DebugTemperature));
- reset_stats = true;
- }
-
- // Resets the temperature statistics and returns.
- if (reset_stats) {
- local_temperature_stats.num_temperature_samples = 0;
- local_temperature_stats.temperature_mean_celsius = 0.0f;
- local_temperature_stats.temperature_var_celsius = 0.0f;
- set_assumed_mean = true; // Sets flag.
-
- // Initialize the min/max temperatures values.
- local_temperature_stats.temperature_min_max_celsius[0] = FLT_MAX;
- local_temperature_stats.temperature_min_max_celsius[1] =
- -1.0f * (FLT_MAX - 1.0f);
- return;
- }
-
- // The first sample received is taken as the "assumed mean".
- if (set_assumed_mean) {
- local_temperature_stats.assumed_mean = temperature;
- set_assumed_mean = false; // Resets flag.
- }
-
- // Increments the number of samples.
- local_temperature_stats.num_temperature_samples++;
-
- // Online computation of mean and variance for the running stillness period.
- float delta = (temperature - local_temperature_stats.assumed_mean);
- local_temperature_stats.temperature_var_celsius += delta * delta;
- local_temperature_stats.temperature_mean_celsius += delta;
-
- // Track the min and max temperature values.
- if (local_temperature_stats.temperature_min_max_celsius[0] > temperature) {
- local_temperature_stats.temperature_min_max_celsius[0] = temperature;
- }
- if (local_temperature_stats.temperature_min_max_celsius[1] < temperature) {
- local_temperature_stats.temperature_min_max_celsius[1] = temperature;
- }
-}
-
void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
uint64_t timestamp_nanos, bool reset_stats) {
// This is used for local calculations of average sampling rate.
@@ -765,18 +797,22 @@
reset_stats = true;
}
- // Resets the sampling rate mean estimator data if:
- // 1. The 'reset_stats' flag is set.
- // 2. A bad timestamp was received (i.e., time not monotonic).
- // 3. 'last_timestamp_nanos' is zero.
- if (reset_stats || (timestamp_nanos <= last_timestamp_nanos) ||
- last_timestamp_nanos == 0) {
- last_timestamp_nanos = timestamp_nanos;
+ // Resets the sampling rate mean estimator data.
+ if (reset_stats) {
+ last_timestamp_nanos = 0;
time_delta_accumulator = 0;
num_samples = 0;
return;
}
+ // Skip adding this data to the accumulator if:
+ // 1. A bad timestamp was received (i.e., time not monotonic).
+ // 2. 'last_timestamp_nanos' is zero.
+ if (timestamp_nanos <= last_timestamp_nanos || last_timestamp_nanos == 0) {
+ last_timestamp_nanos = timestamp_nanos;
+ return;
+ }
+
// Increments the number of samples.
num_samples++;
@@ -785,121 +821,237 @@
last_timestamp_nanos = timestamp_nanos;
}
-void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data,
+void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
+ // Only update this data if debug printing is not currently in progress
+ // (i.e., don't want to risk overwriting debug information that is actively
+ // being reported).
+ if (gyro_cal->debug_state != GYRO_IDLE) {
+ return;
+ }
+
+ // Probability of stillness (acc, rot, still), duration, timestamp.
+ gyro_cal->debug_gyro_cal.accel_stillness_conf =
+ gyro_cal->accel_stillness_detect.prev_stillness_confidence;
+ gyro_cal->debug_gyro_cal.gyro_stillness_conf =
+ gyro_cal->gyro_stillness_detect.prev_stillness_confidence;
+ gyro_cal->debug_gyro_cal.mag_stillness_conf =
+ gyro_cal->mag_stillness_detect.prev_stillness_confidence;
+
+ // Magnetometer usage.
+ gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor;
+
+ // Stillness start, stop, and duration times.
+ gyro_cal->debug_gyro_cal.start_still_time_nanos =
+ gyro_cal->start_still_time_nanos;
+ gyro_cal->debug_gyro_cal.end_still_time_nanos =
+ gyro_cal->calibration_time_nanos;
+ gyro_cal->debug_gyro_cal.stillness_duration_nanos =
+ gyro_cal->calibration_time_nanos - gyro_cal->start_still_time_nanos;
+
+ // Records the current calibration values.
+ gyro_cal->debug_gyro_cal.calibration[0] = gyro_cal->bias_x;
+ gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y;
+ gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
+
+ // Records the mean gyroscope sampling rate.
+ gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
+ /*reset_stats=*/true);
+
+ // Records the min/max and mean temperature values.
+ gyro_cal->debug_gyro_cal.temperature_mean_celsius =
+ gyro_cal->temperature_mean_celsius;
+ memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
+ gyro_cal->temperature_min_max_celsius, 2 * sizeof(float));
+
+ // Records the min/max gyroscope window stillness mean values.
+ memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
+ 3 * sizeof(float));
+ memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
+ 3 * sizeof(float));
+
+ // Records the previous stillness window means.
+ gyro_cal->debug_gyro_cal.accel_mean[0] =
+ gyro_cal->accel_stillness_detect.prev_mean_x;
+ gyro_cal->debug_gyro_cal.accel_mean[1] =
+ gyro_cal->accel_stillness_detect.prev_mean_y;
+ gyro_cal->debug_gyro_cal.accel_mean[2] =
+ gyro_cal->accel_stillness_detect.prev_mean_z;
+
+ gyro_cal->debug_gyro_cal.gyro_mean[0] =
+ gyro_cal->gyro_stillness_detect.prev_mean_x;
+ gyro_cal->debug_gyro_cal.gyro_mean[1] =
+ gyro_cal->gyro_stillness_detect.prev_mean_y;
+ gyro_cal->debug_gyro_cal.gyro_mean[2] =
+ gyro_cal->gyro_stillness_detect.prev_mean_z;
+
+ gyro_cal->debug_gyro_cal.mag_mean[0] =
+ gyro_cal->mag_stillness_detect.prev_mean_x;
+ gyro_cal->debug_gyro_cal.mag_mean[1] =
+ gyro_cal->mag_stillness_detect.prev_mean_y;
+ gyro_cal->debug_gyro_cal.mag_mean[2] =
+ gyro_cal->mag_stillness_detect.prev_mean_z;
+
+ // Records the variance data.
+ // NOTE: These statistics include the final captured window, which may be
+ // outside of the "stillness" period. Therefore, these values may exceed the
+ // stillness thresholds.
+ gyro_cal->debug_gyro_cal.accel_var[0] =
+ gyro_cal->accel_stillness_detect.win_var_x;
+ gyro_cal->debug_gyro_cal.accel_var[1] =
+ gyro_cal->accel_stillness_detect.win_var_y;
+ gyro_cal->debug_gyro_cal.accel_var[2] =
+ gyro_cal->accel_stillness_detect.win_var_z;
+
+ gyro_cal->debug_gyro_cal.gyro_var[0] =
+ gyro_cal->gyro_stillness_detect.win_var_x;
+ gyro_cal->debug_gyro_cal.gyro_var[1] =
+ gyro_cal->gyro_stillness_detect.win_var_y;
+ gyro_cal->debug_gyro_cal.gyro_var[2] =
+ gyro_cal->gyro_stillness_detect.win_var_z;
+
+ gyro_cal->debug_gyro_cal.mag_var[0] =
+ gyro_cal->mag_stillness_detect.win_var_x;
+ gyro_cal->debug_gyro_cal.mag_var[1] =
+ gyro_cal->mag_stillness_detect.win_var_y;
+ gyro_cal->debug_gyro_cal.mag_var[2] =
+ gyro_cal->mag_stillness_detect.win_var_z;
+
+ // Trigger a printout of the debug information.
+ gyro_cal->debug_print_trigger = true;
+}
+
+void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag,
enum DebugPrintData print_data) {
// Prints out the desired debug data.
float mag_data;
switch (print_data) {
- case BIAS_CAL:
- CAL_DEBUG_LOG(
- "[GYRO_CAL:BIAS]",
- "Gyro Bias Calibration [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MILLI_DEGREES,
- 8),
- CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MILLI_DEGREES,
- 8),
- CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MILLI_DEGREES,
- 8));
+ case OFFSET:
+ CAL_DEBUG_LOG(debug_tag,
+ "Cal#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3),
+ (unsigned long long int)
+ gyro_cal->debug_gyro_cal.end_still_time_nanos);
break;
- case CAL_TIME:
- CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness Start Time [nsec]: %llu",
- (unsigned long long int)debug_data->start_still_time_nanos);
-
- CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness End Time [nsec]: %llu",
- (unsigned long long int)debug_data->end_still_time_nanos);
-
+ 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(
- "[GYRO_CAL:TIME]", "Stillness Duration [nsec]: %llu",
- (unsigned long long int)debug_data->stillness_duration_nanos);
+ debug_tag,
+ "Cal#|Start|End|Confidence [nsec]: %lu, %llu, %llu, "
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ (unsigned long long int)
+ gyro_cal->debug_gyro_cal.start_still_time_nanos,
+ (unsigned long long int)gyro_cal->debug_gyro_cal.end_still_time_nanos,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
+ CAL_ENCODE_FLOAT(mag_data, 3));
+ break;
+
+ case SAMPLE_RATE_AND_TEMPERATURE:
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: %lu, %s%d.%03d, "
+ "%s%d.%03d, %s%d.%03d, %s%d.%04d, %s%d.%03d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
+ 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0], 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1], 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.temperature_min_max_celsius[1] -
+ gyro_cal->debug_gyro_cal.temperature_min_max_celsius[0],
+ 4),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
+ break;
+
+ case GYRO_MINMAX_STILLNESS_MEAN:
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Cal#|Gyro Peak Stillness Variation [mdps]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
+ gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
+ gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
+ gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
+ RAD_TO_MILLI_DEGREES,
+ 6));
break;
case ACCEL_STATS:
- CAL_DEBUG_LOG("[GYRO_CAL:ACCEL_STATS]",
- "Accel Mean [m/sec^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->accel_mean[0], 8),
- CAL_ENCODE_FLOAT(debug_data->accel_mean[1], 8),
- CAL_ENCODE_FLOAT(debug_data->accel_mean[2], 8));
CAL_DEBUG_LOG(
- "[GYRO_CAL:ACCEL_STATS]",
- "Accel Variance [(m/sec^2)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->accel_var[0], 8),
- CAL_ENCODE_FLOAT(debug_data->accel_var[1], 8),
- CAL_ENCODE_FLOAT(debug_data->accel_var[2], 8));
+ debug_tag,
+ "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 8));
break;
case GYRO_STATS:
CAL_DEBUG_LOG(
- "[GYRO_CAL:GYRO_STATS]",
- "Gyro Mean [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->gyro_mean[0] * RAD_TO_MILLI_DEGREES, 8),
- CAL_ENCODE_FLOAT(debug_data->gyro_mean[1] * RAD_TO_MILLI_DEGREES, 8),
- CAL_ENCODE_FLOAT(debug_data->gyro_mean[2] * RAD_TO_MILLI_DEGREES, 8));
- CAL_DEBUG_LOG(
- "[GYRO_CAL:GYRO_STATS]",
- "Gyro Variance [(rad/sec)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->gyro_var[0], 8),
- CAL_ENCODE_FLOAT(debug_data->gyro_var[1], 8),
- CAL_ENCODE_FLOAT(debug_data->gyro_var[2], 8));
+ debug_tag,
+ "Cal#|Gyro Mean|Var [mdps|(rad/sec)^2]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
break;
case MAG_STATS:
- if (debug_data->using_mag_sensor) {
- CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
- "Mag Mean [uT]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 8),
- CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 8),
- CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 8));
- CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
- "Mag Variance [uT^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->mag_var[0], 8),
- CAL_ENCODE_FLOAT(debug_data->mag_var[1], 8),
- CAL_ENCODE_FLOAT(debug_data->mag_var[2], 8));
+ if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%08d, %s%d.%08d, %s%d.%08d",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1], 8),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2], 8));
} else {
- CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", "Mag Mean [uT]: 0, 0, 0");
- // The -1's indicate that the magnetometer sensor was not used.
- CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]",
- "Mag Variance [uT^2]: -1.0, -1.0, -1.0");
+ CAL_DEBUG_LOG(debug_tag,
+ "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
+ "-1.0",
+ (unsigned long int)gyro_cal->debug_calibration_count);
}
- break;
- case TEMP_STATS:
- CAL_DEBUG_LOG("[GYRO_CAL:TEMP_STATS]",
- "Latest Temperature [C]: %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 8));
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TEMP_STATS]",
- "Min/Max Temperature [C]: %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(
- debug_data->debug_temperature.temperature_min_max_celsius[0], 8),
- CAL_ENCODE_FLOAT(
- debug_data->debug_temperature.temperature_min_max_celsius[1], 8));
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TEMP_STATS]", "Temperature Mean [C]: %s%d.%08d",
- CAL_ENCODE_FLOAT(
- debug_data->debug_temperature.temperature_mean_celsius, 8));
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TEMP_STATS]", "Temperature Variance [C^2]: %s%d.%08d",
- CAL_ENCODE_FLOAT(
- debug_data->debug_temperature.temperature_var_celsius, 8));
- break;
-
- case STILLNESS_DATA:
- mag_data = (debug_data->using_mag_sensor)
- ? debug_data->mag_stillness_conf
- : -1.0f; // Signals that magnetometer was not used.
- CAL_DEBUG_LOG("[GYRO_CAL:STILLNESS]",
- "Stillness [G/A/M]: %s%d.%08d, %s%d.%08d, %s%d.%08d",
- CAL_ENCODE_FLOAT(debug_data->gyro_stillness_conf, 8),
- CAL_ENCODE_FLOAT(debug_data->accel_stillness_conf, 8),
- CAL_ENCODE_FLOAT(mag_data, 8));
- break;
-
- case SAMPLING_RATE:
- CAL_DEBUG_LOG("[GYRO_CAL:SAMPLING_RATE]",
- "Gyro Sampling Rate [Hz]: %s%d.%06d",
- CAL_ENCODE_FLOAT(
- debug_data->mean_sampling_rate_hz, 6));
break;
default:
@@ -907,117 +1059,91 @@
}
}
-// Debug printout state enumeration.
-enum GyroCalDebugState {
- IDLE = 0,
- WAIT_STATE,
- PRINT_BIAS,
- PRINT_TIME,
- PRINT_TEMP,
- PRINT_ACCEL,
- PRINT_GYRO,
- PRINT_MAG,
- PRINT_STILLNESS,
- PRINT_SAMPLING_RATE
-};
-
void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
- static enum GyroCalDebugState debug_state = IDLE;
- static enum GyroCalDebugState next_state = IDLE;
+ static enum GyroCalDebugState next_state = GYRO_IDLE;
static uint64_t wait_timer_nanos = 0;
// This is a state machine that controls the reporting out of debug data.
- switch (debug_state) {
- case IDLE:
+ switch (gyro_cal->debug_state) {
+ case GYRO_IDLE:
// Wait for a trigger and start the debug printout sequence.
if (gyro_cal->debug_print_trigger) {
- debug_state = PRINT_BIAS;
- CAL_DEBUG_LOG("[GYRO_CAL]", "");
+ 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 {
- debug_state = IDLE;
+ gyro_cal->debug_state = GYRO_IDLE;
}
break;
- case WAIT_STATE:
+ case GYRO_WAIT_STATE:
// This helps throttle the print statements.
- if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
- debug_state = next_state;
+ if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_WAIT_TIME_NANOS) {
+ gyro_cal->debug_state = next_state;
}
break;
- case PRINT_BIAS:
- CAL_DEBUG_LOG("[GYRO_CAL:BIAS]", "Total # Calibrations: %lu",
- (unsigned long int)gyro_cal->debug_calibration_count);
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, BIAS_CAL);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_TIME; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_OFFSET:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_STILLNESS_DATA; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_TIME:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, CAL_TIME);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_TEMP; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_STILLNESS_DATA:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE; // Sets next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_TEMP:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, TEMP_STATS);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_ACCEL; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
+ SAMPLE_RATE_AND_TEMPERATURE);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN; // Sets next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_ACCEL:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, ACCEL_STATS);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_GYRO; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
+ GYRO_MINMAX_STILLNESS_MEAN);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_GYRO:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, GYRO_STATS);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_MAG; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_ACCEL_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_MAG:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, MAG_STATS);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_STILLNESS; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_GYRO_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_MAG_STATS; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_STILLNESS:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, STILLNESS_DATA);
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_SAMPLING_RATE; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case PRINT_SAMPLING_RATE:
- gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, SAMPLING_RATE);
- debug_state = IDLE;
+ case GYRO_PRINT_MAG_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_IDLE; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
default:
// Sends this state machine to its idle state.
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = IDLE; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
}
#ifdef GYRO_CAL_DBG_TUNE_ENABLED
- if (debug_state == IDLE) {
+ if (gyro_cal->debug_state == GYRO_IDLE) {
// This check keeps the tuning printout from interleaving with the above
// debug print data.
gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos);
@@ -1028,8 +1154,8 @@
#ifdef GYRO_CAL_DBG_TUNE_ENABLED
void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
uint64_t timestamp_nanos) {
- static enum GyroCalDebugState debug_state = IDLE;
- static enum GyroCalDebugState next_state = IDLE;
+ static enum GyroCalDebugState debug_state = GYRO_IDLE;
+ static enum GyroCalDebugState next_state = GYRO_IDLE;
static uint64_t wait_timer_nanos = 0;
// Output sensor variance levels to assist with tuning thresholds.
@@ -1042,126 +1168,57 @@
bool condition_ii = ((timestamp_nanos > 60000000000) &&
((timestamp_nanos - wait_timer_nanos) > 60000000000));
- // This is a state machine that controls the reporting out of debug data.
+ // This is a state machine that controls the reporting out of tuning data.
switch (debug_state) {
- case IDLE:
- // Wait for a trigger and start the debug printout sequence.
+ case GYRO_IDLE:
+ // Wait for a trigger and start the data tuning printout sequence.
if (condition_i || condition_ii) {
- debug_state = PRINT_BIAS;
+ CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "");
+ debug_state = GYRO_PRINT_OFFSET;
} else {
- debug_state = IDLE;
+ debug_state = GYRO_IDLE;
}
break;
- case WAIT_STATE:
+ case GYRO_WAIT_STATE:
// This helps throttle the print statements.
- if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
+ if ((timestamp_nanos - wait_timer_nanos) >= GYROCAL_WAIT_TIME_NANOS) {
debug_state = next_state;
}
break;
- case PRINT_BIAS:
- CAL_DEBUG_LOG("[GYRO_CAL]", "");
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} "
- "[mdps]\n",
- (unsigned long int)gyro_cal->debug_calibration_count,
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6));
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_TIME; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ case GYRO_PRINT_OFFSET:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, OFFSET);
+ wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state.
+ debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_TIME:
- CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Timestamp = %llu [nsec]\n",
- (unsigned long long int)timestamp_nanos);
- CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Total Gyro Calibrations: %lu\n",
- (unsigned long int)gyro_cal->debug_calibration_count);
-
+ case GYRO_PRINT_ACCEL_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS);
wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_TEMP; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state.
+ debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_TEMP:
- CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Temperature = %s%d.%06d [C]\n",
- CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6));
-
+ case GYRO_PRINT_GYRO_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS);
wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_ACCEL; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ next_state = GYRO_PRINT_MAG_STATS; // Sets the next state.
+ debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_ACCEL:
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- " 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));
-
+ case GYRO_PRINT_MAG_STATS:
+ gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS);
wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_GYRO; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case PRINT_GYRO:
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- " 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));
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_MAG; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case PRINT_MAG:
- if (gyro_cal->using_mag_sensor) {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- " Mag Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [uT]^2\n",
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8));
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- " Stillness = {G%s%d.%03d, A%s%d.%03d, M%s%d.%03d}\n",
- CAL_ENCODE_FLOAT(
- gyro_cal->gyro_stillness_detect.stillness_confidence, 3),
- CAL_ENCODE_FLOAT(
- gyro_cal->accel_stillness_detect.stillness_confidence, 3),
- CAL_ENCODE_FLOAT(
- gyro_cal->mag_stillness_detect.stillness_confidence, 3));
- } else {
- CAL_DEBUG_LOG("[GYRO_CAL:TUNE]",
- " Mag Variance = {---, ---, ---} [uT]^2\n");
- CAL_DEBUG_LOG(
- "[GYRO_CAL:TUNE]",
- " Stillness = {G%s%d.%03d, A%s%d.%03d, M---}\n",
- CAL_ENCODE_FLOAT(
- gyro_cal->gyro_stillness_detect.stillness_confidence, 3),
- CAL_ENCODE_FLOAT(
- gyro_cal->accel_stillness_detect.stillness_confidence, 3));
- }
-
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = IDLE; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ next_state = GYRO_IDLE; // Sets the next state.
+ debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
default:
// Sends this state machine to its idle state.
wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = IDLE; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ debug_state = GYRO_IDLE;
}
}
#endif // GYRO_CAL_DBG_TUNE_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 4b5a9c4..9cf06d2 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -36,7 +36,7 @@
*
* Optional Sensors and Units:
* - Magnetometer [micro-Tesla, uT]
- * - Temperature [Celcius]
+ * - Temperature [Celsius]
*
* #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
* #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
@@ -53,23 +53,25 @@
#endif
#ifdef GYRO_CAL_DBG_ENABLED
-// Temperature debug statistics.
-struct DebugTemperature {
- size_t num_temperature_samples;
- float temperature_min_max_celsius[2];
- float temperature_mean_celsius;
- float temperature_var_celsius;
- float assumed_mean;
+// Debug printout state enumeration.
+enum GyroCalDebugState {
+ GYRO_IDLE = 0,
+ GYRO_WAIT_STATE,
+ GYRO_PRINT_OFFSET,
+ GYRO_PRINT_STILLNESS_DATA,
+ GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE,
+ GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN,
+ GYRO_PRINT_ACCEL_STATS,
+ GYRO_PRINT_GYRO_STATS,
+ GYRO_PRINT_MAG_STATS
};
// Gyro Cal debug information/data tracking structure.
struct DebugGyroCal {
- struct DebugTemperature debug_temperature;
uint64_t start_still_time_nanos;
uint64_t end_still_time_nanos;
uint64_t stillness_duration_nanos;
float mean_sampling_rate_hz;
- float temperature_celcius;
float accel_stillness_conf;
float gyro_stillness_conf;
float mag_stillness_conf;
@@ -80,6 +82,10 @@
float accel_var[3];
float gyro_var[3];
float mag_var[3];
+ float gyro_winmean_min[3];
+ float gyro_winmean_max[3];
+ float temperature_min_max_celsius[2]; // 0=min; 1=max
+ float temperature_mean_celsius;
bool using_mag_sensor;
};
#endif
@@ -90,9 +96,6 @@
struct GyroStillDet mag_stillness_detect;
struct GyroStillDet gyro_stillness_detect;
- // Latest temperature measurement.
- float latest_temperature_celcius;
-
// Aggregated sensor stillness threshold required for gyro bias calibration.
float stillness_threshold;
@@ -106,15 +109,16 @@
// Timestamp when device started a still period.
uint64_t start_still_time_nanos;
- // gyro bias estimates and last calibration timestamp.
+ // Gyro offset estimate, and the associated calibration temperature,
+ // timestamp, and stillness confidence values.
float bias_x, bias_y, bias_z; // [rad/sec]
- uint64_t calibration_time_nanos;
- uint64_t calibration_time_duration_nanos;
+ float bias_temperature_celsius;
float stillness_confidence;
+ uint64_t calibration_time_nanos;
- // Current window end time for all sensors. Used to assist in keeping
+ // Current window end-time for all sensors. Used to assist in keeping
// sensor data collection in sync. On initialization this will be set to
- // zero indicating that sensor data will be dropped until a valid end time
+ // zero indicating that sensor data will be dropped until a valid end-time
// is set from the first gyro timestamp received.
uint64_t stillness_win_endtime_nanos;
@@ -136,11 +140,26 @@
// Flag to indicate if device was previously still.
bool prev_still;
+ // Min and maximum stillness window mean. This is used to check the stability
+ // of the mean values computed for the gyroscope (i.e., provides further
+ // validation for stillness).
+ float gyro_winmean_min[3];
+ float gyro_winmean_max[3];
+ float stillness_mean_delta_limit;
+
+ // Computes the min/max/mean temperature over the stillness period. This is
+ // used to check the temperature stability and provide a gate for when
+ // temperature is rapidly changing.
+ float temperature_min_max_celsius[2]; // 0=min; 1=max
+ float temperature_mean_celsius;
+ float temperature_delta_limit_celsius;
+
//----------------------------------------------------------------
#ifdef GYRO_CAL_DBG_ENABLED
// Debug info.
struct DebugGyroCal debug_gyro_cal; // Debug data structure.
+ enum GyroCalDebugState debug_state; // Debug printout state machine.
size_t debug_calibration_count; // Total number of cals performed.
size_t debug_watchdog_count; // Total number of watchdog timeouts.
bool debug_print_trigger; // Flag used to trigger data printout.
@@ -157,14 +176,16 @@
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);
+ float stillness_mean_delta_limit,
+ float temperature_delta_limit_celsius,
+ bool gyro_calibration_enable);
// Void all pointers in the gyro calibration data structure.
void gyroCalDestroy(struct GyroCal* gyro_cal);
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
- float* bias_z);
+ float* bias_z, float* temperature_celsius);
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
@@ -179,7 +200,7 @@
// 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);
+ float x, float y, float z, float temperature_celsius);
// Update the gyro calibration with mag data [micro Tesla].
void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index afddb48..0b38e5b 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -122,28 +122,41 @@
// Calculates and returns the stillness confidence score [0,1].
float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) {
float tmp_denom = 1.f;
- float tmp_mean = 1.f;
+ float tmp_denom_mean = 1.f;
// Don't divide by zero (not likely, but a precaution).
if (gyro_still_det->num_acc_win_samples > 1) {
tmp_denom = 1.f / (gyro_still_det->num_acc_win_samples - 1);
+ tmp_denom_mean = 1.f / gyro_still_det->num_acc_win_samples;
} else {
// Return zero stillness confidence.
gyro_still_det->stillness_confidence = 0;
return gyro_still_det->stillness_confidence;
}
- // Update the final calculation of variance.
- // variance_x = win_var_x / (num_samples - 1).
- tmp_mean = gyro_still_det->win_mean_x * tmp_denom;
+ // Update the final calculation of window mean and variance.
+ float tmp = gyro_still_det->win_mean_x;
+ gyro_still_det->win_mean_x *= tmp_denom_mean;
gyro_still_det->win_var_x =
- (gyro_still_det->acc_var_x * tmp_denom) - tmp_mean * tmp_mean;
- tmp_mean = gyro_still_det->win_mean_y * tmp_denom;
+ (gyro_still_det->acc_var_x - gyro_still_det->win_mean_x * tmp) *
+ tmp_denom;
+
+ tmp = gyro_still_det->win_mean_y;
+ gyro_still_det->win_mean_y *= tmp_denom_mean;
gyro_still_det->win_var_y =
- (gyro_still_det->acc_var_y * tmp_denom) - tmp_mean * tmp_mean;
- tmp_mean = gyro_still_det->win_mean_z * tmp_denom;
+ (gyro_still_det->acc_var_y - gyro_still_det->win_mean_y * tmp) *
+ tmp_denom;
+
+ tmp = gyro_still_det->win_mean_z;
+ gyro_still_det->win_mean_z *= tmp_denom_mean;
gyro_still_det->win_var_z =
- (gyro_still_det->acc_var_z * tmp_denom) - tmp_mean * tmp_mean;
+ (gyro_still_det->acc_var_z - gyro_still_det->win_mean_z * tmp) *
+ tmp_denom;
+
+ // Adds the assumed mean value back to the total mean calculation.
+ gyro_still_det->win_mean_x += gyro_still_det->assumed_mean_x;
+ gyro_still_det->win_mean_y += gyro_still_det->assumed_mean_y;
+ gyro_still_det->win_mean_z += gyro_still_det->assumed_mean_z;
// Define the variance thresholds.
float upper_var_thresh =
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 9c9849a..8603575 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -30,8 +30,8 @@
#define MIN_BATCH_SIZE 25 // samples
#else
#define MAX_EIGEN_RATIO 15.0f
-#define MAX_EIGEN_MAG 60.0f // uT
-#define MIN_EIGEN_MAG 30.0f // uT
+#define MAX_EIGEN_MAG 70.0f // uT
+#define MIN_EIGEN_MAG 20.0f // uT
#define MAX_FIT_MAG 70.0f
#define MIN_FIT_MAG 20.0f
#define MIN_BATCH_WINDOW 3000000UL // 3 sec
@@ -149,13 +149,7 @@
(moc->kasa.nsamples > MIN_BATCH_SIZE)) {
complete = 1;
- } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW
-#ifdef DIVERSITY_CHECK_ENABLED
- ||
- moc->diversity_checker.num_max_dist_violations
- >= MAX_DISTANCE_VIOLATIONS
-#endif
- ) {
+ } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
// not enough samples collected in MAX_BATCH_WINDOW or too many
// maximum distance violations detected.
magCalReset(moc);
@@ -172,11 +166,13 @@
float c00, float c01, float c02, float c10, float c11,
float c12, float c20, float c21, float c22
#ifdef DIVERSITY_CHECK_ENABLED
- ,float threshold, float max_distance
,size_t min_num_diverse_vectors
,size_t max_num_max_distance
,float var_threshold
,float max_min_threshold
+ ,float local_field
+ ,float threshold_tuning_param
+ ,float max_distance_tuning_param
#endif
) {
magCalReset(moc);
@@ -200,12 +196,13 @@
#ifdef DIVERSITY_CHECK_ENABLED
// Diversity Checker Init
diversityCheckerInit(&moc->diversity_checker,
- threshold,
- max_distance,
min_num_diverse_vectors,
max_num_max_distance,
var_threshold,
- max_min_threshold);
+ max_min_threshold,
+ local_field,
+ threshold_tuning_param,
+ max_distance_tuning_param);
#endif
}
@@ -269,14 +266,17 @@
if (moc_eigen_test(&moc->kasa)) {
struct Vec3 bias;
float radius;
-
// 4. Kasa sphere fitting
if (magKasaFit(&moc->kasa, &bias, &radius)) {
#ifdef DIVERSITY_CHECK_ENABLED
+ diversityCheckerLocalFieldUpdate(&moc->diversity_checker,
+ radius);
if (diversityCheckerNormQuality(&moc->diversity_checker,
bias.x,
bias.y,
- bias.z)) {
+ bias.z) &&
+ moc->diversity_checker.num_max_dist_violations
+ <= MAX_DISTANCE_VIOLATIONS) {
#endif
moc->x_bias = bias.x;
moc->y_bias = bias.y;
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
index 2640caf..e5158c5 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h
@@ -57,11 +57,9 @@
void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
float c00, float c01, float c02, float c10, float c11,
float c12, float c20, float c21, float c22,
- float threshold, float max_distance,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold);
+ size_t min_num_diverse_vectors, size_t max_num_max_distance,
+ float var_threshold, float max_min_threshold, float local_field,
+ float threshold_tuning_param, float max_distance_tuning_param);
#else
void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
float c00, float c01, float c02, float c10, float c11,
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index 0b8b6c7..f1d02b5 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -30,9 +30,8 @@
// model is in its initial state.
#define MODEL_INITIAL_STATE (1e6f)
-// Rate-limits the search for the nearest offset estimate to every 10 seconds
-// when no model has been updated yet.
-#define OVERTEMPCAL_NEAREST_NANOS (10000000000)
+// Rate-limits the search for the nearest offset estimate to every 2 seconds.
+#define OVERTEMPCAL_NEAREST_NANOS (2000000000)
// Rate-limits the check of old data to every 2 hours.
#define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000)
@@ -43,10 +42,10 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
// A debug version label to help with tracking results.
-#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Dec 12, 2016]"
+#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Jan 20, 2017]"
// The time value used to throttle debug messaging.
-#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000)
+#define OVERTEMPCAL_WAIT_TIME_NANOS (300000000)
// Debug log tag string used to identify debug report output data.
#define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]"
@@ -61,8 +60,8 @@
/////// FORWARD DECLARATIONS //////////////////////////////////////////////////
// Updates the most recent model estimate data.
-static void setLatestEstimate(struct OverTempCal *over_temp_cal,
- const float *offset, float offset_temp,
+static void setNearestEstimate(struct OverTempCal *over_temp_cal,
+ const float *offset, float offset_temp_celsius,
uint64_t timestamp_nanos);
/*
@@ -78,9 +77,9 @@
/*
* Searches 'model_data' for the sensor offset estimate closest to the current
- * temperature. Sets the 'latest_offset' pointer to the result.
+ * temperature. Sets the 'nearest_offset' pointer to the result.
*/
-static void setNearestEstimate(struct OverTempCal *over_temp_cal);
+static void findNearestEstimate(struct OverTempCal *over_temp_cal);
/*
* Removes the "old" offset estimates from 'model_data' (i.e., eliminates the
@@ -126,28 +125,34 @@
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
- * vi: Single axis sensor data to be compensated.
+ * axis_in: Single axis sensor data to be compensated.
* index: Index for model parameter compensation (0=x, 1=y, 2=z).
* OUTPUTS:
- * vo: Single axis sensor data that has been compensated.
+ * axis_out: Single axis sensor data that has been compensated.
*/
static void removeSensorOffset(const struct OverTempCal *over_temp_cal,
- float vi, size_t index, float *vo);
+ float axis_in, size_t index, float *axis_out);
/*
- * Computes the over-temperature compensated sensor offset estimate based on the
- * input model parameters. Note, this is a single axis calculation.
- * comp_offset = (temp_sensitivity * temperature + sensor_intercept)
+ * Checks new offset estimates to determine if they could be an outlier that
+ * should be rejected. Operates on a per-axis basis determined by 'axis_index'.
*
* INPUTS:
- * temperature: Temperature value at which to compute the estimate.
- * temp_sensitivity: Temperature sensitivity model parameter.
- * sensor_intercept: Sensor intercept model parameter.
- * RETURNS:
- * comp_offset: Over-temperature compensated sensor offset estimate.
+ * over_temp_cal: Over-temp data structure.
+ * offset: Offset array.
+ * axis_index: Index of the axis to check (0=x, 1=y, 2=z).
+ *
+ * Returns 'true' if the deviation of the offset value from the linear model
+ * exceeds 'max_error_limit'.
*/
-static float getCompensatedOffset(float temperature, float temp_sensitivity,
- float sensor_intercept);
+static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
+ size_t axis_index, float temperature_celsius);
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// This helper function stores all of the debug tracking information necessary
+// for printing log messages.
+static void updateDebugData(struct OverTempCal* over_temp_cal);
+#endif // OVERTEMPCAL_DBG_ENABLED
/////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
@@ -164,7 +169,7 @@
// Initializes the pointer to the most recent sensor offset estimate. Sets it
// as the first element in 'model_data'.
- over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
// Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to
// indicate that the model is in an "initial" state.
@@ -186,13 +191,19 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu",
(unsigned long int)sizeof(struct OverTempCal));
- CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Over-Temp Cal: Initialized.");
+ if (over_temp_cal->over_temp_enable) {
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]",
+ "Over-temperature compensation ENABLED.");
+ } else {
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]",
+ "Over-temperature compensation DISABLED.");
+ }
#endif // OVERTEMPCAL_DBG_ENABLED
}
void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
- float offset_temp, uint64_t timestamp_nanos,
+ float offset_temp_celsius, uint64_t timestamp_nanos,
const float *temp_sensitivity,
const float *sensor_intercept, bool jump_start_model) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -219,7 +230,8 @@
if (!model_jump_started) {
// Sets the initial over-temp calibration estimate and model data.
- setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos);
+ setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+ timestamp_nanos);
// Now there is one offset estimate in the model.
over_temp_cal->num_model_pts = 1;
@@ -228,50 +240,46 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
// Prints the updated model data.
CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]",
- "Model parameters recalled from memory.");
+ "Over-temperature model parameters recalled.");
- // Trigger a debug print out to view the new model parameters.
- over_temp_cal->debug_print_trigger = true;
+ // Triggers a debug print out to view the new model parameters.
+ updateDebugData(over_temp_cal);
#endif // OVERTEMPCAL_DBG_ENABLED
}
void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
- float *offset_temp, uint64_t *timestamp_nanos,
+ float *offset_temp_celsius, uint64_t *timestamp_nanos,
float *temp_sensitivity, float *sensor_intercept) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
ASSERT_NOT_NULL(offset);
- ASSERT_NOT_NULL(offset_temp);
+ ASSERT_NOT_NULL(offset_temp_celsius);
ASSERT_NOT_NULL(timestamp_nanos);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
- // Updates the latest offset so that it is the one nearest to the current
- // temperature.
- setNearestEstimate(over_temp_cal);
-
// Gets the over-temp calibration estimate and model data.
- memcpy(offset, over_temp_cal->latest_offset->offset, 3 * sizeof(float));
+ memcpy(offset, over_temp_cal->nearest_offset->offset, 3 * sizeof(float));
memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
- *offset_temp = over_temp_cal->latest_offset->offset_temp;
- *timestamp_nanos = over_temp_cal->latest_offset->timestamp_nanos;
+ *offset_temp_celsius = over_temp_cal->nearest_offset->offset_temp_celsius;
+ *timestamp_nanos = over_temp_cal->nearest_offset->timestamp_nanos;
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:STORED]",
+ "Over-temperature model parameters stored.");
+#endif // OVERTEMPCAL_DBG_ENABLED
}
void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos, float xi, float yi,
float zi, float *xo, float *yo, float *zo) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
ASSERT_NOT_NULL(xo);
ASSERT_NOT_NULL(yo);
ASSERT_NOT_NULL(zo);
- // Determines whether over-temp compensation will be applied.
- if (!over_temp_cal->over_temp_enable) {
- return;
- }
-
// Removes very old data from the collected model estimates (eliminates
// drift-compromised data). Only does this when there is more than one
// estimate in the model (i.e., don't want to remove all data, even if it is
@@ -283,10 +291,17 @@
if (removeStaleModelData(over_temp_cal, timestamp_nanos)) {
// If anything was removed, then this attempts to recompute the model.
- computeModelUpdate(over_temp_cal, timestamp_nanos);
+ if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
+ computeModelUpdate(over_temp_cal, timestamp_nanos);
+ }
}
}
+ // Determines whether over-temp compensation will be applied.
+ if (!over_temp_cal->over_temp_enable) {
+ return;
+ }
+
// Removes the over-temperature compensated offset from the input sensor data.
removeSensorOffset(over_temp_cal, xi, 0, xo);
removeSensorOffset(over_temp_cal, yi, 1, yo);
@@ -309,7 +324,7 @@
const float *offset,
float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
ASSERT_NOT_NULL(offset);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
@@ -318,22 +333,35 @@
return;
}
-#ifdef OVERTEMPCAL_DBG_ENABLED
- // Updates the total count of offset estimates.
- over_temp_cal->debug_num_estimates++;
+ // Checks whether this offset estimate is a likely outlier. A limit is placed
+ // on 'num_outliers', the previous number of successive rejects, to prevent
+ // too many back-to-back rejections.
+ if (over_temp_cal->num_outliers < OVERTEMPCAL_MAX_OUTLIER_COUNT) {
+ if (outlierCheck(over_temp_cal, offset, 0, temperature_celsius) ||
+ outlierCheck(over_temp_cal, offset, 1, temperature_celsius) ||
+ outlierCheck(over_temp_cal, offset, 2, temperature_celsius)) {
+ // Increments the count of rejected outliers.
+ over_temp_cal->num_outliers++;
- // If there are fewer than the minimum number of points to produce a model,
- // then trigger a debug printout to view the model building process.
- over_temp_cal->debug_print_trigger |=
- (over_temp_cal->num_model_pts <= over_temp_cal->min_num_model_pts);
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:OUTLIER]",
+ "Offset|Temperature|Time [mdps|Celcius|nsec] = "
+ "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+ CAL_ENCODE_FLOAT(offset[0] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(offset[1] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(offset[2] * RAD_TO_MILLI_DEGREES, 6),
+ CAL_ENCODE_FLOAT(temperature_celsius, 3),
+ (unsigned long long int)timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
- // Provides an early escape if this is the first model estimate.
- if (over_temp_cal->num_model_pts == 0) {
- setLatestEstimate(over_temp_cal, offset, temperature_celsius,
- timestamp_nanos);
- over_temp_cal->num_model_pts = 1; // one estimate was added above.
- return;
+ return; // Skips the process of adding this offset to the model.
+ } else {
+ // Resets the count of rejected outliers.
+ over_temp_cal->num_outliers = 0;
+ }
+ } else {
+ // Resets the count of rejected outliers.
+ over_temp_cal->num_outliers = 0;
}
// Computes the temperature bin range data.
@@ -351,48 +379,83 @@
// temp_lo_check = bin_num * delta_temp_per_bin
// temp_hi_check = (bin_num + 1) * delta_temp_per_bin
// Check condition:
- // temp_lo_check <= model_data[i].offset_temp < temp_hi_check
+ // temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check
bool replaced_one = false;
size_t i = 0;
for (i = 0; i < over_temp_cal->num_model_pts; i++) {
- if (over_temp_cal->model_data[i].offset_temp < temp_hi_check &&
- over_temp_cal->model_data[i].offset_temp >= temp_lo_check) {
- // NOTE - the pointer to the estimate that is getting replaced is set
- // here; the offset values are set below in the call to
- // 'setLatestEstimate'.
- over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
+ if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check &&
+ over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) {
+ // NOTE - the pointer to the new model data point is set here; the offset
+ // data is set below in the call to 'setNearestEstimate'.
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
replaced_one = true;
break;
}
}
- // 3) If nothing was replaced, and the 'model_data' buffer is not full
- // then add the estimate data to the array.
- // 4) Otherwise (nothing was replaced and buffer is full), replace the
- // 'latest_offset' with the incoming one. This is done below.
+ // NOTE - the pointer to the new model data point is set here; the offset
+ // data is set below in the call to 'setNearestEstimate'.
if (!replaced_one && over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) {
- // NOTE - the pointer to the next available array location is set here;
- // the offset values are set below in the call to 'setLatestEstimate'.
- over_temp_cal->latest_offset =
- &over_temp_cal->model_data[over_temp_cal->num_model_pts];
- over_temp_cal->num_model_pts++;
+ if (over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) {
+ // 3) If nothing was replaced, and the 'model_data' buffer is not full
+ // then add the estimate data to the array.
+ over_temp_cal->nearest_offset =
+ &over_temp_cal->model_data[over_temp_cal->num_model_pts];
+ over_temp_cal->num_model_pts++;
+ } else {
+ // 4) Otherwise (nothing was replaced and buffer is full), replace the
+ // oldest data with the incoming one.
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
+ for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+ if (over_temp_cal->nearest_offset->timestamp_nanos <
+ over_temp_cal->model_data[i].timestamp_nanos) {
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
+ }
+ }
+ }
}
- // Updates the latest model estimate data.
- setLatestEstimate(over_temp_cal, offset, temperature_celsius,
- timestamp_nanos);
+ // Updates the model estimate data nearest to the sensor's temperature.
+ setNearestEstimate(over_temp_cal, offset, temperature_celsius,
+ timestamp_nanos);
- // Conditionally updates the over-temp model. See 'computeModelUpdate' for
- // update conditions.
- computeModelUpdate(over_temp_cal, timestamp_nanos);
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ // Updates the latest sensor offset estimate so this can be tracked for debug
+ // printout later.
+ memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
+ over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
+ // Updates the total number of received sensor offset estimates.
+ over_temp_cal->debug_num_estimates++;
+#endif // OVERTEMPCAL_DBG_ENABLED
+
+ // The rules for determining whether a new model fit is computed are:
+ // 1) A minimum number of data points must have been collected:
+ // num_model_pts >= min_num_model_pts
+ // NOTE: Collecting 'num_model_pts' and given that only one point is
+ // kept per temperature bin (spanning a thermal range specified by
+ // 'delta_temp_per_bin'), implies that model data covers at least,
+ // model_temperature_span >= 'num_model_pts' * delta_temp_per_bin
+ // 2) New model updates will not occur for intervals less than:
+ // (current_timestamp_nanos - modelupdate_timestamp_nanos) <
+ // min_update_interval_nanos
+ if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts ||
+ (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) <
+ over_temp_cal->min_update_interval_nanos) {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ // Triggers a log printout to show the updated sensor offset estimate.
+ updateDebugData(over_temp_cal);
+#endif // OVERTEMPCAL_DBG_ENABLED
+ } else {
+ // The conditions satisfy performing a new model update.
+ computeModelUpdate(over_temp_cal, timestamp_nanos);
+ }
}
void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos,
float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
#ifdef OVERTEMPCAL_DBG_ENABLED
#ifdef OVERTEMPCAL_DBG_LOG_TEMP
@@ -414,17 +477,13 @@
// Updates the sensor temperature.
over_temp_cal->temperature_celsius = temperature_celsius;
- // If any of the models for the sensor axes are in an initial state, then
- // this searches for the sensor offset estimate closest to the current
+ // This searches for the sensor offset estimate closest to the current
// temperature. A timer is used to limit the rate at which this search is
// performed.
- if (over_temp_cal->num_model_pts > 1 &&
- (over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE ||
- over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE ||
- over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) &&
+ if (over_temp_cal->num_model_pts > 0 &&
(timestamp_nanos - over_temp_cal->nearest_search_timer) >=
OVERTEMPCAL_NEAREST_NANOS) {
- setNearestEstimate(over_temp_cal);
+ findNearestEstimate(over_temp_cal);
over_temp_cal->nearest_search_timer = timestamp_nanos; // Reset timer.
}
}
@@ -446,8 +505,9 @@
for (j = 0; j < 3; j++) {
max_error_test =
NANO_ABS(over_temp_cal->model_data[i].offset[j] -
- getCompensatedOffset(over_temp_cal->model_data[i].offset_temp,
- temp_sensitivity[j], sensor_intercept[j]));
+ (temp_sensitivity[j] *
+ over_temp_cal->model_data[i].offset_temp_celsius +
+ sensor_intercept[j]));
if (max_error_test > max_error[j]) {
max_error[j] = max_error_test;
}
@@ -457,40 +517,22 @@
/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
-void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
- float offset_temp, uint64_t timestamp_nanos) {
+void setNearestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
+ float offset_temp_celsius, uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(offset);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
// Sets the latest over-temp calibration estimate.
- over_temp_cal->latest_offset->offset[0] = offset[0];
- over_temp_cal->latest_offset->offset[1] = offset[1];
- over_temp_cal->latest_offset->offset[2] = offset[2];
- over_temp_cal->latest_offset->offset_temp = offset_temp;
- over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
+ memcpy(over_temp_cal->nearest_offset->offset, offset, 3 * sizeof(float));
+ over_temp_cal->nearest_offset->offset_temp_celsius = offset_temp_celsius;
+ over_temp_cal->nearest_offset->timestamp_nanos = timestamp_nanos;
}
void computeModelUpdate(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
- // The rules for determining whether a new model fit is computed are:
- // 1) A minimum number of data points must have been collected:
- // num_model_pts >= min_num_model_pts
- // NOTE: Collecting 'num_model_pts' and given that only one point is
- // kept per temperature bin (spanning a thermal range specified by
- // 'delta_temp_per_bin'), implies that model data covers at least,
- // model_temp_span >= 'num_model_pts' * delta_temp_per_bin
- // 2) New model updates will not occur for intervals less than:
- // (current_timestamp_nanos - modelupdate_timestamp_nanos) <
- // min_update_interval_nanos
- if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts ||
- (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) <
- over_temp_cal->min_update_interval_nanos) {
- return;
- }
-
// Updates the linear model fit.
float temp_sensitivity[3];
float sensor_intercept[3];
@@ -508,55 +550,62 @@
// a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
// b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
size_t i;
+ bool updated_one = false;
for (i = 0; i < 3; i++) {
if (max_error[i] < over_temp_cal->max_error_limit &&
NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit &&
NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) {
over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
over_temp_cal->sensor_intercept[i] = sensor_intercept[i];
+ updated_one = true;
} else {
#ifdef OVERTEMPCAL_DBG_ENABLED
CAL_DEBUG_LOG(
"[OVER_TEMP_CAL:REJECT]",
- "Rejected %c-Axis Parameters|Max Error [mdps/C|mdps|mdps] = "
- "%s%d.%06d, %s%d.%06d, %s%d.%06d",
+ "%c-Axis Parameters|Max Error|Time [mdps/C|mdps|mdps|nsec] = "
+ "%s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
kDebugAxisLabel[i],
CAL_ENCODE_FLOAT(temp_sensitivity[i] * RAD_TO_MILLI_DEGREES, 6),
CAL_ENCODE_FLOAT(sensor_intercept[i] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6));
+ CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6),
+ (unsigned long long int)timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
}
}
- // Resets the timer and sets the update flag.
- over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
- over_temp_cal->new_overtemp_model_available = true;
+ // If at least one of the axes updated then consider this a valid model
+ // update.
+ if (updated_one) {
+ // Resets the timer and sets the update flag.
+ over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
+ over_temp_cal->new_overtemp_model_available = true;
- // Track the total number of model updates, and set trigger to print log data.
#ifdef OVERTEMPCAL_DBG_ENABLED
- over_temp_cal->debug_num_model_updates++;
- over_temp_cal->debug_print_trigger |= true;
+ // Updates the total number of model updates, the debug data package, and
+ // triggers a log printout.
+ over_temp_cal->debug_num_model_updates++;
+ updateDebugData(over_temp_cal);
#endif // OVERTEMPCAL_DBG_ENABLED
+ }
}
-void setNearestEstimate(struct OverTempCal *over_temp_cal) {
+void findNearestEstimate(struct OverTempCal *over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
+ // Performs a brute force search for the estimate nearest the current sensor
+ // temperature.
size_t i = 0;
float dtemp_new = 0.0f;
float dtemp_old = FLT_MAX;
- struct OverTempCalDataPt *nearest_estimate = &over_temp_cal->model_data[0];
- for (i = 1; i < over_temp_cal->num_model_pts; i++) {
- dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp -
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
+ for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+ dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
over_temp_cal->temperature_celsius);
if (dtemp_new < dtemp_old) {
- nearest_estimate = &over_temp_cal->model_data[i];
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
dtemp_old = dtemp_new;
}
}
-
- // Set the 'latest_offset' to the estimate nearest the current temperature.
- over_temp_cal->latest_offset = nearest_estimate;
}
bool removeStaleModelData(struct OverTempCal *over_temp_cal,
@@ -566,15 +615,16 @@
size_t i;
bool removed_one = false;
for (i = 0; i < over_temp_cal->num_model_pts; i++) {
- if ((timestamp_nanos - over_temp_cal->model_data[i].timestamp_nanos) >
- over_temp_cal->age_limit_nanos) {
+ if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
+ (timestamp_nanos - over_temp_cal->model_data[i].timestamp_nanos) >
+ over_temp_cal->age_limit_nanos) {
removed_one |= removeModelDataByIndex(over_temp_cal, i);
}
}
// Updates the latest offset so that it is the one nearest to the current
// temperature.
- setNearestEstimate(over_temp_cal);
+ findNearestEstimate(over_temp_cal);
return removed_one;
}
@@ -589,6 +639,26 @@
return false;
}
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ CAL_DEBUG_LOG(
+ "[OVER_TEMP_CAL:REMOVE]",
+ "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+ "%s%d.%03d, %llu",
+ CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
+ (unsigned long long int)over_temp_cal->model_data[model_index]
+ .timestamp_nanos);
+#endif // OVERTEMPCAL_DBG_ENABLED
+
// Remove the model data at 'model_index'.
size_t i;
for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
@@ -597,12 +667,6 @@
}
over_temp_cal->num_model_pts--;
-#ifdef OVERTEMPCAL_DBG_ENABLED
- CAL_DEBUG_LOG("[OVER_TEMP_CAL:REMOVE]",
- "Removed Stale Data: Model Index = %lu",
- (unsigned long int)model_index);
-#endif // OVERTEMPCAL_DBG_ENABLED
-
return true;
}
@@ -634,8 +698,8 @@
// temperature range that is likely to get replaced with actual data soon.
const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS /
over_temp_cal->delta_temp_per_bin);
- float offset_temp =
- start_bin_num * (1.5f * over_temp_cal->delta_temp_per_bin);
+ float offset_temp_celsius =
+ (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
size_t i;
size_t j;
@@ -643,19 +707,19 @@
float offset[3];
const uint64_t timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;
for (j = 0; j < 3; j++) {
- offset[j] =
- getCompensatedOffset(offset_temp, over_temp_cal->temp_sensitivity[j],
- over_temp_cal->sensor_intercept[j]);
+ offset[j] = over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
+ over_temp_cal->sensor_intercept[j];
}
- over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
- setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos);
- offset_temp += over_temp_cal->delta_temp_per_bin;
+ over_temp_cal->nearest_offset = &over_temp_cal->model_data[i];
+ setNearestEstimate(over_temp_cal, offset, offset_temp_celsius,
+ timestamp_nanos);
+ offset_temp_celsius += over_temp_cal->delta_temp_per_bin;
over_temp_cal->num_model_pts++;
}
#ifdef OVERTEMPCAL_DBG_ENABLED
if (over_temp_cal->min_num_model_pts > 0) {
- CAL_DEBUG_LOG("[OVER_TEMP_CAL]", "Jump-Started Model: #Points = %lu.",
+ CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Model Jump-Start: #Points = %lu.",
(unsigned long int)over_temp_cal->min_num_model_pts);
}
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -679,7 +743,7 @@
// First pass computes the mean values.
for (i = 0; i < n; ++i) {
- st += over_temp_cal->model_data[i].offset_temp;
+ st += over_temp_cal->model_data[i].offset_temp_celsius;
sx += over_temp_cal->model_data[i].offset[0];
sy += over_temp_cal->model_data[i].offset[1];
sz += over_temp_cal->model_data[i].offset[2];
@@ -688,7 +752,8 @@
// Second pass computes the mean corrected second moment values.
const float inv_n = 1.0f / n;
for (i = 0; i < n; ++i) {
- const float t = over_temp_cal->model_data[i].offset_temp - st * inv_n;
+ const float t =
+ over_temp_cal->model_data[i].offset_temp_celsius - st * inv_n;
stt += t * t;
stsx += t * over_temp_cal->model_data[i].offset[0];
stsy += t * over_temp_cal->model_data[i].offset[1];
@@ -706,205 +771,232 @@
sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
}
-void removeSensorOffset(const struct OverTempCal *over_temp_cal, float vi,
- size_t index, float *vo) {
+void removeSensorOffset(const struct OverTempCal *over_temp_cal, float axis_in,
+ size_t index, float *axis_out) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
+ ASSERT_NOT_NULL(axis_out);
+
// Removes the over-temperature compensated offset from the input sensor data.
- if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE) {
- // If this axis is in its initial state, use the nearest estimate to perform
- // the compensation (in this case the latest estimate will be the nearest):
- // sensor_out = sensor_in - nearest_offset
- *vo = vi - over_temp_cal->latest_offset->offset[index];
+ if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE ||
+ NANO_ABS(over_temp_cal->temperature_celsius -
+ over_temp_cal->nearest_offset->offset_temp_celsius) <
+ over_temp_cal->delta_temp_per_bin) {
+ // Use the nearest estimate to perform the compensation if either of the
+ // following is true:
+ // 1) This axis model is in its initial state.
+ // 2) The current temperature is within a small neighborhood of the
+ // 'nearest_offset'.
+ // axis_out = axis_in - nearest_offset
+ *axis_out = axis_in - over_temp_cal->nearest_offset->offset[index];
} else {
- // sensor_out = sensor_in - compensated_offset
+ // axis_out = axis_in - compensated_offset
// Where,
- // compensated_offset = (temp_sensitivity * temp_meas + sensor_intercept)
- *vo = vi - getCompensatedOffset(over_temp_cal->temperature_celsius,
- over_temp_cal->temp_sensitivity[index],
- over_temp_cal->sensor_intercept[index]);
+ // compensated_offset = (temp_sensitivity * temperature + sensor_intercept)
+ *axis_out = axis_in - (over_temp_cal->temp_sensitivity[index] *
+ over_temp_cal->temperature_celsius +
+ over_temp_cal->sensor_intercept[index]);
}
}
-float getCompensatedOffset(float temperature, float temp_sensitivity,
- float sensor_intercept) {
- return temp_sensitivity * temperature + sensor_intercept;
+bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
+ size_t axis_index, float temperature_celsius) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(offset);
+
+ // If a model has been defined, then check to see if this offset could be a
+ // potential outlier:
+ if (over_temp_cal->temp_sensitivity[axis_index] < MODEL_INITIAL_STATE) {
+ const float max_error_test = NANO_ABS(
+ offset[axis_index] -
+ (over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius +
+ over_temp_cal->sensor_intercept[axis_index]));
+
+ if (max_error_test > over_temp_cal->max_error_limit) {
+ return true;
+ }
+ }
+
+ return false;
}
/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
#ifdef OVERTEMPCAL_DBG_ENABLED
-// Debug printout state enumeration.
-enum DebugState {
- IDLE = 0,
- WAIT_STATE,
- PRINT_HEADER,
- PRINT_OFFSET,
- PRINT_SENSITIVITY,
- PRINT_INTERCEPT,
- PRINT_ERROR,
- PRINT_MODEL_PTS,
- PRINT_MODEL_DATA
-};
-
-void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos) {
+void updateDebugData(struct OverTempCal* over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
- ASSERT_NOT_NULL(over_temp_cal->latest_offset);
+ ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
- static enum DebugState debug_state = IDLE;
- static enum DebugState next_state = 0;
- static uint64_t wait_timer = 0;
- static size_t i = 0; // Counter.
+ // 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 (over_temp_cal->debug_state != OTC_IDLE) {
+ return;
+ }
- // NOTE - The un-initialized model state is indicated by
- // temp_sensitivity=MODEL_INITIAL_STATE. The following filters out this
- // condition for the data printout below.
- float compensated_offset[3];
- float temp_sensitivity[3];
- float max_error[3];
+ // Triggers a debug log printout.
+ over_temp_cal->debug_print_trigger = true;
+
+ // Initializes the debug data structure.
+ memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
+
+ // Copies over the relevant data.
+ memcpy(over_temp_cal->debug_overtempcal.sensor_intercept,
+ over_temp_cal->sensor_intercept, 3 * sizeof(float));
+ memcpy(&over_temp_cal->debug_overtempcal.nearest_offset,
+ over_temp_cal->nearest_offset, sizeof(struct OverTempCalDataPt));
+
+ over_temp_cal->debug_overtempcal.num_model_pts = over_temp_cal->num_model_pts;
+ over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos =
+ over_temp_cal->modelupdate_timestamp_nanos;
+ over_temp_cal->debug_overtempcal.temperature_celsius =
+ over_temp_cal->temperature_celsius;
+
size_t j;
for (j = 0; j < 3; j++) {
- temp_sensitivity[j] =
+ over_temp_cal->debug_overtempcal.temp_sensitivity[j] =
(over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
? 0.0f
: over_temp_cal->temp_sensitivity[j];
}
+ // Computes the maximum error over all of the model data.
+ getModelError(over_temp_cal,
+ over_temp_cal->debug_overtempcal.temp_sensitivity,
+ over_temp_cal->debug_overtempcal.sensor_intercept,
+ over_temp_cal->debug_overtempcal.max_error);
+}
+
+void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos) {
+ ASSERT_NOT_NULL(over_temp_cal);
+
+ static enum OverTempCalDebugState next_state = 0;
+ static uint64_t wait_timer = 0;
+ static size_t i = 0; // Counter.
+
// This is a state machine that controls the reporting out of debug data.
- switch (debug_state) {
- case IDLE:
+ switch (over_temp_cal->debug_state) {
+ case OTC_IDLE:
// Wait for a trigger and start the debug printout sequence.
if (over_temp_cal->debug_print_trigger) {
- debug_state = PRINT_HEADER;
CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "");
+ CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s",
+ OVERTEMPCAL_DEBUG_VERSION_STRING);
over_temp_cal->debug_print_trigger = false; // Resets trigger.
+ over_temp_cal->debug_state = OTC_PRINT_OFFSET;
} else {
- debug_state = IDLE;
+ over_temp_cal->debug_state = OTC_IDLE;
}
break;
- case PRINT_HEADER:
- // Print out header.
- CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s",
- OVERTEMPCAL_DEBUG_VERSION_STRING);
-
- // Prints out number of offsets.
- CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Total Offsets = %lu",
- (unsigned long int)over_temp_cal->debug_num_estimates);
-
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_OFFSET; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case PRINT_OFFSET:
- // Computes the compensated sensor offset based on the current
- // temperature.
- for (j = 0; j < 3; j++) {
- compensated_offset[j] =
- (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE)
- ? over_temp_cal->latest_offset->offset[j]
- : getCompensatedOffset(over_temp_cal->temperature_celsius,
- over_temp_cal->temp_sensitivity[j],
- over_temp_cal->sensor_intercept[j]);
- }
-
- CAL_DEBUG_LOG(
- OVERTEMPCAL_REPORT_TAG,
- "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, "
- "%s%d.%06d, %s%d.%06d, %llu",
- CAL_ENCODE_FLOAT(compensated_offset[0] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(compensated_offset[1] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(compensated_offset[2] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(over_temp_cal->temperature_celsius, 6),
- (unsigned long long int)
- over_temp_cal->latest_offset->timestamp_nanos);
-
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_SENSITIVITY; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case WAIT_STATE:
+ case OTC_WAIT_STATE:
// This helps throttle the print statements.
if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) {
- debug_state = next_state;
+ over_temp_cal->debug_state = next_state;
}
break;
- case PRINT_SENSITIVITY:
- // Prints out the modeled temperature sensitivity.
+ case OTC_PRINT_OFFSET:
+ // Prints out the latest GyroCal offset estimate (input data).
CAL_DEBUG_LOG(
OVERTEMPCAL_REPORT_TAG,
- "Modeled Temperature Sensitivity [mdps/C] = %s%d.%06d, %s%d.%06d, "
- "%s%d.%06d",
- CAL_ENCODE_FLOAT(temp_sensitivity[0] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(temp_sensitivity[1] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(temp_sensitivity[2] * RAD_TO_MILLI_DEGREES, 6));
+ "Cal#|Offset|Temp|Time [mdps|C|nsec]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%03d, %llu",
+ (unsigned long int)over_temp_cal->debug_num_estimates,
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.nearest_offset.offset[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.nearest_offset.offset[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.nearest_offset.offset[2] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.nearest_offset
+ .offset_temp_celsius,
+ 6),
+ (unsigned long long int)
+ over_temp_cal->debug_overtempcal.nearest_offset.timestamp_nanos);
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_INTERCEPT; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_PRINT_MODEL_PARAMETERS; // Sets the next state.
+ over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_INTERCEPT:
- // Prints out the modeled temperature intercept.
- CAL_DEBUG_LOG(
- OVERTEMPCAL_REPORT_TAG,
- "Modeled Temperature Intercept [mdps] = %s%d.%06d, %s%d.%06d, "
- "%s%d.%06d",
- CAL_ENCODE_FLOAT(
- over_temp_cal->sensor_intercept[0] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(
- over_temp_cal->sensor_intercept[1] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(
- over_temp_cal->sensor_intercept[2] * RAD_TO_MILLI_DEGREES, 6));
+ case OTC_PRINT_MODEL_PARAMETERS:
+ // Prints out the model parameters.
+ CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG,
+ "Cal#|Sensitivity|Intercept [mdps/C|mdps]: %lu, %s%d.%06d, "
+ "%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
+ (unsigned long int)over_temp_cal->debug_num_estimates,
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.sensor_intercept[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.sensor_intercept[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.sensor_intercept[2] *
+ RAD_TO_MILLI_DEGREES,
+ 6));
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_ERROR; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state.
+ over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_ERROR:
+ case OTC_PRINT_MODEL_ERROR:
// Computes the maximum error over all of the model data.
- if (over_temp_cal->num_model_pts > 0) {
- getModelError(over_temp_cal, temp_sensitivity,
- over_temp_cal->sensor_intercept, max_error);
+ CAL_DEBUG_LOG(
+ OVERTEMPCAL_REPORT_TAG,
+ "Cal#|#Updates|#ModelPts|Model Error|Update Time [mdps|nsec]: %lu, "
+ "%lu, %lu, %s%d.%06d, %s%d.%06d, %s%d.%06d, %llu",
+ (unsigned long int)over_temp_cal->debug_num_estimates,
+ (unsigned long int)over_temp_cal->debug_num_model_updates,
+ (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
+ CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[1] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[2] *
+ RAD_TO_MILLI_DEGREES,
+ 6),
+ (unsigned long long int)
+ over_temp_cal->debug_overtempcal.modelupdate_timestamp_nanos);
- // Reports the resulting model error.
- CAL_DEBUG_LOG(
- OVERTEMPCAL_REPORT_TAG,
- "Model Error [mdps] = %s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(max_error[0] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(max_error[1] * RAD_TO_MILLI_DEGREES, 6),
- CAL_ENCODE_FLOAT(max_error[2] * RAD_TO_MILLI_DEGREES, 6));
- }
-
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_MODEL_PTS; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ i = 0; // Resets the model data printer counter.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_PRINT_MODEL_DATA; // Sets the next state.
+ over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
- case PRINT_MODEL_PTS:
- // Prints out the number of model points/updates.
- CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Points = %lu",
- (unsigned long int)over_temp_cal->num_model_pts);
-
- CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Updates = %lu",
- (unsigned long int)over_temp_cal->debug_num_model_updates);
-
- i = 0; // Resets the counter.
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_MODEL_DATA; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
- break;
-
- case PRINT_MODEL_DATA:
+ case OTC_PRINT_MODEL_DATA:
// Prints out all of the model data.
if (i < over_temp_cal->num_model_pts) {
CAL_DEBUG_LOG(
OVERTEMPCAL_REPORT_TAG,
- " Model[%lu] [mdps|C] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
- "%s%d.%03d ",
+ " Model[%lu] [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, %s%d.%06d, "
+ "%s%d.%03d, %llu",
(unsigned long int)i,
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[i].offset[0] * RAD_TO_MILLI_DEGREES,
@@ -915,26 +1007,30 @@
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[i].offset[2] * RAD_TO_MILLI_DEGREES,
6),
- CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp, 3));
+ CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp_celsius,
+ 3),
+ (unsigned long long int)over_temp_cal->model_data[i]
+ .timestamp_nanos);
i++;
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = PRINT_MODEL_DATA; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_PRINT_MODEL_DATA; // Sets the next state.
+ over_temp_cal->debug_state =
+ OTC_WAIT_STATE; // First, go to wait state.
} else {
- debug_state = IDLE; // Goes to idle state.
- CAL_DEBUG_LOG(
- OVERTEMPCAL_REPORT_TAG, "Last Model Update [nsec] = %llu",
- (unsigned long long int)over_temp_cal->modelupdate_timestamp_nanos);
+ // Sends this state machine to its idle state.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_IDLE; // Sets the next state.
+ over_temp_cal->debug_state =
+ OTC_WAIT_STATE; // First, go to wait state.
}
break;
default:
// Sends this state machine to its idle state.
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = IDLE; // Sets the next state.
- debug_state = WAIT_STATE; // First, go to wait state.
+ wait_timer = timestamp_nanos; // Starts the wait timer.
+ next_state = OTC_IDLE; // Sets the next state.
+ over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
}
}
-
#endif // OVERTEMPCAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 928576f..4ae8952 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -66,17 +66,50 @@
#endif
// Defines the maximum size of the 'model_data' array.
-#define OVERTEMPCAL_MODEL_SIZE (20)
+#define OVERTEMPCAL_MODEL_SIZE (40)
+
+// The maximum number of successive outliers that may be rejected.
+#define OVERTEMPCAL_MAX_OUTLIER_COUNT (3)
// Over-temperature sensor offset estimate structure.
struct OverTempCalDataPt {
// Sensor offset estimate, temperature, and timestamp.
float offset[3];
- float offset_temp; // [Celsius]
- uint64_t timestamp_nanos; // [nanoseconds]
- // TODO(davejacobs) - Design option: add variance to provide weighting info.
+ float offset_temp_celsius; // [Celsius]
+ uint64_t timestamp_nanos; // [nanoseconds]
};
+#ifdef OVERTEMPCAL_DBG_ENABLED
+// Debug printout state enumeration.
+enum OverTempCalDebugState {
+ OTC_IDLE = 0,
+ OTC_WAIT_STATE,
+ OTC_PRINT_OFFSET,
+ OTC_PRINT_MODEL_PARAMETERS,
+ OTC_PRINT_MODEL_ERROR,
+ OTC_PRINT_MODEL_DATA
+};
+
+// OverTempCal debug information/data tracking structure.
+struct DebugOverTempCal {
+ uint64_t modelupdate_timestamp_nanos;
+
+ // The most recent offset estimate received.
+ struct OverTempCalDataPt latest_offset;
+
+ // The offset estimate nearest the current sensor temperature.
+ struct OverTempCalDataPt nearest_offset;
+
+ // The maximum model error over all model_data points.
+ float max_error[3];
+
+ float temp_sensitivity[3];
+ float sensor_intercept[3];
+ float temperature_celsius;
+ size_t num_model_pts;
+};
+#endif // OVERTEMPCAL_DBG_ENABLED
+
// The following data structure contains all of the necessary components for
// modeling a sensor's temperature dependency and providing over-temperature
// offset corrections.
@@ -99,10 +132,8 @@
// The temperature at which the offset compensation is performed.
float temperature_celsius;
- // Pointer to the most recent sensor offset estimate. This is also updated
- // periodically to point to the offset estimate closest to the current sensor
- // temperature.
- struct OverTempCalDataPt *latest_offset;
+ // Pointer to the offset estimate closest to the current sensor temperature.
+ struct OverTempCalDataPt *nearest_offset;
///// Online Model Identification Parameters ////////////////////////////////
//
@@ -130,6 +161,11 @@
float temp_sensitivity_limit; // [sensor units/Celsius]
float sensor_intercept_limit; // [sensor units]
+ // The number of successive outliers rejected in a row. This is used to
+ // prevent the possibility of a bad state where an initial bad fit causes
+ // good data to be continually rejected.
+ size_t num_outliers;
+
// The rules for accepting new offset estimates into the 'model_data'
// collection:
// 1) The temperature domain is divided into bins each spanning
@@ -139,15 +175,15 @@
// temp_lo_check = bin_num * delta_temp_per_bin
// temp_hi_check = (bin_num + 1) * delta_temp_per_bin
// Check condition:
- // temp_lo_check <= model_data[i].offset_temp < temp_hi_check
+ // temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check
// 3) If nothing was replaced, and the 'model_data' buffer is not full then
// add the sensor offset estimate to the array.
// 4) Otherwise (nothing was replaced and buffer is full), replace the
- // 'latest_offset' with the incoming one.
+ // oldest data with the incoming one.
// This approach ensures a uniform spread of collected data, keeps the most
// recent estimates in cases where they arrive frequently near a given
// temperature, and prevents model oversampling (i.e., dominance of estimates
- // concentrated at given set of temperatures).
+ // concentrated at a given set of temperatures).
float delta_temp_per_bin; // [Celsius/bin]
// Timer used to limit the rate at which a search for the nearest offset
@@ -169,11 +205,13 @@
// overTempCalNewModelUpdateAvailable() is called. This variable indicates
// that the following should be stored/updated in persistent system memory:
// 1) 'temp_sensitivity' and 'sensor_intercept'.
- // 2) The sensor offset data pointed to by 'latest_offset'
+ // 2) The sensor offset data pointed to by 'nearest_offset'
// (saving timestamp information is not required).
bool new_overtemp_model_available;
#ifdef OVERTEMPCAL_DBG_ENABLED
+ struct DebugOverTempCal debug_overtempcal; // Debug data structure.
+ enum OverTempCalDebugState debug_state; // Debug printout state machine.
size_t debug_num_model_updates; // Total number of model updates.
size_t debug_num_estimates; // Total number of offset estimates.
bool debug_print_trigger; // Flag used to trigger data printout.
@@ -216,7 +254,7 @@
* INPUTS:
* over_temp_cal: Over-temp main data structure.
* offset: Update values for the latest offset estimate (array).
- * offset_temp: Measured temperature for the offset estimate.
+ * offset_temp_celsius: Measured temperature for the offset estimate.
* timestamp_nanos: Timestamp for the offset estimate [nanoseconds].
* temp_sensitivity: Modeled temperature sensitivity (array).
* sensor_intercept: Linear model intercept for the over-temp model (array).
@@ -226,7 +264,7 @@
* NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
*/
void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
- float offset_temp, uint64_t timestamp_nanos,
+ float offset_temp_celsius, uint64_t timestamp_nanos,
const float *temp_sensitivity,
const float *sensor_intercept, bool jump_start_model);
@@ -237,7 +275,7 @@
* over_temp_cal: Over-temp data structure.
* OUTPUTS:
* offset: Offset values for the latest offset estimate (array).
- * offset_temp: Measured temperature for the offset estimate.
+ * offset_temp_celsius: Measured temperature for the offset estimate.
* timestamp_nanos: Timestamp for the offset estimate [nanoseconds].
* temp_sensitivity: Modeled temperature sensitivity (array).
* sensor_intercept: Linear model intercept for the over-temp model (array).
@@ -245,7 +283,7 @@
* NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
*/
void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset,
- float *offset_temp, uint64_t *timestamp_nanos,
+ float *offset_temp_celsius, uint64_t *timestamp_nanos,
float *temp_sensitivity, float *sensor_intercept);
/*
@@ -296,8 +334,8 @@
* the estimate determined by the input model parameters.
* max_error (over all i)
* |model_data[i]->offset_xyz -
- * getCompensatedOffset(model_data[i]->offset_temp, temp_sensitivity,
- * sensor_intercept)|
+ * getCompensatedOffset(model_data[i]->offset_temp_celsius,
+ * temp_sensitivity, sensor_intercept)|
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
