[GyroCal/OTC-Gyro] Algorithm Sync for MR1
This CL includes code maintanence and clean-up for MR1.
Changes are synced from Google3.
Included:
- Moved helper macros from vec.h to separate file macros.h.
- Addressed some reviewer comments ag/2595812.
Tip of G3 CL: 165657110
Bug: 64755461
Test: Built and verified on hardware.
Change-Id: Ic19e54d98d760ef0173eebe452dd713ebe45a35a
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index d6a69f3..3179b0e 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -21,7 +21,7 @@
#include <string.h>
#include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
/////// DEFINITIONS AND MACROS ///////////////////////////////////////
@@ -29,28 +29,19 @@
// of the given sensor).
#define MAX_GYRO_BIAS (0.2f) // [rad/sec]
-// Converts units of radians to milli-degrees.
-#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / NANO_PI)
-
// Watchdog timeout value (5 seconds). Monitors dropouts in sensor data and
// resets when exceeded.
-#define GYRO_WATCHDOG_TIMEOUT_NANOS (5000000000)
+#define GYRO_WATCHDOG_TIMEOUT_NANOS (SEC_TO_NANOS(5))
#ifdef GYRO_CAL_DBG_ENABLED
// The time value used to throttle debug messaging.
-#define GYROCAL_WAIT_TIME_NANOS (100000000)
-
-// Unit conversion: nanoseconds to seconds.
-#define NANOS_TO_SEC (1.0e-9f)
+#define GYROCAL_WAIT_TIME_NANOS (MSEC_TO_NANOS(100))
// A debug version label to help with tracking results.
#define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
// 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 /////////////////////////////////////////
@@ -113,7 +104,7 @@
};
/*
- * Updates running calculation of the gyro's mean sampling rate.
+ * Updates the running calculation of the gyro's mean sampling rate.
*
* Behavior:
* 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
@@ -123,11 +114,13 @@
* 3) Otherwise, the local estimate of the mean sampling rates is updated.
*
* INPUTS:
- * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update.
+ * sample_rate_estimator: Pointer to the estimator data structure.
+ * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update.
* timestamp_nanos: Time stamp (nanoseconds).
* reset_stats: Flag that signals a reset of the sampling rate estimate.
*/
-static void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+ float* debug_mean_sampling_rate_hz,
uint64_t timestamp_nanos, bool reset_stats);
// Updates the information used for debug printouts.
@@ -146,17 +139,11 @@
uint32_t hi = v >> 32, lo = v;
if (!hi) //this is very fast for cases where we fit into a uint32_t
- return(float)lo;
+ return (float)lo;
else {
return ((float)hi) * 4294967296.0f + (float)lo;
}
}
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-// Prints debug information useful for tuning the GyroCal parameters.
-static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
- uint64_t timestamp_nanos);
-#endif // GYRO_CAL_DBG_TUNE_ENABLED
#endif // GYRO_CAL_DBG_ENABLED
/////// FUNCTION DEFINITIONS /////////////////////////////////////////
@@ -235,7 +222,8 @@
}
// Ensures that the gyro sampling rate estimate is reset.
- gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true);
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
+ /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
}
@@ -264,10 +252,10 @@
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
- "Gyro Bias Calibration [mDPS]: %s%d.%03d, %s%d.%03d, %s%d.%03d",
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 3));
+ "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
+ CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
#endif // GYRO_CAL_DBG_ENABLED
}
@@ -310,7 +298,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Update the gyro sampling rate estimate.
- gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false);
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+ sample_time_nanos, /*reset_stats=*/false);
#endif // GYRO_CAL_DBG_ENABLED
// Pass gyro data to stillness detector
@@ -453,7 +442,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Resets the sampling rate estimate.
- gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+ sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
// Update stillness flag. Force the start of a new stillness period.
@@ -496,7 +486,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Resets the sampling rate estimate.
- gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+ sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
// Update stillness flag.
@@ -517,20 +508,18 @@
gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
#ifdef GYRO_CAL_DBG_ENABLED
- CAL_DEBUG_LOG("[GYRO_CAL:REJECT]",
- "Offset|Temp|Time [mDPS|C|nsec]: %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %llu",
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
- (unsigned long long int)calibration_time_nanos);
+ CAL_DEBUG_LOG(
+ "[GYRO_CAL:REJECT]",
+ "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
+ CAL_ENCODE_FLOAT(
+ gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->gyro_stillness_detect.prev_mean_y * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
+ (unsigned long long int)calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Outside of range. Ignore, reset, and continue.
@@ -620,7 +609,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Resets the sampling rate estimate.
- gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true);
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
+ sample_time_nanos, /*reset_stats=*/true);
#endif // GYRO_CAL_DBG_ENABLED
// Resets the stillness window end-time.
@@ -661,10 +651,8 @@
gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f;
// Initializes the min/max temperatures values.
- gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
- FLT_MAX;
- gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
- -FLT_MAX;
+ gyro_cal->temperature_mean_tracker.temperature_min_celsius = FLT_MAX;
+ gyro_cal->temperature_mean_tracker.temperature_max_celsius = -FLT_MAX;
break;
case DO_UPDATE_DATA:
@@ -676,14 +664,14 @@
// Tracks the min, max, and latest temperature values.
gyro_cal->temperature_mean_tracker.latest_temperature_celsius =
temperature_celsius;
- if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] >
+ if (gyro_cal->temperature_mean_tracker.temperature_min_celsius >
temperature_celsius) {
- gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
+ gyro_cal->temperature_mean_tracker.temperature_min_celsius =
temperature_celsius;
}
- if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] <
+ if (gyro_cal->temperature_mean_tracker.temperature_max_celsius <
temperature_celsius) {
- gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
+ gyro_cal->temperature_mean_tracker.temperature_max_celsius =
temperature_celsius;
}
break;
@@ -709,9 +697,10 @@
// Records the min/max and mean temperature values for debug purposes.
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_mean_tracker.temperature_min_max_celsius,
- 2 * sizeof(float));
+ gyro_cal->debug_gyro_cal.temperature_min_celsius =
+ gyro_cal->temperature_mean_tracker.temperature_min_celsius;
+ gyro_cal->debug_gyro_cal.temperature_max_celsius =
+ gyro_cal->temperature_mean_tracker.temperature_max_celsius;
#endif
break;
@@ -719,9 +708,8 @@
// Determines if the min/max delta exceeded the set limit.
if (gyro_cal->temperature_mean_tracker.num_points > 0) {
min_max_temp_exceeded =
- (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] -
- gyro_cal->temperature_mean_tracker
- .temperature_min_max_celsius[0]) >
+ (gyro_cal->temperature_mean_tracker.temperature_max_celsius -
+ gyro_cal->temperature_mean_tracker.temperature_min_celsius) >
gyro_cal->temperature_delta_limit_celsius;
#ifdef GYRO_CAL_DBG_ENABLED
@@ -743,41 +731,50 @@
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] = -FLT_MAX;
+ for (size_t i = 0; i < 3; i++) {
+ gyro_cal->window_mean_tracker.gyro_winmean_min[i] = FLT_MAX;
+ gyro_cal->window_mean_tracker.gyro_winmean_max[i] = -FLT_MAX;
}
break;
case DO_UPDATE_DATA:
// Computes the min/max window mean values.
- if (gyro_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_cal->window_mean_tracker.gyro_winmean_min[0] >
+ gyro_cal->gyro_stillness_detect.win_mean_x) {
+ gyro_cal->window_mean_tracker.gyro_winmean_min[0] =
+ gyro_cal->gyro_stillness_detect.win_mean_x;
}
- if (gyro_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_cal->window_mean_tracker.gyro_winmean_max[0] <
+ gyro_cal->gyro_stillness_detect.win_mean_x) {
+ gyro_cal->window_mean_tracker.gyro_winmean_max[0] =
+ gyro_cal->gyro_stillness_detect.win_mean_x;
}
- if (gyro_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_cal->window_mean_tracker.gyro_winmean_min[1] >
+ gyro_cal->gyro_stillness_detect.win_mean_y) {
+ gyro_cal->window_mean_tracker.gyro_winmean_min[1] =
+ gyro_cal->gyro_stillness_detect.win_mean_y;
}
- if (gyro_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_cal->window_mean_tracker.gyro_winmean_max[1] <
+ gyro_cal->gyro_stillness_detect.win_mean_y) {
+ gyro_cal->window_mean_tracker.gyro_winmean_max[1] =
+ gyro_cal->gyro_stillness_detect.win_mean_y;
}
- if (gyro_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_cal->window_mean_tracker.gyro_winmean_min[2] >
+ gyro_cal->gyro_stillness_detect.win_mean_z) {
+ gyro_cal->window_mean_tracker.gyro_winmean_min[2] =
+ gyro_cal->gyro_stillness_detect.win_mean_z;
}
- if (gyro_winmean_max[2] < gyro_cal->gyro_stillness_detect.win_mean_z) {
- gyro_winmean_max[2] = gyro_cal->gyro_stillness_detect.win_mean_z;
+ if (gyro_cal->window_mean_tracker.gyro_winmean_max[2] <
+ gyro_cal->gyro_stillness_detect.win_mean_z) {
+ gyro_cal->window_mean_tracker.gyro_winmean_max[2] =
+ gyro_cal->gyro_stillness_detect.win_mean_z;
}
break;
@@ -785,34 +782,45 @@
// 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;
+ memcpy(gyro_cal->gyro_winmean_min,
+ gyro_cal->window_mean_tracker.gyro_winmean_min,
+ sizeof(gyro_cal->window_mean_tracker.gyro_winmean_min));
+ memcpy(gyro_cal->gyro_winmean_max,
+ gyro_cal->window_mean_tracker.gyro_winmean_max,
+ sizeof(gyro_cal->window_mean_tracker.gyro_winmean_max));
+ break;
case DO_EVALUATE:
// Performs the stability check and returns the 'true' if the difference
// between min/max window mean value is outside the stable range.
- for (i = 0; i < 3; i++) {
- mean_not_stable |= (gyro_winmean_max[i] - gyro_winmean_min[i]) >
+ for (size_t i = 0; i < 3; i++) {
+ mean_not_stable |= (gyro_cal->window_mean_tracker.gyro_winmean_max[i] -
+ gyro_cal->window_mean_tracker.gyro_winmean_min[i]) >
gyro_cal->stillness_mean_delta_limit;
}
#ifdef GYRO_CAL_DBG_ENABLED
if (mean_not_stable) {
CAL_DEBUG_LOG(
"[GYRO_CAL:MEAN_STABILITY_GATE]",
- "Variation Limit|Delta [mDPS]: %s%d.%03d | %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d",
+ "Variation Limit|Delta [mDPS]: " CAL_FORMAT_3DIGITS
+ " | " CAL_FORMAT_3DIGITS_TRIPLET,
+ CAL_ENCODE_FLOAT(gyro_cal->stillness_mean_delta_limit * RAD_TO_MDEG,
+ 3),
CAL_ENCODE_FLOAT(
- gyro_cal->stillness_mean_delta_limit * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT((gyro_winmean_max[0] - gyro_winmean_min[0]) *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT((gyro_winmean_max[1] - gyro_winmean_min[1]) *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT((gyro_winmean_max[2] - gyro_winmean_min[2]) *
- RAD_TO_MILLI_DEGREES,
- 3));
+ (gyro_cal->window_mean_tracker.gyro_winmean_max[0] -
+ gyro_cal->window_mean_tracker.gyro_winmean_min[0]) *
+ RAD_TO_MDEG,
+ 3),
+ CAL_ENCODE_FLOAT(
+ (gyro_cal->window_mean_tracker.gyro_winmean_max[1] -
+ gyro_cal->window_mean_tracker.gyro_winmean_min[1]) *
+ RAD_TO_MDEG,
+ 3),
+ CAL_ENCODE_FLOAT(
+ (gyro_cal->window_mean_tracker.gyro_winmean_max[2] -
+ gyro_cal->window_mean_tracker.gyro_winmean_min[2]) *
+ RAD_TO_MDEG,
+ 3));
}
#endif // GYRO_CAL_DBG_ENABLED
break;
@@ -825,21 +833,19 @@
}
#ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz,
+void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
+ float* debug_mean_sampling_rate_hz,
uint64_t timestamp_nanos, bool reset_stats) {
- // This is used for local calculations of average sampling rate.
- static uint64_t last_timestamp_nanos = 0;
- static uint64_t time_delta_accumulator = 0;
- static size_t num_samples = 0;
-
// If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
// out the estimate of the sampling rate.
if (debug_mean_sampling_rate_hz) {
- if (num_samples > 1 && time_delta_accumulator > 0) {
+ if (sample_rate_estimator->num_samples > 1 &&
+ sample_rate_estimator->time_delta_accumulator > 0) {
// Computes the final mean calculation.
*debug_mean_sampling_rate_hz =
- num_samples /
- (floatFromUint64(time_delta_accumulator) * NANOS_TO_SEC);
+ sample_rate_estimator->num_samples /
+ (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
+ NANOS_TO_SEC);
} else {
// Not enough samples to compute a valid sample rate estimate. Indicate
// this with a -1 value.
@@ -850,26 +856,28 @@
// Resets the sampling rate mean estimator data.
if (reset_stats) {
- last_timestamp_nanos = 0;
- time_delta_accumulator = 0;
- num_samples = 0;
+ sample_rate_estimator->last_timestamp_nanos = 0;
+ sample_rate_estimator->time_delta_accumulator = 0;
+ sample_rate_estimator->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;
+ if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+ sample_rate_estimator->last_timestamp_nanos == 0) {
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
return;
}
// Increments the number of samples.
- num_samples++;
+ sample_rate_estimator->num_samples++;
// Accumulate the time steps.
- time_delta_accumulator += timestamp_nanos - last_timestamp_nanos;
- last_timestamp_nanos = timestamp_nanos;
+ sample_rate_estimator->time_delta_accumulator +=
+ timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
}
void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
@@ -905,14 +913,15 @@
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,
+ gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
+ &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
/*reset_stats=*/true);
// 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));
+ sizeof(gyro_cal->gyro_winmean_min));
memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_max, gyro_cal->gyro_winmean_max,
- 3 * sizeof(float));
+ sizeof(gyro_cal->gyro_winmean_max));
// Records the previous stillness window means.
gyro_cal->debug_gyro_cal.accel_mean[0] =
@@ -971,23 +980,21 @@
float mag_data;
switch (print_data) {
case OFFSET:
- CAL_DEBUG_LOG(debug_tag,
- "Cal#|Offset|Temp|Time [mDPS|C|nsec]: %lu, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %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,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[1] *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.calibration[2] *
- RAD_TO_MILLI_DEGREES,
- 3),
- 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);
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
+ "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.calibration[1] * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
+ 3),
+ (unsigned long long int)
+ gyro_cal->debug_gyro_cal.end_still_time_nanos);
break;
case STILLNESS_DATA:
@@ -996,8 +1003,8 @@
: -1.0f; // Signals that magnetometer was not used.
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Stillness|Confidence [nsec]: %lu, %llu, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d",
+ "Cal#|Stillness|Confidence [nsec]: %lu, "
+ "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
(unsigned long int)gyro_cal->debug_calibration_count,
(unsigned long long int)(gyro_cal->debug_gyro_cal
.end_still_time_nanos -
@@ -1011,91 +1018,90 @@
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",
+ "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
+ "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+ ", " CAL_FORMAT_3DIGITS,
(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.temperature_min_celsius, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius, 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
+ gyro_cal->debug_gyro_cal.temperature_min_celsius,
+ 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.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.%03d, "
- "%s%d.%03d, %s%d.%03d",
+ "Cal#|Gyro Peak Stillness Variation [mDPS]: "
+ "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
(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,
+ RAD_TO_MDEG,
3),
CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[1] -
gyro_cal->debug_gyro_cal.gyro_winmean_min[1]) *
- RAD_TO_MILLI_DEGREES,
+ RAD_TO_MDEG,
3),
CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[2] -
gyro_cal->debug_gyro_cal.gyro_winmean_min[2]) *
- RAD_TO_MILLI_DEGREES,
+ RAD_TO_MDEG,
3));
break;
case ACCEL_STATS:
- CAL_DEBUG_LOG(
- debug_tag,
- "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %lu, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %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.accel_mean[0], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
+ CAL_DEBUG_LOG(debug_tag,
+ "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
+ "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_6DIGITS_TRIPLET,
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[0], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[1], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_var[2], 6));
break;
case GYRO_STATS:
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+ "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS_TRIPLET,
(unsigned long int)gyro_cal->debug_calibration_count,
- CAL_ENCODE_FLOAT(
- gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT(
- gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT(
- gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MILLI_DEGREES, 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[0] *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 3));
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[2] * RAD_TO_MDEG,
+ 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_var[0] * RAD_TO_MDEG * RAD_TO_MDEG,
+ 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_var[1] * RAD_TO_MDEG * RAD_TO_MDEG,
+ 3),
+ CAL_ENCODE_FLOAT(
+ gyro_cal->debug_gyro_cal.gyro_var[2] * RAD_TO_MDEG * RAD_TO_MDEG,
+ 3));
break;
case MAG_STATS:
if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
- CAL_DEBUG_LOG(debug_tag,
- "Cal#|Mag Mean|Var [uT|uT^2]: %lu, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %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.mag_mean[0], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
+ CAL_DEBUG_LOG(
+ debug_tag,
+ "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_6DIGITS_TRIPLET,
+ (unsigned long int)gyro_cal->debug_calibration_count,
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[0], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[1], 6),
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
} else {
CAL_DEBUG_LOG(debug_tag,
"Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
@@ -1104,73 +1110,12 @@
}
break;
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
- case ACCEL_STATS_TUNING:
- CAL_DEBUG_LOG(
- debug_tag,
- "Accel Mean|Var [m/sec^2|(m/sec^2)^2]: %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_x, 3),
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_y, 3),
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.prev_mean_z, 3),
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 6),
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 6),
- CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 6));
- break;
-
- case GYRO_STATS_TUNING:
- CAL_DEBUG_LOG(
- debug_tag,
- "Gyro Mean|Var [mDPS|mDPS^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_x *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_y *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.prev_mean_z *
- RAD_TO_MILLI_DEGREES,
- 3),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 6),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 6),
- CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z *
- RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 6));
- break;
-
- case MAG_STATS_TUNING:
- if (gyro_cal->using_mag_sensor) {
- CAL_DEBUG_LOG(
- debug_tag,
- "Mag Mean|Var [uT|uT^2]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%06d, %s%d.%06d, %s%d.%06d",
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_x, 3),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_y, 3),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.prev_mean_z, 3),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6),
- CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6));
- } else {
- CAL_DEBUG_LOG(GYROCAL_TUNE_TAG,
- "Mag Mean|Var [uT|uT^2]: 0, 0, 0, -1.0, -1.0, -1.0");
- }
- break;
-#endif // GYRO_CAL_DBG_TUNE_ENABLED
-
default:
break;
}
}
void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) {
- 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 (gyro_cal->debug_state) {
case GYRO_IDLE:
@@ -1188,144 +1133,70 @@
case GYRO_WAIT_STATE:
// This helps throttle the print statements.
- if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
- gyro_cal->debug_state = next_state;
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos,
+ gyro_cal->wait_timer_nanos,
+ GYROCAL_WAIT_TIME_NANOS)) {
+ gyro_cal->debug_state = gyro_cal->next_state;
}
break;
case GYRO_PRINT_OFFSET:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, OFFSET);
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = GYRO_PRINT_STILLNESS_DATA; // Sets the next state.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state = GYRO_PRINT_STILLNESS_DATA; // Sets the next state.
gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_STILLNESS_DATA:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, STILLNESS_DATA);
- 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.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state =
+ GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE; // Sets next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_SAMPLE_RATE_AND_TEMPERATURE:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
SAMPLE_RATE_AND_TEMPERATURE);
- 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.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state =
+ GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN; // Sets next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_GYRO_MINMAX_STILLNESS_MEAN:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG,
GYRO_MINMAX_STILLNESS_MEAN);
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state = GYRO_PRINT_ACCEL_STATS; // Sets the next state.
gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_ACCEL_STATS:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, ACCEL_STATS);
- 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.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_GYRO_STATS:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, GYRO_STATS);
- 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.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state = GYRO_PRINT_MAG_STATS; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
case GYRO_PRINT_MAG_STATS:
gyroCalDebugPrintData(gyro_cal, GYROCAL_REPORT_TAG, MAG_STATS);
- 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.
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->next_state = GYRO_IDLE; // Sets the next state.
+ gyro_cal->debug_state = GYRO_WAIT_STATE; // First, go to wait state.
break;
default:
// Sends this state machine to its idle state.
- 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 (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);
- }
-#endif // GYRO_CAL_DBG_TUNE_ENABLED
-}
-
-#ifdef GYRO_CAL_DBG_TUNE_ENABLED
-void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal,
- uint64_t timestamp_nanos) {
- 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.
- // i. Within the first 300 seconds of boot: output interval = 5
- // seconds.
- // ii. Thereafter: output interval is 60 seconds.
- bool condition_i = ((timestamp_nanos <= 300000000000) &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, wait_timer_nanos, 5000000000));
- bool condition_ii = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, wait_timer_nanos, 60000000000);
-
- // This is a state machine that controls the reporting out of tuning data.
- switch (debug_state) {
- case GYRO_IDLE:
- // Wait for a trigger and start the data tuning printout sequence.
- if (condition_i || condition_ii) {
- CAL_DEBUG_LOG(GYROCAL_TUNE_TAG, "Temp [C]: %s%d.%03d",
- CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3));
- 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.
- } else {
- debug_state = GYRO_IDLE;
- }
- break;
-
- case GYRO_WAIT_STATE:
- // This helps throttle the print statements.
- if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, wait_timer_nanos, GYROCAL_WAIT_TIME_NANOS)) {
- debug_state = next_state;
- }
- break;
-
- case GYRO_PRINT_ACCEL_STATS:
- gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, ACCEL_STATS_TUNING);
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = GYRO_PRINT_GYRO_STATS; // Sets the next state.
- debug_state = GYRO_WAIT_STATE; // First, go to wait state.
- break;
-
- case GYRO_PRINT_GYRO_STATS:
- gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, GYRO_STATS_TUNING);
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- next_state = GYRO_PRINT_MAG_STATS; // Sets the next state.
- debug_state = GYRO_WAIT_STATE; // First, go to wait state.
- break;
-
- case GYRO_PRINT_MAG_STATS:
- gyroCalDebugPrintData(gyro_cal, GYROCAL_TUNE_TAG, MAG_STATS_TUNING);
- wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
- 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.
- debug_state = GYRO_IDLE;
+ gyro_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer.
+ gyro_cal->debug_state = GYRO_IDLE; // Go to idle state.
}
}
-#endif // GYRO_CAL_DBG_TUNE_ENABLED
#endif // GYRO_CAL_DBG_ENABLED
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index cd96676..5e7d5ee 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
* - Temperature [Celsius]
*
* #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance
* data to assist in tuning the GyroCal parameters.
*/
@@ -80,15 +79,30 @@
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_min_celsius;
+ float temperature_max_celsius;
float temperature_mean_celsius;
bool using_mag_sensor;
};
+
+// Data structure for sample rate estimation.
+struct SampleRateData {
+ uint64_t last_timestamp_nanos;
+ uint64_t time_delta_accumulator;
+ size_t num_samples;
+};
#endif // GYRO_CAL_DBG_ENABLED
+// Data structure for tracking min/max window mean during device stillness.
+struct MinMaxWindowMeanData {
+ float gyro_winmean_min[3];
+ float gyro_winmean_max[3];
+};
+
// Data structure for tracking temperature data during device stillness.
struct TemperatureMeanData {
- float temperature_min_max_celsius[2];
+ float temperature_min_celsius;
+ float temperature_max_celsius;
float latest_temperature_celsius;
float mean_accumulator;
size_t num_points;
@@ -103,6 +117,9 @@
// Data for tracking temperature mean during periods of device stillness.
struct TemperatureMeanData temperature_mean_tracker;
+ // Data for tracking gyro mean during periods of device stillness.
+ struct MinMaxWindowMeanData window_mean_tracker;
+
// Aggregated sensor stillness threshold required for gyro bias calibration.
float stillness_threshold;
@@ -166,6 +183,11 @@
// Debug info.
struct DebugGyroCal debug_gyro_cal; // Debug data structure.
enum GyroCalDebugState debug_state; // Debug printout state machine.
+ enum GyroCalDebugState next_state; // Debug state machine next state.
+ uint64_t wait_timer_nanos; // Debug message throttle timer.
+
+ struct SampleRateData sample_rate_estimator; // Debug sample rate estimator.
+
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.
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 254dd7b..d92f1da 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -22,7 +22,7 @@
#include <string.h>
#include "calibration/util/cal_log.h"
-#include "common/math/vec.h"
+#include "common/math/macros.h"
#include "util/nano_assert.h"
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,9 +32,17 @@
// Defines the default weighting function for the linear model fit routine.
// Weighting = 10.0; for offsets newer than 5 minutes.
-#define OTC_WEIGHT_DEFINITION_0 0, 300000000000, 10.0f
+static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+ .offset_age_nanos = MIN_TO_NANOS(5),
+ .weight = 10.0f,
+};
+
// Weighting = 0.1; for offsets newer than 15 minutes.
-#define OTC_WEIGHT_DEFINITION_1 1, 900000000000, 0.1f
+static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+ .offset_age_nanos = MIN_TO_NANOS(15),
+ .weight = 0.1f,
+};
+
// The default weighting used for all older offsets.
#define OTC_MIN_WEIGHT_VALUE (0.04f)
@@ -42,11 +50,11 @@
// A debug version label to help with tracking results.
#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
-// The time value used to throttle debug messaging (100msec).
-#define OTC_WAIT_TIME_NANOS (100000000)
+// The time interval used to throttle debug messaging (100msec).
+#define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
-// The time value used to throttle temperture print messaging (1 second).
-#define OTC_PRINT_TEMP_NANOS (1000000000)
+// The time interval used to throttle temperture print messaging (1 second).
+#define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
static const char kDebugAxisLabel[3] = "XYZ";
@@ -166,80 +174,25 @@
size_t axis_index, float temperature_celsius);
// Sets the OTC model parameters to an "initialized" state.
-static void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
- ASSERT_NOT_NULL(over_temp_cal);
-
- // Sets the temperature sensitivity model parameters to
- // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
- // state.
- over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
- over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
- over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
- memset(over_temp_cal->sensor_intercept, 0, 3 * sizeof(float));
-}
+static void resetOtcLinearModel(struct OverTempCal *over_temp_cal);
// Checks that the input temperature value is within the valid range. If outside
// of range, then 'temperature_celsius' is coerced to within the limits.
-static bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
- if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
- *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
- return false;
- }
- if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
- *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
- return false;
- }
- return true;
-}
+static bool checkAndEnforceTemperatureRange(float *temperature_celsius);
// Returns "true" if the candidate linear model parameters are within the valid
// range, and not all zeros.
static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
- float temp_sensitivity, float sensor_intercept) {
- ASSERT_NOT_NULL(over_temp_cal);
-
- return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
- NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
- NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
- NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
-}
+ float temp_sensitivity, float sensor_intercept);
// Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
-static bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
- ASSERT_NOT_NULL(offset);
-
- // Simple check to ensure that:
- // 1. All of the input data is non "zero".
- // 2. The offset temperature is within the valid range.
- if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
- NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
- NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
- NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
- return false;
- }
-
- // Only returns the "check" result. Don't care about coercion.
- return checkAndEnforceTemperatureRange(&offset_temp_celsius);
-}
+static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
// Returns the least-squares weight based on the age of a particular offset
// estimate.
static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
uint64_t offset_timestamp_nanos,
- uint64_t current_timestamp_nanos) {
- ASSERT_NOT_NULL(over_temp_cal);
- size_t i;
- for (i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
- if (current_timestamp_nanos <=
- offset_timestamp_nanos +
- over_temp_cal->weighting_function[i].offset_age_nanos) {
- return over_temp_cal->weighting_function[i].weight;
- }
- }
-
- // Returning the default weight for all older offsets.
- return OTC_MIN_WEIGHT_VALUE;
-}
+ uint64_t current_timestamp_nanos);
// Updates 'compensated_offset' using the linear OTC model.
static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -292,14 +245,7 @@
// new_debug_tag = "INIT]"
// Output: "[OVER_TEMP_CAL:INIT]"
static void createDebugTag(struct OverTempCal *over_temp_cal,
- const char *new_debug_tag) {
- over_temp_cal->otc_debug_tag[0] = '[';
- memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
- strlen(over_temp_cal->otc_sensor_tag));
- memcpy(
- over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
- new_debug_tag, strlen(new_debug_tag) + 1);
-}
+ const char *new_debug_tag);
#endif // OVERTEMPCAL_DBG_ENABLED
/////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
@@ -342,13 +288,13 @@
OTC_TEMP_INVALID_CELSIUS;
// Defines the default weighting function for the linear model fit routine.
- overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_0);
- overTempSetWeightingFunction(over_temp_cal, OTC_WEIGHT_DEFINITION_1);
+ overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
+ overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
#ifdef OVERTEMPCAL_DBG_ENABLED
// Sets the default sensor descriptors for debugging.
overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
- 1e3f * 180.0f / NANO_PI);
+ RAD_TO_MDEG);
createDebugTag(over_temp_cal, ":INIT]");
if (over_temp_cal->over_temp_enable) {
@@ -376,8 +322,7 @@
// Sets the model parameters if they are within the acceptable limits.
// Includes a check to reject input model parameters that may have been passed
// in as all zeros.
- size_t i;
- for (i = 0; i < 3; i++) {
+ for (size_t i = 0; i < 3; i++) {
if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
sensor_intercept[i])) {
over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -394,7 +339,8 @@
// Checks that the new offset data is valid.
if (isValidOtcOffset(offset, offset_temp_celsius)) {
// Sets the initial over-temp calibration estimate.
- memcpy(over_temp_cal->model_data[0].offset, offset, 3 * sizeof(float));
+ memcpy(over_temp_cal->model_data[0].offset, offset,
+ sizeof(over_temp_cal->model_data[0].offset));
over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
over_temp_cal->num_model_pts = 1;
@@ -412,7 +358,8 @@
// If the new offset is valid, then it will be used as the current compensated
// offset, otherwise the current value will be kept.
if (isValidOtcOffset(offset, offset_temp_celsius)) {
- memcpy(over_temp_cal->compensated_offset.offset, offset, 3 * sizeof(float));
+ memcpy(over_temp_cal->compensated_offset.offset, offset,
+ sizeof(over_temp_cal->compensated_offset.offset));
over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
}
@@ -430,7 +377,8 @@
createDebugTag(over_temp_cal, ":SET MODEL]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temp [%s|C]: %s%d.%03d, %s%d.%03d, %s%d.%03d | %s%d.%03d",
+ "Offset|Temp [%s|C]: " CAL_FORMAT_3DIGITS_TRIPLET
+ " | " CAL_FORMAT_3DIGITS,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
@@ -439,8 +387,8 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Sensitivity|Intercept [%s/C|%s]: %s%d.%03d, %s%d.%03d, %s%d.%03d | "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d",
+ "Sensitivity|Intercept [%s/C|%s]: " CAL_FORMAT_3DIGITS_TRIPLET
+ " | " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(temp_sensitivity[0] * over_temp_cal->otc_unit_conversion,
3),
@@ -475,8 +423,10 @@
ASSERT_NOT_NULL(sensor_intercept);
// Gets the latest over-temp calibration model data.
- memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float));
- memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float));
+ memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity,
+ sizeof(over_temp_cal->temp_sensitivity));
+ memcpy(sensor_intercept, over_temp_cal->sensor_intercept,
+ sizeof(over_temp_cal->sensor_intercept));
*timestamp_nanos = over_temp_cal->last_model_update_nanos;
// Gets the latest temperature compensated offset estimate.
@@ -491,9 +441,8 @@
// Load only "good" data from the input 'model_data'.
over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE);
- size_t i;
size_t valid_data_count = 0;
- for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
if (isValidOtcOffset(model_data[i].offset,
model_data[i].offset_temp_celsius)) {
memcpy(&over_temp_cal->model_data[i], &model_data[i],
@@ -553,7 +502,7 @@
float *compensated_offset_temperature_celsius,
float *compensated_offset) {
memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
- 3 * sizeof(float));
+ sizeof(over_temp_cal->compensated_offset.offset));
*compensated_offset_temperature_celsius =
over_temp_cal->compensated_offset.offset_temp_celsius;
}
@@ -638,7 +587,7 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"Offset|Temperature|Time [%s|C|nsec]: "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+ CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
@@ -674,8 +623,7 @@
// Check condition:
// 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++) {
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; 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
@@ -688,7 +636,7 @@
// NOTE - The pointer to the new model data point is set here; the offset
// data is set below in the call to 'setLatestEstimate'.
- if (!replaced_one && over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
+ if (!replaced_one) {
if (over_temp_cal->num_model_pts < OTC_MODEL_SIZE) {
// 3) If nothing was replaced, and the 'model_data' buffer is not full
// then add the estimate data to the array.
@@ -699,7 +647,7 @@
// 4) Otherwise (nothing was replaced and buffer is full), replace the
// oldest data with the incoming one.
over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
- for (i = 1; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
if (over_temp_cal->latest_offset->timestamp_nanos <
over_temp_cal->model_data[i].timestamp_nanos) {
over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
@@ -757,7 +705,7 @@
// Prints out temperature and the current timestamp.
createDebugTag(over_temp_cal, ":TEMP]");
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Temperature|Time [C|nsec] = %s%d.%03d, %llu",
+ "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
CAL_ENCODE_FLOAT(temperature_celsius, 3),
(unsigned long long int)timestamp_nanos);
}
@@ -805,13 +753,11 @@
ASSERT_NOT_NULL(sensor_intercept);
ASSERT_NOT_NULL(max_error);
- size_t i;
- size_t j;
float max_error_test;
memset(max_error, 0, 3 * sizeof(float));
- for (i = 0; i < over_temp_cal->num_model_pts; i++) {
- for (j = 0; j < 3; j++) {
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t j = 0; j < 3; j++) {
max_error_test =
NANO_ABS(over_temp_cal->model_data[i].offset[j] -
(temp_sensitivity[j] *
@@ -824,16 +770,13 @@
}
}
-// TODO: Refactor to implement a compliance check on the storage of
+// TODO(davejacobs): Refactor to implement a compliance check on the storage of
// 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
- size_t index,
- uint64_t offset_age_nanos,
- float weight) {
+void overTempSetWeightingFunction(
+ struct OverTempCal *over_temp_cal, size_t index,
+ const struct OverTempCalWeightPt *new_otc_weight) {
if (index < OTC_NUM_WEIGHT_LEVELS) {
- over_temp_cal->weighting_function[index].offset_age_nanos =
- offset_age_nanos;
- over_temp_cal->weighting_function[index].weight = weight;
+ over_temp_cal->weighting_function[index] = *new_otc_weight;
}
}
@@ -847,10 +790,9 @@
// Defaults to using the current compensated offset value.
float compensated_offset[3];
memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
- 3 * sizeof(float));
+ sizeof(over_temp_cal->compensated_offset.offset));
- size_t index;
- for (index = 0; index < 3; index++) {
+ for (size_t index = 0; index < 3; index++) {
if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
// If a valid axis model is defined then the default compensation will
// use the linear model:
@@ -875,8 +817,7 @@
// Adds a delta term to the 'compensated_offset' using the temperature
// difference defined by 'delta_temp_celsius'.
- size_t index;
- for (index = 0; index < 3; index++) {
+ for (size_t index = 0; index < 3; index++) {
if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
// If a valid axis model is defined, then use the linear model to assist
// with computing an extrapolated compensation term.
@@ -894,7 +835,7 @@
// Uses the most recent offset estimate for offset compensation.
float compensated_offset[3];
- memcpy(compensated_offset, estimate->offset, 3 * sizeof(float));
+ memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
// Checks that the offset temperature is valid.
if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
@@ -922,12 +863,11 @@
// The default compensated offset is the nearest-temperature offset vector.
float compensated_offset[3];
memcpy(compensated_offset, over_temp_cal->nearest_offset->offset,
- 3 * sizeof(float));
+ sizeof(compensated_offset));
const float compensated_offset_temperature_celsius =
over_temp_cal->nearest_offset->offset_temp_celsius;
- size_t index;
- for (index = 0; index < 3; index++) {
+ for (size_t index = 0; index < 3; index++) {
if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) {
// If a valid axis model is defined, then use the linear model to assist
// with computing an extrapolated compensation term.
@@ -1099,9 +1039,8 @@
// If the 'compensated_offset' value has changed significantly, then set
// 'new_overtemp_offset_available' true.
- size_t i;
bool new_overtemp_offset_available = false;
- for (i = 0; i < 3; i++) {
+ for (size_t i = 0; i < 3; i++) {
if (NANO_ABS(over_temp_cal->compensated_offset.offset[i] -
compensated_offset[i]) >=
over_temp_cal->significant_offset_change) {
@@ -1115,7 +1054,7 @@
// vector and timestamp are updated.
if (new_overtemp_offset_available) {
memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
- 3 * sizeof(float));
+ sizeof(over_temp_cal->compensated_offset.offset));
over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
}
@@ -1128,7 +1067,8 @@
if (over_temp_cal->latest_offset) {
// Sets the latest over-temp calibration estimate.
- memcpy(over_temp_cal->latest_offset->offset, offset, 3 * sizeof(float));
+ memcpy(over_temp_cal->latest_offset->offset, offset,
+ sizeof(over_temp_cal->latest_offset->offset));
over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
}
@@ -1178,9 +1118,8 @@
// set. Otherwise, a lockout condition could occur where the entire model
// data set would need to be replaced in order to bring the model fit error
// below the error limit and allow a successful model update.
- size_t i;
bool updated_one = false;
- for (i = 0; i < 3; i++) {
+ for (size_t i = 0; i < 3; i++) {
if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i],
sensor_intercept[i])) {
over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i];
@@ -1191,7 +1130,8 @@
createDebugTag(over_temp_cal, ":REJECT]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "%c-Axis Parameters|Time [%s/C|%s|nsec]: %s%d.%03d, %s%d.%03d, %llu",
+ "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
+ ", " CAL_FORMAT_3DIGITS ", %llu",
kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
@@ -1227,11 +1167,10 @@
// Performs a brute force search for the estimate nearest
// 'temperature_celsius'.
- size_t i = 0;
float dtemp_new = 0.0f;
float dtemp_old = FLT_MAX;
over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
- for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius -
temperature_celsius);
if (dtemp_new < dtemp_old) {
@@ -1245,9 +1184,8 @@
uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
- size_t i;
bool removed_one = false;
- for (i = 0; i < over_temp_cal->num_model_pts; i++) {
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
timestamp_nanos > over_temp_cal->age_limit_nanos +
over_temp_cal->model_data[i].timestamp_nanos) {
@@ -1285,8 +1223,8 @@
createDebugTag(over_temp_cal, ":REMOVE]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temp|Time [%s|C|nsec]: %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %llu",
+ "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
over_temp_cal->otc_unit_conversion,
@@ -1304,8 +1242,7 @@
#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++) {
+ for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
sizeof(struct OverTempCalDataPt));
}
@@ -1328,8 +1265,7 @@
// complete (i.e., x, y, z values are all provided). Therefore, the jumpstart
// data produced here requires that the model parameters have all been fully
// defined and are all within the valid range.
- size_t i;
- for (i = 0; i < 3; i++) {
+ for (size_t i = 0; i < 3; i++) {
if (!isValidOtcLinearModel(over_temp_cal,
over_temp_cal->temp_sensitivity[i],
over_temp_cal->sensor_intercept[i])) {
@@ -1348,9 +1284,8 @@
float offset_temp_celsius =
(start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin;
- size_t j;
- for (i = 0; i < over_temp_cal->min_num_model_pts; i++) {
- for (j = 0; j < 3; j++) {
+ for (size_t i = 0; i < over_temp_cal->min_num_model_pts; i++) {
+ for (size_t j = 0; j < 3; j++) {
over_temp_cal->model_data[i].offset[j] =
over_temp_cal->temp_sensitivity[j] * offset_temp_celsius +
over_temp_cal->sensor_intercept[j];
@@ -1391,8 +1326,7 @@
// First pass computes the weighted mean values.
const size_t n = over_temp_cal->num_model_pts;
- size_t i = 0;
- for (i = 0; i < n; ++i) {
+ for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
timestamp_nanos);
@@ -1407,7 +1341,7 @@
// Second pass computes the mean corrected second moment values.
ASSERT(sw > 0.0f);
const float inv_sw = 1.0f / sw;
- for (i = 0; i < n; ++i) {
+ for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
timestamp_nanos);
@@ -1453,9 +1387,87 @@
return false;
}
+void resetOtcLinearModel(struct OverTempCal *over_temp_cal) {
+ ASSERT_NOT_NULL(over_temp_cal);
+
+ // Sets the temperature sensitivity model parameters to
+ // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial"
+ // state.
+ over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY;
+ over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY;
+ over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY;
+ memset(over_temp_cal->sensor_intercept, 0,
+ sizeof(over_temp_cal->sensor_intercept));
+}
+
+bool checkAndEnforceTemperatureRange(float *temperature_celsius) {
+ if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) {
+ *temperature_celsius = OTC_TEMP_MAX_CELSIUS;
+ return false;
+ }
+ if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) {
+ *temperature_celsius = OTC_TEMP_MIN_CELSIUS;
+ return false;
+ }
+ return true;
+}
+
+bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
+ float temp_sensitivity, float sensor_intercept) {
+ ASSERT_NOT_NULL(over_temp_cal);
+
+ return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit &&
+ NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit &&
+ NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL &&
+ NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL;
+}
+
+bool isValidOtcOffset(const float *offset, float offset_temp_celsius) {
+ ASSERT_NOT_NULL(offset);
+
+ // Simple check to ensure that:
+ // 1. All of the input data is non "zero".
+ // 2. The offset temperature is within the valid range.
+ if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+ NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+ NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL &&
+ NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) {
+ return false;
+ }
+
+ // Only returns the "check" result. Don't care about coercion.
+ return checkAndEnforceTemperatureRange(&offset_temp_celsius);
+}
+
+float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
+ uint64_t offset_timestamp_nanos,
+ uint64_t current_timestamp_nanos) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
+ if (current_timestamp_nanos <=
+ offset_timestamp_nanos +
+ over_temp_cal->weighting_function[i].offset_age_nanos) {
+ return over_temp_cal->weighting_function[i].weight;
+ }
+ }
+
+ // Returning the default weight for all older offsets.
+ return OTC_MIN_WEIGHT_VALUE;
+}
+
/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
#ifdef OVERTEMPCAL_DBG_ENABLED
+void createDebugTag(struct OverTempCal *over_temp_cal,
+ const char *new_debug_tag) {
+ over_temp_cal->otc_debug_tag[0] = '[';
+ memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag,
+ strlen(over_temp_cal->otc_sensor_tag));
+ memcpy(
+ over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1,
+ new_debug_tag, strlen(new_debug_tag) + 1);
+}
+
void updateDebugData(struct OverTempCal* over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -1473,8 +1485,7 @@
memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal));
// Copies over the relevant data.
- size_t i;
- for (i = 0; i < 3; i++) {
+ for (size_t i = 0; i < 3; i++) {
if (isValidOtcLinearModel(over_temp_cal, over_temp_cal->temp_sensitivity[i],
over_temp_cal->sensor_intercept[i])) {
over_temp_cal->debug_overtempcal.temp_sensitivity[i] =
@@ -1543,8 +1554,8 @@
// Prints out the latest offset estimate (input data).
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+ "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
over_temp_cal->otc_unit_tag,
(unsigned long int)over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
@@ -1576,7 +1587,7 @@
// Prints out the model parameters.
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|Sensitivity [%s/C]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+ "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
(unsigned long int)over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
@@ -1593,7 +1604,7 @@
3));
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Cal#|Intercept [%s]: %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+ "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
(unsigned long int)over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
@@ -1620,7 +1631,7 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
- "%lu, %lu, %s%d.%03d, %s%d.%03d, %s%d.%03d",
+ "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
(unsigned long int)over_temp_cal->debug_num_estimates,
(unsigned long int)over_temp_cal->debug_num_model_updates,
@@ -1647,8 +1658,8 @@
if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- " Model[%lu] [%s|C|nsec] = %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %llu",
+ " Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
(unsigned long int)over_temp_cal->model_counter,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
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 81b2173..8a404d3 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -130,17 +130,26 @@
#define OTC_NUM_WEIGHT_LEVELS (2)
// Rate-limits the check of old data to every 2 hours.
-#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)
+#define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
// Time duration in which to enforce using the last offset estimate for
// compensation (30 seconds).
-#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)
+#define OTC_USE_RECENT_OFFSET_TIME_NANOS (SEC_TO_NANOS(30))
// The age at which an offset estimate is considered stale (30 minutes).
-#define OTC_OFFSET_IS_STALE_NANOS (1800000000000)
+#define OTC_OFFSET_IS_STALE_NANOS (MIN_TO_NANOS(30))
// The refresh interval for the OTC model (30 seconds).
-#define OTC_REFRESH_MODEL_NANOS (30000000000)
+#define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
+
+// Defines a weighting function value for the linear model fit routine.
+struct OverTempCalWeightPt {
+ // The age limit below which an offset will use this weight value.
+ uint64_t offset_age_nanos;
+
+ // The weighting applied (>0).
+ float weight;
+};
// Over-temperature sensor offset estimate structure.
struct OverTempCalDataPt {
@@ -150,15 +159,6 @@
float offset[3];
};
-// Weighting data used to improve the quality of the linear model fit.
-struct OverTempCalWeightPt {
- // Offset age below which this weight applies.
- uint64_t offset_age_nanos;
-
- // Weighting value for offset estimates more recent than 'offset_age_nanos'.
- float weight;
-};
-
#ifdef OVERTEMPCAL_DBG_ENABLED
// Debug printout state enumeration.
enum OverTempCalDebugState {
@@ -532,14 +532,13 @@
* INPUTS:
* over_temp_cal: Over-temp data structure.
* index: Weighting function index.
- * offset_age_nanos: The age limit below which an offset will use this weight
- * value.
- * weight: The weighting applied (>0).
+ * new_otc_weight: Pointer to the settings for the new non-zero weighting
+ * value and corresponding age limit below which an offset
+ * will use the weight.
*/
-void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
- size_t index,
- uint64_t offset_age_nanos,
- float weight);
+void overTempSetWeightingFunction(
+ struct OverTempCal *over_temp_cal, size_t index,
+ const struct OverTempCalWeightPt *new_otc_weight);
#ifdef OVERTEMPCAL_DBG_ENABLED
// This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index f2e711f..1bd80fd 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -54,6 +54,13 @@
((x < 0) ? "-" : ""), \
(int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits)) // NOLINT
+// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
+// desired significant digits.
+#define CAL_FORMAT_3DIGITS "%s%d.%03d"
+#define CAL_FORMAT_6DIGITS "%s%d.%06d"
+#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+
#ifdef __cplusplus
}
#endif
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c
index 9c179ac..66e423f 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.c
+++ b/firmware/os/algos/common/math/levenberg_marquardt.c
@@ -4,6 +4,7 @@
#include <stdio.h>
#include <string.h>
+#include "common/math/macros.h"
#include "common/math/mat.h"
#include "common/math/vec.h"
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
new file mode 100644
index 0000000..5c06e24
--- /dev/null
+++ b/firmware/os/algos/common/math/macros.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This file contains helper macros and definitions.
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
+
+// Mathematical constants.
+#define NANO_PI (3.14159265359f)
+
+// Common math operations.
+#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
+#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
+#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+
+// Timestamp conversion macros.
+#ifdef __cplusplus
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#else
+#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000) // NOLINT
+#endif
+
+#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * 1000)
+#define MIN_TO_NANOS(x) SEC_TO_NANOS(x * 60)
+#define HRS_TO_NANOS(x) MIN_TO_NANOS(x * 60)
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+
+// Unit conversion: nanoseconds to seconds.
+#define NANOS_TO_SEC (1.0e-9f)
+
+// Unit conversion: milli-degrees to radians.
+#define MDEG_TO_RAD (NANO_PI / 180.0e3f)
+
+// Unit conversion: radians to milli-degrees.
+#define RAD_TO_MDEG (180.0e3f / NANO_PI)
+
+// Time check helper macro that returns true if:
+// i. 't1' is equal to or exceeds 't2' plus 't_delta'.
+// ii. Or, a negative timestamp delta occurred since,
+// 't1' should always >= 't2'. This prevents potential lockout conditions
+// if the timer count 't1' rolls over or an erroneously large
+// timestamp is passed through.
+#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
+ (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c
index b55ea8e..bb9f929 100644
--- a/firmware/os/algos/common/math/vec.c
+++ b/firmware/os/algos/common/math/vec.c
@@ -15,6 +15,7 @@
*/
#include "common/math/vec.h"
+#include "common/math/macros.h"
void findOrthogonalVector(float inX, float inY, float inZ, float *outX,
float *outY, float *outZ) {
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index 49f197c..0a4c8b3 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -53,23 +53,6 @@
float x, y, z, w;
};
-#define NANO_PI (3.14159265359f)
-
-#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
-
-#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
-
-#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
-
-// Time check helper macro that returns true if:
-// i. 't1' is equal to or exceeds 't2' plus 't_delta'.
-// ii. Or, a negative timestamp delta occurred since,
-// 't1' should always >= 't2'. This prevents potential lockout conditions
-// if the timer count 't1' rolls over or an erroneously large
-// timestamp is passed through.
-#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
- (((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
-
// 3-DIMENSIONAL VECTOR MATH ///////////////////////////////////////////
static inline void initVec3(struct Vec3 *v, float x, float y, float z) {
ASSERT_NOT_NULL(v);
