release-request-bb5b45da-eaa6-4d79-aca0-d3dca522258f-for-git_oc-dr1-release-4221981 snap-temp-L58600000086509073
Change-Id: I0d78c5ae07af01630e0c17a62bbd91fed43ed475
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 2dd4c9a..d6a69f3 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -579,7 +579,7 @@
// Checks for the following watchdog timeout conditions:
// i. The current timestamp has exceeded the allowed watchdog duration.
// ii. A timestamp was received that has jumped backwards by more than the
- // allowed watchdog duration (e.g., timestamp clock roll-over).
+ // allowed watchdog duration (e.g., timestamp clock roll-over).
watchdog_timeout =
(sample_time_nanos > gyro_cal->gyro_watchdog_timeout_duration_nanos +
gyro_cal->gyro_watchdog_start_nanos) ||
@@ -652,34 +652,39 @@
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;
+ gyro_cal->temperature_mean_tracker.num_points = 0;
+ gyro_cal->temperature_mean_tracker.mean_accumulator = 0.0f;
// Initializes the min/max temperatures values.
- temperature_min_max_celsius[0] = FLT_MAX;
- temperature_min_max_celsius[1] = -FLT_MAX;
+ gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
+ FLT_MAX;
+ gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
+ -FLT_MAX;
break;
case DO_UPDATE_DATA:
// Does the mean accumulation.
- mean_accumulator += temperature_celsius;
- num_points++;
+ gyro_cal->temperature_mean_tracker.mean_accumulator +=
+ temperature_celsius;
+ gyro_cal->temperature_mean_tracker.num_points++;
- // Tracks the min and max temperature values.
- if (temperature_min_max_celsius[0] > temperature_celsius) {
- temperature_min_max_celsius[0] = temperature_celsius;
+ // 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] >
+ temperature_celsius) {
+ gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[0] =
+ temperature_celsius;
}
- if (temperature_min_max_celsius[1] < temperature_celsius) {
- temperature_min_max_celsius[1] = temperature_celsius;
+ if (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] <
+ temperature_celsius) {
+ gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] =
+ temperature_celsius;
}
break;
@@ -687,19 +692,36 @@
// Store the most recent temperature statistics data to the GyroCal data
// structure. This functionality allows previous results to be recalled
// when the device suddenly becomes "not still".
- if (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;
+ if (gyro_cal->temperature_mean_tracker.num_points > 0) {
+ gyro_cal->temperature_mean_celsius =
+ gyro_cal->temperature_mean_tracker.mean_accumulator /
+ gyro_cal->temperature_mean_tracker.num_points;
+ } else {
+ gyro_cal->temperature_mean_celsius =
+ gyro_cal->temperature_mean_tracker.latest_temperature_celsius;
+#ifdef GYRO_CAL_DBG_ENABLED
+ CAL_DEBUG_LOG("[GYRO_CAL:TEMP_GATE]",
+ "Insufficient statistics (num_points = 0), using latest "
+ "measured temperature as the mean value.");
+#endif // GYRO_CAL_DBG_ENABLED
}
+#ifdef GYRO_CAL_DBG_ENABLED
+ // Records the min/max and mean temperature values for debug purposes.
+ gyro_cal->debug_gyro_cal.temperature_mean_celsius =
+ gyro_cal->temperature_mean_celsius;
+ memcpy(gyro_cal->debug_gyro_cal.temperature_min_max_celsius,
+ gyro_cal->temperature_mean_tracker.temperature_min_max_celsius,
+ 2 * sizeof(float));
+#endif
break;
case DO_EVALUATE:
// Determines if the min/max delta exceeded the set limit.
- if (num_points > 0) {
+ if (gyro_cal->temperature_mean_tracker.num_points > 0) {
min_max_temp_exceeded =
- (temperature_min_max_celsius[1] -
- temperature_min_max_celsius[0]) >
+ (gyro_cal->temperature_mean_tracker.temperature_min_max_celsius[1] -
+ gyro_cal->temperature_mean_tracker
+ .temperature_min_max_celsius[0]) >
gyro_cal->temperature_delta_limit_celsius;
#ifdef GYRO_CAL_DBG_ENABLED
@@ -886,12 +908,6 @@
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));
@@ -1035,21 +1051,21 @@
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.%08d, %s%d.%08d, %s%d.%08d",
+ "%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], 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));
+ 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.%06d, %s%d.%06d, %s%d.%06d",
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d",
(unsigned long int)gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(
gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MILLI_DEGREES, 3),
@@ -1059,13 +1075,13 @@
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,
- 6),
+ 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[1] *
RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 6),
+ 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_var[2] *
RAD_TO_MILLI_DEGREES * RAD_TO_MILLI_DEGREES,
- 6));
+ 3));
break;
case MAG_STATS:
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 9cf06d2..cd96676 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -15,20 +15,16 @@
*/
/*
- * This module contains the algorithms for producing a
- * gyroscope offset calibration. The algorithm looks
- * for periods of stillness as indicated by accelerometer,
- * magnetometer and gyroscope, and computes a bias estimate
- * by taking the average of the gyroscope during the
- * stillness times.
+ * This module contains the algorithms for producing a gyroscope offset
+ * calibration. The algorithm looks for periods of stillness as indicated by
+ * accelerometer, magnetometer and gyroscope, and computes a bias estimate by
+ * taking the average of the gyroscope during the stillness times.
*
- * Currently, this algorithm is tuned such that the device
- * is only considered still when the device is on a
- * stationary surface (e.g., not on a person).
+ * Currently, this algorithm is tuned such that the device is only considered
+ * still when the device is on a stationary surface (e.g., not on a person).
*
- * NOTE - Time units are agnostic (i.e., determined by the
- * user's application and usage). However, typical time units
- * are nanoseconds.
+ * NOTE - Time units are agnostic (i.e., determined by the user's application
+ * and usage). However, typical time units are nanoseconds.
*
* Required Sensors and Units:
* - Gyroscope [rad/sec]
@@ -88,7 +84,15 @@
float temperature_mean_celsius;
bool using_mag_sensor;
};
-#endif
+#endif // GYRO_CAL_DBG_ENABLED
+
+// Data structure for tracking temperature data during device stillness.
+struct TemperatureMeanData {
+ float temperature_min_max_celsius[2];
+ float latest_temperature_celsius;
+ float mean_accumulator;
+ size_t num_points;
+};
struct GyroCal {
// Stillness detectors.
@@ -96,6 +100,9 @@
struct GyroStillDet mag_stillness_detect;
struct GyroStillDet gyro_stillness_detect;
+ // Data for tracking temperature mean during periods of device stillness.
+ struct TemperatureMeanData temperature_mean_tracker;
+
// Aggregated sensor stillness threshold required for gyro bias calibration.
float stillness_threshold;
@@ -147,10 +154,9 @@
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
+ // The mean temperature over the stillness period. The limit is used to check
+ // for temperature stability and provide a gate for when temperature is
+ // rapidly changing.
float temperature_mean_celsius;
float temperature_delta_limit_celsius;
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 35e4d58..254dd7b 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -27,23 +27,27 @@
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
-// Rate-limits the check of old data to every 2 hours.
-#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)
-
-// Time duration in which to enforce using the last offset estimate for
-// compensation (30 seconds).
-#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)
-
// Value used to check whether OTC model data is near zero.
#define OTC_MODELDATA_NEAR_ZERO_TOL (1e-7f)
+// 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
+// Weighting = 0.1; for offsets newer than 15 minutes.
+#define OTC_WEIGHT_DEFINITION_1 1, 900000000000, 0.1f
+// The default weighting used for all older offsets.
+#define OTC_MIN_WEIGHT_VALUE (0.04f)
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// 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.
+// The time value used to throttle debug messaging (100msec).
#define OTC_WAIT_TIME_NANOS (100000000)
+// The time value used to throttle temperture print messaging (1 second).
+#define OTC_PRINT_TEMP_NANOS (1000000000)
+
// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
static const char kDebugAxisLabel[3] = "XYZ";
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -99,32 +103,37 @@
/*
* Computes a new model fit and provides updated model parameters for the
- * over-temperature model data.
+ * over-temperature model data. Uses a simple weighting function determined from
+ * the age of the model data.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
+ * timestamp_nanos: Current timestamp for the model update.
* OUTPUTS:
* temp_sensitivity: Updated modeled temperature sensitivity (array).
* sensor_intercept: Updated model intercept (array).
*
* NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z.
*
- * Reference: "Comparing two ways to fit a line to data", John D. Cook.
- * http://www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/
+ * Reference: Press, William H. "15.2 Fitting Data to a Straight Line."
+ * Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992.
*/
static void updateModel(const struct OverTempCal *over_temp_cal,
- float *temp_sensitivity, float *sensor_intercept);
+ uint64_t timestamp_nanos, float *temp_sensitivity,
+ float *sensor_intercept);
/*
* Computes a new over-temperature compensated offset estimate based on the
- * temperature specified by, 'compensated_offset.offset_temp_celsius'.
+ * temperature specified by, 'temperature_celsius'.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
* timestamp_nanos: The current system timestamp.
+ * temperature_celsius: The sensor temperature to compensate the offset for.
*/
static void updateCalOffset(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos);
+ uint64_t timestamp_nanos,
+ float temperature_celsius);
/*
* Sets the new over-temperature compensated offset estimate vector and
@@ -134,10 +143,12 @@
* over_temp_cal: Over-temp data structure.
* compensated_offset: The new temperature compensated offset array.
* timestamp_nanos: The current system timestamp.
+ * temperature_celsius: The sensor temperature to compensate the offset for.
*/
static void setCompensatedOffset(struct OverTempCal *over_temp_cal,
const float *compensated_offset,
- uint64_t timestamp_nanos);
+ uint64_t timestamp_nanos,
+ float temperature_celsius);
/*
* Checks new offset estimates to determine if they could be an outlier that
@@ -211,6 +222,61 @@
return checkAndEnforceTemperatureRange(&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;
+}
+
+// Updates 'compensated_offset' using the linear OTC model.
+static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float temperature_celsius);
+
+// Adds a linear extrapolated term to 'compensated_offset' (3-element array)
+// based on the linear OTC model and 'delta_temp_celsius' (the difference
+// between the current sensor temperature and the offset temperature associated
+// with 'compensated_offset').
+static void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
+ float *compensated_offset,
+ float delta_temp_celsius);
+
+// Provides an over-temperature compensated offset based on the 'estimate'.
+static void compensateWithEstimate(
+ struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
+ struct OverTempCalDataPt *estimate, float temperature_celsius);
+
+// Evaluates the nearest-temperature compensation (with linear extrapolation
+// term due to temperature), and compares it with the compensation due to
+// just the linear model when 'compare_with_linear_model' is true, otherwise
+// the comparison will be made with an extrapolated version of the current
+// compensation value. The comparison tests whether the nearest-temperature
+// estimate deviates from the linear-model (or current-compensated) value by
+// more than 'jump_tolerance'. If a "jump" is detected, then it keeps the
+// linear-model (or current-compensated) value.
+static void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float temperature_celsius,
+ bool compare_to_linear_model);
+
+// Refreshes the OTC model to ensure that the most relevant model weighting is
+// being used.
+static void refreshOtcModel(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos);
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// This helper function stores all of the debug tracking information necessary
// for printing log messages.
@@ -240,8 +306,8 @@
void overTempCalInit(struct OverTempCal *over_temp_cal,
size_t min_num_model_pts,
- uint64_t min_update_interval_nanos,
- float delta_temp_per_bin, float max_error_limit,
+ uint64_t min_temp_update_period_nanos,
+ float delta_temp_per_bin, float jump_tolerance,
float outlier_limit, uint64_t age_limit_nanos,
float temp_sensitivity_limit, float sensor_intercept_limit,
float significant_offset_change, bool over_temp_enable) {
@@ -261,9 +327,9 @@
over_temp_cal->new_overtemp_model_available = false;
over_temp_cal->new_overtemp_offset_available = false;
over_temp_cal->min_num_model_pts = min_num_model_pts;
- over_temp_cal->min_update_interval_nanos = min_update_interval_nanos;
+ over_temp_cal->min_temp_update_period_nanos = min_temp_update_period_nanos;
over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
- over_temp_cal->max_error_limit = max_error_limit;
+ over_temp_cal->jump_tolerance = jump_tolerance;
over_temp_cal->outlier_limit = outlier_limit;
over_temp_cal->age_limit_nanos = age_limit_nanos;
over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
@@ -275,6 +341,10 @@
over_temp_cal->compensated_offset.offset_temp_celsius =
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);
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// Sets the default sensor descriptors for debugging.
overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
@@ -315,9 +385,6 @@
}
}
- // Sets the model update time to the current timestamp.
- over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos;
-
// Model "Jump-Start".
const bool model_jump_started =
(jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos)
@@ -342,7 +409,7 @@
}
}
- // If the new offset is valid, then use this as the current compensated
+ // 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));
@@ -354,6 +421,10 @@
// track yet.
over_temp_cal->latest_offset = NULL;
+ // Sets the model and offset update times to the current timestamp.
+ over_temp_cal->last_offset_update_nanos = timestamp_nanos;
+ over_temp_cal->last_model_update_nanos = timestamp_nanos;
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// Prints the recalled model data.
createDebugTag(over_temp_cal, ":SET MODEL]");
@@ -406,7 +477,7 @@
// 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));
- *timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos;
+ *timestamp_nanos = over_temp_cal->last_model_update_nanos;
// Gets the latest temperature compensated offset estimate.
overTempCalGetOffset(over_temp_cal, offset_temp_celsius, offset);
@@ -451,7 +522,8 @@
over_temp_cal->compensated_offset.offset_temp_celsius);
// Updates the current over-temperature compensated offset estimate.
- updateCalOffset(over_temp_cal, timestamp_nanos);
+ updateCalOffset(over_temp_cal, timestamp_nanos,
+ over_temp_cal->compensated_offset.offset_temp_celsius);
#ifdef OVERTEMPCAL_DBG_ENABLED
// Prints the updated model data.
@@ -548,6 +620,9 @@
return;
}
+ // Ensures that the most relevant model weighting is being used.
+ refreshOtcModel(over_temp_cal, timestamp_nanos);
+
// 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.
@@ -645,20 +720,15 @@
// 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,
+ // '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 &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, over_temp_cal->modelupdate_timestamp_nanos,
- over_temp_cal->min_update_interval_nanos)) {
+ // 2) ...shown in 'computeModelUpdate'.
+ if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) {
computeModelUpdate(over_temp_cal, timestamp_nanos);
}
// Updates the current over-temperature compensated offset estimate.
- updateCalOffset(over_temp_cal, timestamp_nanos);
+ updateCalOffset(over_temp_cal, timestamp_nanos, temperature_celsius);
#ifdef OVERTEMPCAL_DBG_ENABLED
// Updates the total number of received sensor offset estimates.
@@ -676,12 +746,13 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
#ifdef OVERTEMPCAL_DBG_LOG_TEMP
- static uint64_t wait_timer = 0;
- // Prints the sensor temperature trajectory for debugging purposes.
- // This throttles the print statements.
- if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, wait_timer,
- 1000000000)) {
- wait_timer = timestamp_nanos; // Starts the wait timer.
+ // Prints the sensor temperature trajectory for debugging purposes. This
+ // throttles the print statements (1Hz).
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->temperature_print_timer,
+ OTC_PRINT_TEMP_NANOS)) {
+ over_temp_cal->temperature_print_timer =
+ timestamp_nanos; // Starts the wait timer.
// Prints out temperature and the current timestamp.
createDebugTag(over_temp_cal, ":TEMP]");
@@ -693,15 +764,23 @@
#endif // OVERTEMPCAL_DBG_LOG_TEMP
#endif // OVERTEMPCAL_DBG_ENABLED
+ // This check throttles new OTC offset compensation updates so that high data
+ // rate temperature samples do not cause excessive computational burden. Note,
+ // temperature sensor updates are expected to potentially increase the data
+ // processing load, however, computational load from new offset estimates is
+ // not a concern as they are a typically provided at a very low rate (< 1 Hz).
+ if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->last_offset_update_nanos,
+ over_temp_cal->min_temp_update_period_nanos)) {
+ return; // Time interval too short, skip further data processing.
+ }
+
// Checks that the offset temperature is within a valid range, saturates if
// outside.
checkAndEnforceTemperatureRange(&temperature_celsius);
- // Updates the offset compensation temperature.
- over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
-
// Searches for the sensor offset estimate closest to the current temperature
- // when the temperature has changed by more than +/-10% of
+ // when the temperature has changed by more than +/-10% of the
// 'delta_temp_per_bin'.
if (over_temp_cal->num_model_pts > 0) {
if (NANO_ABS(over_temp_cal->last_temp_check_celsius - temperature_celsius) >
@@ -712,7 +791,10 @@
}
// Updates the current over-temperature compensated offset estimate.
- updateCalOffset(over_temp_cal, timestamp_nanos);
+ updateCalOffset(over_temp_cal, timestamp_nanos, temperature_celsius);
+
+ // Sets the OTC offset compensation time check.
+ over_temp_cal->last_offset_update_nanos = timestamp_nanos;
}
void overTempGetModelError(const struct OverTempCal *over_temp_cal,
@@ -742,26 +824,168 @@
}
}
+// TODO: 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) {
+ 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;
+ }
+}
+
/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
-void updateCalOffset(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos) {
+void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float temperature_celsius) {
+ ASSERT_NOT_NULL(over_temp_cal);
+
+ // Defaults to using the current compensated offset value.
+ float compensated_offset[3];
+ memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
+ 3 * sizeof(float));
+
+ size_t index;
+ for (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:
+ // compensated_offset = (temp_sensitivity * temperature +
+ // sensor_intercept)
+ compensated_offset[index] =
+ over_temp_cal->temp_sensitivity[index] * temperature_celsius +
+ over_temp_cal->sensor_intercept[index];
+ }
+ }
+
+ // Sets the offset compensation vector, temperature, and timestamp.
+ setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos,
+ temperature_celsius);
+}
+
+void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal,
+ float *compensated_offset,
+ float delta_temp_celsius) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(compensated_offset);
+
+ // 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++) {
+ 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.
+ compensated_offset[index] +=
+ over_temp_cal->temp_sensitivity[index] * delta_temp_celsius;
+ }
+ }
+}
+
+void compensateWithEstimate(
+ struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
+ struct OverTempCalDataPt *estimate, float temperature_celsius) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(estimate);
+
+ // Uses the most recent offset estimate for offset compensation.
+ float compensated_offset[3];
+ memcpy(compensated_offset, estimate->offset, 3 * sizeof(float));
+
+ // Checks that the offset temperature is valid.
+ if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
+ const float delta_temp_celsius =
+ temperature_celsius - estimate->offset_temp_celsius;
+
+ // Adds a delta term to the compensated offset using the temperature
+ // difference defined by 'delta_temp_celsius'.
+ addLinearTemperatureExtrapolation(over_temp_cal, compensated_offset,
+ delta_temp_celsius);
+ }
+
+ // Sets the offset compensation vector, temperature, and timestamp.
+ setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos,
+ temperature_celsius);
+}
+
+void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ float temperature_celsius,
+ bool compare_to_linear_model) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(over_temp_cal->nearest_offset);
- float temperature_celsius =
- over_temp_cal->compensated_offset.offset_temp_celsius;
+ // 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));
+ const float compensated_offset_temperature_celsius =
+ over_temp_cal->nearest_offset->offset_temp_celsius;
- // If 'temperature_celsius' is invalid, then do not update the compensated
- // offset (keeps the current values).
+ size_t index;
+ for (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.
+ float delta_temp_celsius =
+ temperature_celsius - compensated_offset_temperature_celsius;
+ compensated_offset[index] +=
+ over_temp_cal->temp_sensitivity[index] * delta_temp_celsius;
+
+ // Computes the test offset (based on the linear model or current offset).
+ float test_offset;
+ if (compare_to_linear_model) {
+ test_offset =
+ over_temp_cal->temp_sensitivity[index] * temperature_celsius +
+ over_temp_cal->sensor_intercept[index];
+ } else {
+ // Adds a delta term to the compensated offset using the temperature
+ // difference defined by 'delta_temp_celsius'.
+ if (over_temp_cal->compensated_offset.offset_temp_celsius <=
+ OTC_TEMP_INVALID_CELSIUS) {
+ // If temperature is invalid, then skip further processing.
+ break;
+ }
+ delta_temp_celsius =
+ temperature_celsius -
+ over_temp_cal->compensated_offset.offset_temp_celsius;
+ test_offset =
+ over_temp_cal->compensated_offset.offset[index] +
+ over_temp_cal->temp_sensitivity[index] * delta_temp_celsius;
+ }
+
+ // Checks for "jumps" in the candidate compensated offset. If detected,
+ // then 'test_offset' is used for the offset update.
+ if (NANO_ABS(test_offset - compensated_offset[index]) >=
+ over_temp_cal->jump_tolerance) {
+ compensated_offset[index] = test_offset;
+ }
+ }
+ }
+
+ // Sets the offset compensation vector, temperature, and timestamp.
+ setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos,
+ temperature_celsius);
+}
+
+void updateCalOffset(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos, float temperature_celsius) {
+ ASSERT_NOT_NULL(over_temp_cal);
+
+ // If 'temperature_celsius' is invalid, then no changes to the compensated
+ // offset are computed.
if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
return;
}
- // Removes very old data from the collected model estimates (i.e., 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
- // very old [something is likely better than nothing]).
+ // Removes very old data from the collected model estimates (i.e.,
+ // 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 very old [something is likely better than nothing]).
if ((timestamp_nanos >=
OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
over_temp_cal->num_model_pts > 1) {
@@ -769,69 +993,107 @@
removeStaleModelData(over_temp_cal, timestamp_nanos);
}
- // If there are points in the model data set, then a 'nearest_offset' has been
- // defined.
- bool nearest_offset_defined = (over_temp_cal->num_model_pts > 0);
+ // Ensures that the most relevant model weighting is being used.
+ refreshOtcModel(over_temp_cal, timestamp_nanos);
- // Uses the most recent offset estimate for offset compensation if it is
- // defined and within a time delta specified by
- // OTC_USE_RECENT_OFFSET_TIME_NANOS.
- if (over_temp_cal->latest_offset && nearest_offset_defined &&
+ // ---------------------------------------------------------------------------
+ // The following boolean expressions help determine how OTC offset updates
+ // are computed below.
+
+ // The nearest-temperature offset estimate is valid if the model data set is
+ // not empty.
+ const bool model_points_available = (over_temp_cal->num_model_pts > 0);
+
+ // True when the latest offset estimate will be used to compute a sensor
+ // offset calibration estimate.
+ const bool use_latest_offset_compensation =
+ over_temp_cal->latest_offset && model_points_available &&
timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos +
- OTC_USE_RECENT_OFFSET_TIME_NANOS) {
- setCompensatedOffset(over_temp_cal, over_temp_cal->latest_offset->offset,
- timestamp_nanos);
- return; // Exits early.
+ OTC_USE_RECENT_OFFSET_TIME_NANOS;
+
+ // True when the conditions are met to use the nearest-temperature offset to
+ // compute a sensor offset calibration estimate.
+ // The nearest-temperature offset:
+ // i. Must be defined.
+ // ii. Offset temperature must be within a small neighborhood of the
+ // current measured temperature (+/- 'delta_temp_per_bin').
+ const bool can_compensate_with_nearest =
+ model_points_available && over_temp_cal->nearest_offset &&
+ NANO_ABS(temperature_celsius -
+ over_temp_cal->nearest_offset->offset_temp_celsius) <
+ over_temp_cal->delta_temp_per_bin;
+
+ // True if the last received sensor offset estimate is old or non-existent.
+ const bool latest_model_point_not_relevant =
+ (over_temp_cal->latest_offset == NULL) ||
+ (over_temp_cal->latest_offset &&
+ NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->latest_offset->timestamp_nanos,
+ OTC_OFFSET_IS_STALE_NANOS));
+
+ // True if the nearest-temperature offset estimate is old or non-existent.
+ const bool nearest_model_point_not_relevant =
+ (over_temp_cal->nearest_offset == NULL) ||
+ (over_temp_cal->nearest_offset &&
+ NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->nearest_offset->timestamp_nanos,
+ OTC_OFFSET_IS_STALE_NANOS));
+
+ // ---------------------------------------------------------------------------
+ // The following conditional expressions govern new OTC offset updates.
+
+ if (!model_points_available) {
+ // Computes the compensation using just the linear model if available,
+ // otherwise the current compensated offset vector will be kept.
+ compensateWithLinearModel(over_temp_cal, timestamp_nanos,
+ temperature_celsius);
+ return; // no further calculations, exit early.
}
- // Sets the default compensated offset vector.
- float compensated_offset[3];
- if (nearest_offset_defined) {
- // Uses the nearest-temperature estimate to perform the compensation.
- memcpy(compensated_offset, over_temp_cal->nearest_offset->offset,
- 3 * sizeof(float));
+ if (use_latest_offset_compensation) {
+ // Computes the compensation using the latest received offset estimate plus
+ // a term based on linear extrapolation from the offset temperature to the
+ // current measured temperature (if a linear model is defined).
+ compensateWithEstimate(over_temp_cal, timestamp_nanos,
+ over_temp_cal->latest_offset, temperature_celsius);
+ return; // no further calculations, exit early.
+ }
+
+ if (can_compensate_with_nearest) {
+ // Evaluates the nearest-temperature compensation (with a linear
+ // extrapolation term), and compares it with the compensation due to just
+ // the linear model, when 'compare_with_linear_model' is true. Otherwise,
+ // the comparison will be made with an extrapolated version of the current
+ // compensation value. The comparison determines whether the
+ // nearest-temperature estimate deviates from the linear-model (or
+ // current-compensated) value by more than 'jump_tolerance'. If a "jump" is
+ // detected, then it keeps the linear-model (or current-compensated) value.
+ const bool compare_with_linear_model = nearest_model_point_not_relevant;
+ compareAndCompensateWithNearest(over_temp_cal, timestamp_nanos,
+ temperature_celsius,
+ compare_with_linear_model);
} else {
- // Uses the current compensated offset value.
- memcpy(compensated_offset, over_temp_cal->compensated_offset.offset,
- 3 * sizeof(float));
- }
-
- // Provides per-axis checks to complete the offset compensation vector update.
- size_t index;
- for (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:
- // compensated_offset = (temp_sensitivity * temperature +
- // sensor_intercept)
- compensated_offset[index] =
- (over_temp_cal->temp_sensitivity[index] * temperature_celsius +
- over_temp_cal->sensor_intercept[index]);
-
- if (nearest_offset_defined &&
- NANO_ABS(temperature_celsius -
- over_temp_cal->nearest_offset->offset_temp_celsius) <
- over_temp_cal->delta_temp_per_bin &&
- NANO_ABS(compensated_offset[index] -
- over_temp_cal->nearest_offset->offset[index]) <
- over_temp_cal->max_error_limit) {
- // Uses the nearest-temperature estimate to perform the compensation if
- // the sensor's temperature is within a small neighborhood of the
- // 'nearest_offset', and the delta between the nearest-temperature
- // estimate and the model fit is less than 'max_error_limit'.
- compensated_offset[index] =
- over_temp_cal->nearest_offset->offset[index];
- }
+ if (latest_model_point_not_relevant) {
+ // If the nearest-temperature offset can't be used for compensation and
+ // the latest offset is stale (in this case, the overall model trend may
+ // be more useful for compensation than extending the most recent vector),
+ // then this resorts to using only the linear model (if defined).
+ compensateWithLinearModel(over_temp_cal, timestamp_nanos,
+ temperature_celsius);
+ } else {
+ // If the nearest-temperature offset can't be used for compensation and
+ // the latest offset is fairly recent, then the compensated offset is
+ // based on the linear extrapolation of the current compensation vector.
+ compensateWithEstimate(over_temp_cal, timestamp_nanos,
+ &over_temp_cal->compensated_offset,
+ temperature_celsius);
}
}
-
- // Finalizes the update to the offset compensation vector, and timestamp.
- setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos);
}
void setCompensatedOffset(struct OverTempCal *over_temp_cal,
const float *compensated_offset,
- uint64_t timestamp_nanos) {
+ uint64_t timestamp_nanos, float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(compensated_offset);
@@ -855,6 +1117,7 @@
memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
3 * sizeof(float));
over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
+ over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
}
}
@@ -871,6 +1134,24 @@
}
}
+void refreshOtcModel(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->last_model_update_nanos,
+ OTC_REFRESH_MODEL_NANOS)) {
+ // Checks the time since the last computed model and recalculates the model
+ // if necessary. This ensures that waking up after a long period of time
+ // allows the properly weighted OTC model to be used. As the estimates age,
+ // the weighting will become more uniform and the model will fit the whole
+ // set uniformly as a better approximation to the expected temperature
+ // sensitivity; Younger estimates will fit tighter to emphasize a more
+ // localized fit of the temp sensitivity function.
+ computeModelUpdate(over_temp_cal, timestamp_nanos);
+ over_temp_cal->last_model_update_nanos = timestamp_nanos;
+ }
+}
+
void computeModelUpdate(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
@@ -883,16 +1164,10 @@
// Updates the linear model fit.
float temp_sensitivity[3];
float sensor_intercept[3];
- updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);
+ updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity,
+ sensor_intercept);
-#ifdef OVERTEMPCAL_DBG_ENABLED
- // Computes the maximum error over all of the model data.
- float max_error[3];
- overTempGetModelError(over_temp_cal, temp_sensitivity, sensor_intercept,
- max_error);
-#endif // OVERTEMPCAL_DBG_ENABLED
-
- // 3) A new set of model parameters are accepted if:
+ // 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
// a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
// b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
@@ -916,31 +1191,26 @@
createDebugTag(over_temp_cal, ":REJECT]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "%c-Axis Parameters|Max Error|Time [%s/C|%s|%s|nsec]: "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %llu",
+ "%c-Axis Parameters|Time [%s/C|%s|nsec]: %s%d.%03d, %s%d.%03d, %llu",
kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
- over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag,
+ over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(
sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3),
- CAL_ENCODE_FLOAT(max_error[i] * over_temp_cal->otc_unit_conversion,
- 3),
(unsigned long long int)timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
}
}
- // If at least one of the axes updated, then consider this a valid model
- // update.
+ // If at least one axis 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;
+ // Resets the OTC model compensation update time and sets the update flag.
+ over_temp_cal->last_model_update_nanos = timestamp_nanos;
over_temp_cal->new_overtemp_model_available = true;
#ifdef OVERTEMPCAL_DBG_ENABLED
- // Updates the total number of model updates, the debug data package, and
- // triggers a log printout.
+ // Updates the total number of model updates.
over_temp_cal->debug_num_model_updates++;
#endif // OVERTEMPCAL_DBG_ENABLED
}
@@ -1105,47 +1375,61 @@
}
void updateModel(const struct OverTempCal *over_temp_cal,
- float *temp_sensitivity, float *sensor_intercept) {
+ uint64_t timestamp_nanos, float *temp_sensitivity,
+ float *sensor_intercept) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
ASSERT(over_temp_cal->num_model_pts > 0);
+ float sw = 0.0f;
float st = 0.0f, stt = 0.0f;
float sx = 0.0f, stsx = 0.0f;
float sy = 0.0f, stsy = 0.0f;
float sz = 0.0f, stsz = 0.0f;
+ float weight = 1.0f;
+
+ // First pass computes the weighted mean values.
const size_t n = over_temp_cal->num_model_pts;
size_t i = 0;
-
- // First pass computes the mean values.
for (i = 0; i < n; ++i) {
- 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];
+ weight = evaluateWeightingFunction(
+ over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
+ timestamp_nanos);
+
+ sw += weight;
+ st += over_temp_cal->model_data[i].offset_temp_celsius * weight;
+ sx += over_temp_cal->model_data[i].offset[0] * weight;
+ sy += over_temp_cal->model_data[i].offset[1] * weight;
+ sz += over_temp_cal->model_data[i].offset[2] * weight;
}
// Second pass computes the mean corrected second moment values.
- const float inv_n = 1.0f / n;
+ ASSERT(sw > 0.0f);
+ const float inv_sw = 1.0f / sw;
for (i = 0; i < n; ++i) {
+ weight = evaluateWeightingFunction(
+ over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
+ timestamp_nanos);
+
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];
- stsz += t * over_temp_cal->model_data[i].offset[2];
+ over_temp_cal->model_data[i].offset_temp_celsius -
+ st * inv_sw;
+ stt += weight * t * t;
+ stsx += t * over_temp_cal->model_data[i].offset[0] * weight;
+ stsy += t * over_temp_cal->model_data[i].offset[1] * weight;
+ stsz += t * over_temp_cal->model_data[i].offset[2] * weight;
}
// Calculates the linear model fit parameters.
- ASSERT(stt > 0);
+ ASSERT(stt > 0.0f);
const float inv_stt = 1.0f / stt;
temp_sensitivity[0] = stsx * inv_stt;
- sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_n;
+ sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_sw;
temp_sensitivity[1] = stsy * inv_stt;
- sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_n;
+ sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_sw;
temp_sensitivity[2] = stsz * inv_stt;
- sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n;
+ sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_sw;
}
bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset,
@@ -1216,9 +1500,8 @@
sizeof(struct OverTempCalDataPt));
}
+ // Total number of OTC model data points.
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;
// Computes the maximum error over all of the model data.
overTempGetModelError(over_temp_cal,
@@ -1231,12 +1514,8 @@
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.
- createDebugTag(over_temp_cal, ":REPORT]");
-
// This is a state machine that controls the reporting out of debug data.
+ createDebugTag(over_temp_cal, ":REPORT]");
switch (over_temp_cal->debug_state) {
case OTC_IDLE:
// Wait for a trigger and start the debug printout sequence.
@@ -1253,9 +1532,10 @@
case OTC_WAIT_STATE:
// This helps throttle the print statements.
- if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(timestamp_nanos, wait_timer,
- OTC_WAIT_TIME_NANOS)) {
- over_temp_cal->debug_state = next_state;
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->wait_timer_nanos,
+ OTC_WAIT_TIME_NANOS)) {
+ over_temp_cal->debug_state = over_temp_cal->next_state;
}
break;
@@ -1285,8 +1565,10 @@
(unsigned long long int)
over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = OTC_PRINT_MODEL_PARAMETERS; // Sets the next state.
+ over_temp_cal->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state =
+ OTC_PRINT_MODEL_PARAMETERS; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1327,8 +1609,9 @@
over_temp_cal->otc_unit_conversion,
3));
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state.
+ over_temp_cal->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1352,52 +1635,67 @@
over_temp_cal->otc_unit_conversion,
3));
- 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->model_counter = 0; // Resets the model data print counter.
+ over_temp_cal->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->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 OTC_PRINT_MODEL_DATA:
// Prints out all of the model data.
- if (i < over_temp_cal->num_model_pts) {
+ 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",
- (unsigned long int)i, over_temp_cal->otc_unit_tag,
- CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[0] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[1] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset[2] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp_celsius,
- 3),
- (unsigned long long int)over_temp_cal->model_data[i]
+ (unsigned long int)over_temp_cal->model_counter,
+ over_temp_cal->otc_unit_tag,
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->model_data[over_temp_cal->model_counter]
+ .offset[0] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->model_data[over_temp_cal->model_counter]
+ .offset[1] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->model_data[over_temp_cal->model_counter]
+ .offset[2] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->model_data[over_temp_cal->model_counter]
+ .offset_temp_celsius,
+ 3),
+ (unsigned long long int)over_temp_cal
+ ->model_data[over_temp_cal->model_counter]
.timestamp_nanos);
- i++;
- wait_timer = timestamp_nanos; // Starts the wait timer.
- next_state = OTC_PRINT_MODEL_DATA; // Sets the next state.
+ over_temp_cal->model_counter++;
+ over_temp_cal->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state =
+ OTC_PRINT_MODEL_DATA; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait state.
+ OTC_WAIT_STATE; // First, go to wait state.
} else {
// 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->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait 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 = OTC_IDLE; // Sets the next state.
+ over_temp_cal->wait_timer_nanos =
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
}
}
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 ff09a83..81b2173 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -15,31 +15,70 @@
*/
/*
- * This module provides an online algorithm for compensating a 3-axis sensor's
- * offset over its operating temperature:
+ * [OTC Sensor Offset Calibration]
+ * This module implements a runtime algorithm for provisioning over-temperature
+ * compensated (OTC) estimates of a 3-axis sensor's offset (i.e., bias):
*
* 1) Estimates of sensor offset with associated temperature are consumed,
* {offset, offset_temperature}.
- * 2) A temperature dependence model is extracted from the collected set of
- * data pairs.
- * 3) Until a "complete" model has been built and a model equation has been
- * computed, the compensation will use the collected offset nearest in
- * temperature. If a model is available, then the compensation will take
- * the form of:
*
- * Linear Compensation Model Equation:
+ * 2) A linear temperature dependence model is extracted from the collected
+ * set of data pairs.
+ *
+ * 3) The linear model is used for compensation when no other model points
+ * (e.g., nearest-temperature, or the latest received offset estimate) can
+ * be used as a better reference to construct the OTC offset.
+ *
+ * 4) The linear model is used as an extrapolator to provide better
+ * compensated offset estimates with rapid changes in temperature.
+ *
+ * 5) Other key features of this algorithm:
+ * a) Jump Detection - The model may contain old data having a variety of
+ * different thermal histories (hysteresis) which could produce
+ * discontinuities when using nearest-temperature compensation. If a
+ * "jump" is detected in comparison to the linear model (or current
+ * compensation vector, depending on the age of the model), then the
+ * discontinuity may be minimized by selecting the alternative.
+ *
+ * b) Outlier Detection - This checks new offset estimates against the
+ * available linear model. If deviations exceeed a specified limit,
+ * then the estimate is rejected.
+ *
+ * c) Model Data Pruning - Old model data that age beyond a specified
+ * limit is eventually removed from the data set.
+ *
+ * d) Model Parameter Limits - Bounds on the linear model parameters may
+ * be specified to qualify acceptable models.
+ *
+ * e) Offset Update Rate Limits - To minimize computational burden, a
+ * temporal limit is placed on offset updates prompted from an
+ * arbitrarily high temperature sampling rate; and a minimum offset
+ * change is applied to gate small variations in offset during stable
+ * periods.
+ *
+ * f) Model-Weighting Based on Age - The least-squares fit uses a
+ * weighting function based on the age of the model estimate data to
+ * favor recent estimates and emphasize localized OTC model fitting
+ * when new updates arrive.
+ *
+ * General Compensation Model Equation:
* sensor_out = sensor_in - compensated_offset
- * Where,
+ *
+ * When the linear model is used,
* compensated_offset = (temp_sensitivity * current_temp + sensor_intercept)
*
- * NOTE - 'current_temp' is the current measured temperature. 'temp_sensitivity'
- * is the modeled temperature sensitivity (i.e., linear slope).
- * 'sensor_intercept' is linear model intercept.
+ * NOTE - 'current_temp' is the current measured temperature.
+ * 'temp_sensitivity' is the modeled temperature sensitivity (i.e., linear
+ * slope). 'sensor_intercept' is linear model intercept.
+ *
+ * When the nearest-temperature or latest-offset is used as a "reference",
+ * delta_temp = current_temp - reference_offset_temperature
+ * extrapolation_term = temp_sensitivity * delta_temp
+ * compensated_offset = reference_offset + extrapolation_term
*
* Assumptions:
- *
- * 1) Sensor hysteresis is negligible.
- * 2) Sensor offset temperature dependence is sufficiently "linear".
+ * 1) Sensor offset temperature dependence is sufficiently "linear".
+ * 2) Impact of sensor hysteresis is small relative to thermal sensitivity.
* 3) The impact of long-term offset drift/aging compared to the magnitude of
* deviation resulting from the thermal sensitivity of the offset is
* relatively small.
@@ -86,6 +125,23 @@
// Invalid sensor temperature.
#define OTC_TEMP_INVALID_CELSIUS (-274.0f)
+// Number of time-interval levels used to define the least-squares weighting
+// function.
+#define OTC_NUM_WEIGHT_LEVELS (2)
+
+// Rate-limits the check of old data to every 2 hours.
+#define OTC_STALE_CHECK_TIME_NANOS (7200000000000)
+
+// Time duration in which to enforce using the last offset estimate for
+// compensation (30 seconds).
+#define OTC_USE_RECENT_OFFSET_TIME_NANOS (30000000000)
+
+// The age at which an offset estimate is considered stale (30 minutes).
+#define OTC_OFFSET_IS_STALE_NANOS (1800000000000)
+
+// The refresh interval for the OTC model (30 seconds).
+#define OTC_REFRESH_MODEL_NANOS (30000000000)
+
// Over-temperature sensor offset estimate structure.
struct OverTempCalDataPt {
// Sensor offset estimate, temperature, and timestamp.
@@ -94,6 +150,15 @@
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 {
@@ -107,8 +172,6 @@
// OverTempCal debug information/data tracking structure.
struct DebugOverTempCal {
- uint64_t modelupdate_timestamp_nanos;
-
// The latest received offset estimate data.
struct OverTempCalDataPt latest_offset;
@@ -128,6 +191,14 @@
// Storage for over-temperature model data.
struct OverTempCalDataPt model_data[OTC_MODEL_SIZE];
+ // Implements a weighting function to emphasize fitting a linear model to
+ // younger offset estimates.
+ struct OverTempCalWeightPt weighting_function[OTC_NUM_WEIGHT_LEVELS];
+
+ // The active over-temperature compensated offset estimate data. Contains the
+ // current sensor temperature at which offset compensation is performed.
+ struct OverTempCalDataPt compensated_offset;
+
// Timer used to limit the rate at which old estimates are removed from
// the 'model_data' collection.
uint64_t stale_data_timer; // [nanoseconds]
@@ -136,12 +207,15 @@
// with drift-compromised data.
uint64_t age_limit_nanos; // [nanoseconds]
- // Timestamp of the last model update.
- uint64_t modelupdate_timestamp_nanos; // [nanoseconds]
+ // Timestamp of the last OTC offset compensation update.
+ uint64_t last_offset_update_nanos; // [nanoseconds]
- // The active over-temperature compensated offset estimate data. Contains the
- // current sensor temperature at which offset compensation is performed.
- struct OverTempCalDataPt compensated_offset;
+ // Timestamp of the last OTC model update.
+ uint64_t last_model_update_nanos; // [nanoseconds]
+
+ // Limit on the minimum interval for offset update calculations resulting from
+ // an arbitrarily high temperature sampling rate.
+ uint64_t min_temp_update_period_nanos; // [nanoseconds]
///// Online Model Identification Parameters ////////////////////////////////
//
@@ -153,14 +227,10 @@
// 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
- // 3) A new set of model parameters are accepted if:
+ // 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
// a. ABS(temp_sensitivity) < temp_sensitivity_limit
// b. ABS(sensor_intercept) < sensor_intercept_limit
- uint64_t min_update_interval_nanos; // [nanoseconds]
float temp_sensitivity_limit; // [sensor units/Celsius]
float sensor_intercept_limit; // [sensor units]
size_t min_num_model_pts;
@@ -178,8 +248,10 @@
float sensor_intercept[3];
// A limit on the error between nearest-temperature estimate and the model fit
- // above which the model fit is preferred for providing offset compensation.
- float max_error_limit; // [sensor units]
+ // above which the model fit is preferred for providing offset compensation
+ // (also applies to checks between the nearest-temperature and the current
+ // compensated estimate).
+ float jump_tolerance; // [sensor units]
// A limit used to reject new offset estimates that deviate from the current
// model fit.
@@ -241,6 +313,14 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
struct DebugOverTempCal debug_overtempcal; // Debug data structure.
enum OverTempCalDebugState debug_state; // Debug printout state machine.
+ enum OverTempCalDebugState next_state; // Debug state machine next state.
+ uint64_t wait_timer_nanos; // Debug message throttle timer.
+
+#ifdef OVERTEMPCAL_DBG_LOG_TEMP
+ uint64_t temperature_print_timer;
+#endif // OVERTEMPCAL_DBG_LOG_TEMP
+
+ size_t model_counter; // Model output print counter.
float otc_unit_conversion; // Unit conversion for debug display.
char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS").
char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO").
@@ -260,10 +340,11 @@
* over_temp_cal: Over-temp main data structure.
* min_num_model_pts: Minimum number of model points per model
* calculation update.
- * min_update_interval_nanos: Minimum model update interval.
+ * min_temp_update_period_nanos: Limits the rate of offset updates due to an
+ * arbitrarily high temperature sampling rate.
* delta_temp_per_bin: Temperature span that defines the spacing of
* collected model estimates.
- * max_error_limit: Model acceptance fit error tolerance.
+ * jump_tolerance: Tolerance on acceptable jumps in offset updates.
* outlier_limit: Outlier offset estimate rejection tolerance.
* age_limit_nanos: Sets the age limit beyond which a offset
* estimate is removed from 'model_data'.
@@ -278,8 +359,8 @@
*/
void overTempCalInit(struct OverTempCal *over_temp_cal,
size_t min_num_model_pts,
- uint64_t min_update_interval_nanos,
- float delta_temp_per_bin, float max_error_limit,
+ uint64_t min_temp_update_period_nanos,
+ float delta_temp_per_bin, float jump_tolerance,
float outlier_limit, uint64_t age_limit_nanos,
float temp_sensitivity_limit, float sensor_intercept_limit,
float significant_offset_change, bool over_temp_enable);
@@ -437,6 +518,29 @@
const float *temp_sensitivity,
const float *sensor_intercept, float *max_error);
+/*
+ * Defines an element in the weighting function that is used to control the
+ * fitting behavior of the simple linear model regression used in this module.
+ * The total number of weighting levels that define this functionality is set by
+ * 'OTC_NUM_WEIGHT_LEVELS'. The weight values are expected to be greater than
+ * zero. A particular weight is assigned to a given offset estimate when it's
+ * age is less than 'offset_age_nanos'. NOTE: The ordering of the
+ * 'offset_age_nanos' values in the weight function array should be
+ * monotonically increasing from lowest index to highest so that weighting
+ * selection can be conveniently evaluated.
+ *
+ * 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).
+ */
+void overTempSetWeightingFunction(struct OverTempCal *over_temp_cal,
+ size_t index,
+ uint64_t offset_age_nanos,
+ float weight);
+
#ifdef OVERTEMPCAL_DBG_ENABLED
// This debug printout function assumes the input sensor data is a gyroscope
// [rad/sec]. 'timestamp_nanos' is the current system time.
