diff options
author | android-build-team Robot <android-build-team-robot@google.com> | 2017-07-27 07:25:34 +0000 |
---|---|---|
committer | android-build-team Robot <android-build-team-robot@google.com> | 2017-07-27 07:25:34 +0000 |
commit | 1760b5cbaf1856b91d5e1d19b807d63195c81d49 (patch) | |
tree | 9fa5485c402389383f175aafea91503263a62bf1 | |
parent | f68dbee466704b527a1815503c21d2bf1cc17ba3 (diff) | |
parent | e3de39c0839415ecfaf124e6c68b1cf043e82439 (diff) | |
download | contexthub-1760b5cbaf1856b91d5e1d19b807d63195c81d49.tar.gz |
release-request-bb5b45da-eaa6-4d79-aca0-d3dca522258f-for-git_oc-dr1-release-4221981 snap-temp-L58600000086509073android-8.0.0_r34android-8.0.0_r33android-8.0.0_r27android-8.0.0_r26android-8.0.0_r25android-8.0.0_r24android-8.0.0_r23android-8.0.0_r22android-8.0.0_r21oreo-dr3-releaseoreo-dr2-releaseoreo-dr1-release
Change-Id: I0d78c5ae07af01630e0c17a62bbd91fed43ed475
-rw-r--r-- | firmware/os/algos/calibration/gyroscope/gyro_cal.c | 92 | ||||
-rw-r--r-- | firmware/os/algos/calibration/gyroscope/gyro_cal.h | 40 | ||||
-rw-r--r-- | firmware/os/algos/calibration/over_temp/over_temp_cal.c | 664 | ||||
-rw-r--r-- | firmware/os/algos/calibration/over_temp/over_temp_cal.h | 174 |
4 files changed, 697 insertions, 273 deletions
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c index 2dd4c9a4..d6a69f3e 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c @@ -579,7 +579,7 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { // 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 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { 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++; - - // Tracks the min and max temperature values. - if (temperature_min_max_celsius[0] > temperature_celsius) { - temperature_min_max_celsius[0] = temperature_celsius; + gyro_cal->temperature_mean_tracker.mean_accumulator += + temperature_celsius; + gyro_cal->temperature_mean_tracker.num_points++; + + // 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 @@ bool gyroTemperatureStatsTracker(struct GyroCal* gyro_cal, // 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 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 9cf06d2c..cd96676d 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 @@ struct DebugGyroCal { 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 GyroCal { 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 @@ struct GyroCal { 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 35e4d589..254dd7b0 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 @@ static bool jumpStartModelData(struct OverTempCal *over_temp_cal, /* * 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 @@ static void updateCalOffset(struct OverTempCal *over_temp_cal, * 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 @@ static bool isValidOtcOffset(const float *offset, float offset_temp_celsius) { 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 @@ static void createDebugTag(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalInit(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalInit(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, } } - // 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 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, } } - // 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 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, // 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 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, // 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 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, // 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 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, #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 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, #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 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, } // 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 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal, } } +// 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; + + 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; + } - // If 'temperature_celsius' is invalid, then do not update the compensated - // offset (keeps the current values). + // 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 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, 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); + + // --------------------------------------------------------------------------- + // The following boolean expressions help determine how OTC offset updates + // are computed below. - // 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 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)); - } else { - // Uses the current compensated offset value. - memcpy(compensated_offset, over_temp_cal->compensated_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. } - // 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 (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 { + 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 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal, 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 setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, } } +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 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, // Updates the linear model fit. float temp_sensitivity[3]; float sensor_intercept[3]; - updateModel(over_temp_cal, 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 + updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity, + sensor_intercept); - // 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 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, 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 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal, } 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 @@ void updateDebugData(struct OverTempCal* over_temp_cal) { 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { ASSERT_NOT_NULL(over_temp_cal); - static enum OverTempCalDebugState next_state = 0; - static uint64_t wait_timer = 0; - static size_t i = 0; // Counter. - 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, (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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 ff09a832..81b2173f 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. * - * Assumptions: + * 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 * - * 1) Sensor hysteresis is negligible. - * 2) Sensor offset temperature dependence is sufficiently "linear". + * Assumptions: + * 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 @@ extern "C" { // 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 @@ struct OverTempCalDataPt { 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 @@ enum OverTempCalDebugState { // 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 @@ struct OverTempCal { // 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 @@ struct OverTempCal { // 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 @@ struct OverTempCal { // 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 @@ struct OverTempCal { 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 @@ struct OverTempCal { #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 @@ struct OverTempCal { * 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 @@ struct OverTempCal { */ 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 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal, 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. |