1 /* 2 * Copyright (C) 2017 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 #include "calibration/over_temp/over_temp_cal.h" 18 19 #include <float.h> 20 #include <inttypes.h> 21 #include <math.h> 22 #include <string.h> 23 24 #include "calibration/util/cal_log.h" 25 #include "util/nano_assert.h" 26 27 /////// DEFINITIONS AND MACROS //////////////////////////////////////////////// 28 29 // Value used to check whether OTC model data is near zero. 30 #define OTC_MODELDATA_NEAR_ZERO_TOL (1e-7f) 31 32 // Defines the default weighting function for the linear model fit routine. 33 // Weighting = 10.0; for offsets newer than 5 minutes. 34 static const struct OverTempCalWeight kOtcDefaultWeight0 = { 35 .offset_age_nanos = MIN_TO_NANOS(5), 36 .weight = 10.0f, 37 }; 38 39 // Weighting = 0.1; for offsets newer than 15 minutes. 40 static const struct OverTempCalWeight kOtcDefaultWeight1 = { 41 .offset_age_nanos = MIN_TO_NANOS(15), 42 .weight = 0.1f, 43 }; 44 45 // The default weighting used for all older offsets. 46 #define OTC_MIN_WEIGHT_VALUE (0.04f) 47 48 #ifdef OVERTEMPCAL_DBG_ENABLED 49 // A debug version label to help with tracking results. 50 #define OTC_DEBUG_VERSION_STRING "[Jan 10, 2018]" 51 52 // The time interval used to throttle debug messaging (100msec). 53 #define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1)) 54 55 // The time interval used to throttle temperature print messaging (1 second). 56 #define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1)) 57 58 // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z. 59 static const char kDebugAxisLabel[3] = "XYZ"; 60 #endif // OVERTEMPCAL_DBG_ENABLED 61 62 /////// FORWARD DECLARATIONS ////////////////////////////////////////////////// 63 64 // Updates the latest received model estimate data. 65 static void setLatestEstimate(struct OverTempCal *over_temp_cal, 66 const float *offset, float offset_temp_celsius); 67 68 /* 69 * Determines if a new over-temperature model fit should be performed, and then 70 * updates the model as needed. 71 * 72 * INPUTS: 73 * over_temp_cal: Over-temp data structure. 74 * timestamp_nanos: Current timestamp for the model update. 75 */ 76 static void computeModelUpdate(struct OverTempCal *over_temp_cal, 77 uint64_t timestamp_nanos); 78 79 /* 80 * Searches 'model_data' for the sensor offset estimate closest to the specified 81 * temperature. Sets the 'nearest_offset' pointer to the result. 82 */ 83 static void findNearestEstimate(struct OverTempCal *over_temp_cal, 84 float temperature_celsius); 85 86 /* 87 * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the 88 * drift-compromised data). 89 */ 90 static void removeStaleModelData(struct OverTempCal *over_temp_cal, 91 uint64_t timestamp_nanos); 92 93 /* 94 * Removes the offset estimates from 'model_data' at index, 'model_index'. 95 * Returns 'true' if data was removed. 96 */ 97 static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, 98 size_t model_index); 99 100 /* 101 * Since it may take a while for an empty model to build up enough data to start 102 * producing new model parameter updates, the model collection can be 103 * jump-started by using the new model parameters to insert "fake" data in place 104 * of actual sensor offset data. The new model data 'offset_age_nanos' is set to 105 * zero. 106 */ 107 static bool jumpStartModelData(struct OverTempCal *over_temp_cal); 108 109 /* 110 * Computes a new model fit and provides updated model parameters for the 111 * over-temperature model data. Uses a simple weighting function determined from 112 * the age of the model data. 113 * 114 * INPUTS: 115 * over_temp_cal: Over-temp data structure. 116 * OUTPUTS: 117 * temp_sensitivity: Updated modeled temperature sensitivity (array). 118 * sensor_intercept: Updated model intercept (array). 119 * 120 * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. 121 * 122 * Reference: Press, William H. "15.2 Fitting Data to a Straight Line." 123 * Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992. 124 */ 125 static void updateModel(const struct OverTempCal *over_temp_cal, 126 float *temp_sensitivity, float *sensor_intercept); 127 128 /* 129 * Computes a new over-temperature compensated offset estimate based on the 130 * temperature specified by, 'temperature_celsius'. 131 * 132 * INPUTS: 133 * over_temp_cal: Over-temp data structure. 134 * timestamp_nanos: The current system timestamp. 135 * temperature_celsius: The sensor temperature to compensate the offset for. 136 */ 137 static void updateCalOffset(struct OverTempCal *over_temp_cal, 138 uint64_t timestamp_nanos, 139 float temperature_celsius); 140 141 /* 142 * Sets the new over-temperature compensated offset estimate vector and 143 * timestamp. 144 * 145 * INPUTS: 146 * over_temp_cal: Over-temp data structure. 147 * compensated_offset: The new temperature compensated offset array. 148 * timestamp_nanos: The current system timestamp. 149 * temperature_celsius: The sensor temperature to compensate the offset for. 150 */ 151 static void setCompensatedOffset(struct OverTempCal *over_temp_cal, 152 const float *compensated_offset, 153 uint64_t timestamp_nanos, 154 float temperature_celsius); 155 156 /* 157 * Checks new offset estimates to determine if they could be an outlier that 158 * should be rejected. Operates on a per-axis basis determined by 'axis_index'. 159 * 160 * INPUTS: 161 * over_temp_cal: Over-temp data structure. 162 * offset: Offset array. 163 * axis_index: Index of the axis to check (0=x, 1=y, 2=z). 164 * 165 * Returns 'true' if the deviation of the offset value from the linear model 166 * exceeds 'outlier_limit'. 167 */ 168 static bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset, 169 size_t axis_index, float temperature_celsius); 170 171 // Sets the OTC model parameters to an "initialized" state. 172 static void resetOtcLinearModel(struct OverTempCal *over_temp_cal); 173 174 // Checks that the input temperature value is within the valid range. If outside 175 // of range, then 'temperature_celsius' is coerced to within the limits. 176 static bool checkAndEnforceTemperatureRange(float *temperature_celsius); 177 178 // Returns "true" if the candidate linear model parameters are within the valid 179 // range, and not all zeros. 180 static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal, 181 float temp_sensitivity, 182 float sensor_intercept); 183 184 // Returns "true" if 'offset' and 'offset_temp_celsius' is valid. 185 static bool isValidOtcOffset(const float *offset, float offset_temp_celsius); 186 187 // Returns the least-squares weight based on the age of a particular offset 188 // estimate. 189 static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, 190 uint64_t offset_age_nanos); 191 192 // Computes the age increment, adds it to the age of each OTC model data point, 193 // and resets the age update counter. 194 static void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal, 195 uint64_t timestamp_nanos); 196 197 // Updates 'compensated_offset' using the linear OTC model. 198 static void compensateWithLinearModel(struct OverTempCal *over_temp_cal, 199 uint64_t timestamp_nanos, 200 float temperature_celsius); 201 202 // Adds a linear extrapolated term to 'compensated_offset' (3-element array) 203 // based on the linear OTC model and 'delta_temp_celsius' (the difference 204 // between the current sensor temperature and the offset temperature associated 205 // with 'compensated_offset'). 206 static void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal, 207 float *compensated_offset, 208 float delta_temp_celsius); 209 210 // Provides an over-temperature compensated offset based on the 'estimate'. 211 static void compensateWithEstimate(struct OverTempCal *over_temp_cal, 212 uint64_t timestamp_nanos, 213 struct OverTempModelThreeAxis *estimate, 214 float temperature_celsius); 215 216 // Evaluates the nearest-temperature compensation (with linear extrapolation 217 // term due to temperature), and compares it with the compensation due to 218 // just the linear model when 'compare_with_linear_model' is true, otherwise 219 // the comparison will be made with an extrapolated version of the current 220 // compensation value. The comparison tests whether the nearest-temperature 221 // estimate deviates from the linear-model (or current-compensated) value by 222 // more than 'jump_tolerance'. If a "jump" is detected, then it keeps the 223 // linear-model (or current-compensated) value. 224 static void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal, 225 uint64_t timestamp_nanos, 226 float temperature_celsius, 227 bool compare_to_linear_model); 228 229 // Refreshes the OTC model to ensure that the most relevant model weighting is 230 // being used. 231 static void refreshOtcModel(struct OverTempCal *over_temp_cal, 232 uint64_t timestamp_nanos); 233 234 #ifdef OVERTEMPCAL_DBG_ENABLED 235 // This helper function stores all of the debug tracking information necessary 236 // for printing log messages. 237 static void updateDebugData(struct OverTempCal *over_temp_cal); 238 239 // Helper function that creates tag strings useful for identifying specific 240 // debug output data (embedded system friendly; not all systems have 'sprintf'). 241 // 'new_debug_tag' is any null-terminated string. Respect the total allowed 242 // length of the 'otc_debug_tag' string. 243 // Constructs: "[" + <otc_debug_tag> + <new_debug_tag> 244 // Example, 245 // otc_debug_tag = "OVER_TEMP_CAL" 246 // new_debug_tag = "INIT]" 247 // Output: "[OVER_TEMP_CAL:INIT]" 248 static void createDebugTag(struct OverTempCal *over_temp_cal, 249 const char *new_debug_tag); 250 #endif // OVERTEMPCAL_DBG_ENABLED 251 252 /////// FUNCTION DEFINITIONS ////////////////////////////////////////////////// 253 254 void overTempCalInit(struct OverTempCal *over_temp_cal, 255 const struct OverTempCalParameters *parameters) { 256 ASSERT_NOT_NULL(over_temp_cal); 257 258 // Clears OverTempCal memory. 259 memset(over_temp_cal, 0, sizeof(struct OverTempCal)); 260 261 // Initializes the pointers to important sensor offset estimates. 262 over_temp_cal->nearest_offset = &over_temp_cal->model_data[0]; 263 over_temp_cal->latest_offset = NULL; 264 265 // Initializes the OTC linear model parameters. 266 resetOtcLinearModel(over_temp_cal); 267 268 // Initializes the model identification parameters. 269 over_temp_cal->new_overtemp_model_available = false; 270 over_temp_cal->new_overtemp_offset_available = false; 271 over_temp_cal->min_num_model_pts = parameters->min_num_model_pts; 272 over_temp_cal->min_temp_update_period_nanos = 273 parameters->min_temp_update_period_nanos; 274 over_temp_cal->delta_temp_per_bin = parameters->delta_temp_per_bin; 275 over_temp_cal->jump_tolerance = parameters->jump_tolerance; 276 over_temp_cal->outlier_limit = parameters->outlier_limit; 277 over_temp_cal->age_limit_nanos = parameters->age_limit_nanos; 278 over_temp_cal->temp_sensitivity_limit = parameters->temp_sensitivity_limit; 279 over_temp_cal->sensor_intercept_limit = parameters->sensor_intercept_limit; 280 over_temp_cal->significant_offset_change = 281 parameters->significant_offset_change; 282 over_temp_cal->over_temp_enable = parameters->over_temp_enable; 283 284 // Initializes the over-temperature compensated offset temperature. 285 over_temp_cal->compensated_offset.offset_temp_celsius = 286 INVALID_TEMPERATURE_CELSIUS; 287 288 #ifdef OVERTEMPCAL_DBG_ENABLED 289 // Sets the default sensor descriptors for debugging. 290 overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS", 291 RAD_TO_MDEG); 292 293 createDebugTag(over_temp_cal, ":INIT]"); 294 if (over_temp_cal->over_temp_enable) { 295 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 296 "Over-temperature compensation ENABLED."); 297 } else { 298 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 299 "Over-temperature compensation DISABLED."); 300 } 301 #endif // OVERTEMPCAL_DBG_ENABLED 302 303 // Defines the default weighting function for the linear model fit routine. 304 overTempValidateAndSetWeight(over_temp_cal, 0, &kOtcDefaultWeight0); 305 overTempValidateAndSetWeight(over_temp_cal, 1, &kOtcDefaultWeight1); 306 } 307 308 void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, 309 float offset_temp_celsius, uint64_t timestamp_nanos, 310 const float *temp_sensitivity, 311 const float *sensor_intercept, bool jump_start_model) { 312 ASSERT_NOT_NULL(over_temp_cal); 313 ASSERT_NOT_NULL(offset); 314 ASSERT_NOT_NULL(temp_sensitivity); 315 ASSERT_NOT_NULL(sensor_intercept); 316 317 // Initializes the OTC linear model parameters. 318 resetOtcLinearModel(over_temp_cal); 319 320 // Sets the model parameters if they are within the acceptable limits. 321 // Includes a check to reject input model parameters that may have been passed 322 // in as all zeros. 323 for (size_t i = 0; i < 3; i++) { 324 if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i], 325 sensor_intercept[i])) { 326 over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; 327 over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; 328 } 329 } 330 331 // Model "Jump-Start". 332 const bool model_jump_started = 333 jump_start_model ? jumpStartModelData(over_temp_cal) : false; 334 335 if (!model_jump_started) { 336 // Checks that the new offset data is valid. 337 if (isValidOtcOffset(offset, offset_temp_celsius)) { 338 // Sets the initial over-temp calibration estimate. 339 memcpy(over_temp_cal->model_data[0].offset, offset, 340 sizeof(over_temp_cal->model_data[0].offset)); 341 over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius; 342 over_temp_cal->model_data[0].offset_age_nanos = 0; 343 over_temp_cal->num_model_pts = 1; 344 } else { 345 // No valid offset data to load. 346 over_temp_cal->num_model_pts = 0; 347 #ifdef OVERTEMPCAL_DBG_ENABLED 348 createDebugTag(over_temp_cal, ":RECALL]"); 349 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 350 "No valid sensor offset vector to load."); 351 #endif // OVERTEMPCAL_DBG_ENABLED 352 } 353 } 354 355 // If the new offset is valid, then it will be used as the current compensated 356 // offset, otherwise the current value will be kept. 357 if (isValidOtcOffset(offset, offset_temp_celsius)) { 358 memcpy(over_temp_cal->compensated_offset.offset, offset, 359 sizeof(over_temp_cal->compensated_offset.offset)); 360 over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius; 361 over_temp_cal->compensated_offset.offset_age_nanos = 0; 362 } 363 364 // Resets the latest offset pointer. There are no new offset estimates to 365 // track yet. 366 over_temp_cal->latest_offset = NULL; 367 368 // Sets the model and offset update times to the current timestamp. 369 over_temp_cal->last_offset_update_nanos = timestamp_nanos; 370 over_temp_cal->last_model_update_nanos = timestamp_nanos; 371 372 #ifdef OVERTEMPCAL_DBG_ENABLED 373 // Prints the recalled model data. 374 createDebugTag(over_temp_cal, ":SET MODEL]"); 375 CAL_DEBUG_LOG( 376 over_temp_cal->otc_debug_tag, 377 "Offset|Temp [%s|C]: " CAL_FORMAT_3DIGITS_TRIPLET 378 " | " CAL_FORMAT_3DIGITS, 379 over_temp_cal->otc_unit_tag, 380 CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3), 381 CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3), 382 CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3), 383 CAL_ENCODE_FLOAT(offset_temp_celsius, 3)); 384 385 CAL_DEBUG_LOG( 386 over_temp_cal->otc_debug_tag, 387 "Sensitivity|Intercept [%s/C|%s]: " CAL_FORMAT_3DIGITS_TRIPLET 388 " | " CAL_FORMAT_3DIGITS_TRIPLET, 389 over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag, 390 CAL_ENCODE_FLOAT(temp_sensitivity[0] * over_temp_cal->otc_unit_conversion, 391 3), 392 CAL_ENCODE_FLOAT(temp_sensitivity[1] * over_temp_cal->otc_unit_conversion, 393 3), 394 CAL_ENCODE_FLOAT(temp_sensitivity[2] * over_temp_cal->otc_unit_conversion, 395 3), 396 CAL_ENCODE_FLOAT(sensor_intercept[0] * over_temp_cal->otc_unit_conversion, 397 3), 398 CAL_ENCODE_FLOAT(sensor_intercept[1] * over_temp_cal->otc_unit_conversion, 399 3), 400 CAL_ENCODE_FLOAT(sensor_intercept[2] * over_temp_cal->otc_unit_conversion, 401 3)); 402 403 // Resets the debug print machine to ensure that updateDebugData() can 404 // produce a debug report and interupt any ongoing report. 405 over_temp_cal->debug_state = OTC_IDLE; 406 407 // Triggers a debug print out to view the new model parameters. 408 updateDebugData(over_temp_cal); 409 #endif // OVERTEMPCAL_DBG_ENABLED 410 } 411 412 void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, 413 float *offset_temp_celsius, uint64_t *timestamp_nanos, 414 float *temp_sensitivity, float *sensor_intercept) { 415 ASSERT_NOT_NULL(over_temp_cal); 416 ASSERT_NOT_NULL(offset); 417 ASSERT_NOT_NULL(offset_temp_celsius); 418 ASSERT_NOT_NULL(timestamp_nanos); 419 ASSERT_NOT_NULL(temp_sensitivity); 420 ASSERT_NOT_NULL(sensor_intercept); 421 422 // Gets the latest over-temp calibration model data. 423 memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 424 sizeof(over_temp_cal->temp_sensitivity)); 425 memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 426 sizeof(over_temp_cal->sensor_intercept)); 427 *timestamp_nanos = over_temp_cal->last_model_update_nanos; 428 429 // Gets the latest temperature compensated offset estimate. 430 overTempCalGetOffset(over_temp_cal, offset_temp_celsius, offset); 431 } 432 433 void overTempCalSetModelData(struct OverTempCal *over_temp_cal, 434 size_t data_length, uint64_t timestamp_nanos, 435 const struct OverTempModelThreeAxis *model_data) { 436 ASSERT_NOT_NULL(over_temp_cal); 437 ASSERT_NOT_NULL(model_data); 438 439 // Load only "good" data from the input 'model_data'. 440 over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE); 441 size_t valid_data_count = 0; 442 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 443 if (isValidOtcOffset(model_data[i].offset, 444 model_data[i].offset_temp_celsius)) { 445 memcpy(&over_temp_cal->model_data[i], &model_data[i], 446 sizeof(struct OverTempModelThreeAxis)); 447 valid_data_count++; 448 } 449 } 450 over_temp_cal->num_model_pts = valid_data_count; 451 452 // Initializes the OTC linear model parameters. 453 resetOtcLinearModel(over_temp_cal); 454 455 // Computes and replaces the model fit parameters. 456 computeModelUpdate(over_temp_cal, timestamp_nanos); 457 458 // Resets the latest offset pointer. There are no new offset estimates to 459 // track yet. 460 over_temp_cal->latest_offset = NULL; 461 462 // Searches for the sensor offset estimate closest to the current temperature. 463 findNearestEstimate(over_temp_cal, 464 over_temp_cal->compensated_offset.offset_temp_celsius); 465 466 // Updates the current over-temperature compensated offset estimate. 467 updateCalOffset(over_temp_cal, timestamp_nanos, 468 over_temp_cal->compensated_offset.offset_temp_celsius); 469 470 #ifdef OVERTEMPCAL_DBG_ENABLED 471 // Prints the updated model data. 472 createDebugTag(over_temp_cal, ":SET MODEL DATA SET]"); 473 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 474 "Over-temperature full model data set recalled."); 475 476 // Resets the debug print machine to ensure that a new debug report will 477 // interupt any ongoing report. 478 over_temp_cal->debug_state = OTC_IDLE; 479 480 // Triggers a log printout to show the updated sensor offset estimate. 481 updateDebugData(over_temp_cal); 482 #endif // OVERTEMPCAL_DBG_ENABLED 483 } 484 485 void overTempCalGetModelData(struct OverTempCal *over_temp_cal, 486 size_t *data_length, 487 struct OverTempModelThreeAxis *model_data) { 488 ASSERT_NOT_NULL(over_temp_cal); 489 *data_length = over_temp_cal->num_model_pts; 490 memcpy(model_data, over_temp_cal->model_data, 491 over_temp_cal->num_model_pts * sizeof(struct OverTempModelThreeAxis)); 492 } 493 494 void overTempCalGetOffset(struct OverTempCal *over_temp_cal, 495 float *compensated_offset_temperature_celsius, 496 float *compensated_offset) { 497 memcpy(compensated_offset, over_temp_cal->compensated_offset.offset, 498 sizeof(over_temp_cal->compensated_offset.offset)); 499 *compensated_offset_temperature_celsius = 500 over_temp_cal->compensated_offset.offset_temp_celsius; 501 } 502 503 void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, 504 uint64_t timestamp_nanos, float xi, float yi, 505 float zi, float *xo, float *yo, float *zo) { 506 ASSERT_NOT_NULL(over_temp_cal); 507 ASSERT_NOT_NULL(xo); 508 ASSERT_NOT_NULL(yo); 509 ASSERT_NOT_NULL(zo); 510 511 // Determines whether over-temp compensation will be applied. 512 if (over_temp_cal->over_temp_enable) { 513 // Removes the over-temperature compensated offset from the input sensor 514 // data. 515 *xo = xi - over_temp_cal->compensated_offset.offset[0]; 516 *yo = yi - over_temp_cal->compensated_offset.offset[1]; 517 *zo = zi - over_temp_cal->compensated_offset.offset[2]; 518 } else { 519 *xo = xi; 520 *yo = yi; 521 *zo = zi; 522 } 523 } 524 525 bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) { 526 ASSERT_NOT_NULL(over_temp_cal); 527 const bool update_available = over_temp_cal->new_overtemp_model_available && 528 over_temp_cal->over_temp_enable; 529 530 // The 'new_overtemp_model_available' flag is reset when it is read here. 531 over_temp_cal->new_overtemp_model_available = false; 532 533 return update_available; 534 } 535 536 bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal) { 537 ASSERT_NOT_NULL(over_temp_cal); 538 const bool update_available = over_temp_cal->new_overtemp_offset_available && 539 over_temp_cal->over_temp_enable; 540 541 // The 'new_overtemp_offset_available' flag is reset when it is read here. 542 over_temp_cal->new_overtemp_offset_available = false; 543 544 return update_available; 545 } 546 547 void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, 548 uint64_t timestamp_nanos, 549 const float *offset, 550 float temperature_celsius) { 551 ASSERT_NOT_NULL(over_temp_cal); 552 ASSERT_NOT_NULL(offset); 553 ASSERT(over_temp_cal->delta_temp_per_bin > 0); 554 555 // Updates the age of each OTC model data point. 556 modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos); 557 558 // Checks that the new offset data is valid, returns if bad. 559 if (!isValidOtcOffset(offset, temperature_celsius)) { 560 return; 561 } 562 563 // Prevent a divide by zero below. 564 if (over_temp_cal->delta_temp_per_bin <= 0) { 565 return; 566 } 567 568 // Ensures that the most relevant model weighting is being used. 569 refreshOtcModel(over_temp_cal, timestamp_nanos); 570 571 // Checks whether this offset estimate is a likely outlier. A limit is placed 572 // on 'num_outliers', the previous number of successive rejects, to prevent 573 // too many back-to-back rejections. 574 if (over_temp_cal->num_outliers < OTC_MAX_OUTLIER_COUNT) { 575 if (outlierCheck(over_temp_cal, offset, 0, temperature_celsius) || 576 outlierCheck(over_temp_cal, offset, 1, temperature_celsius) || 577 outlierCheck(over_temp_cal, offset, 2, temperature_celsius)) { 578 // Increments the count of rejected outliers. 579 over_temp_cal->num_outliers++; 580 581 #ifdef OVERTEMPCAL_DBG_ENABLED 582 createDebugTag(over_temp_cal, ":OUTLIER]"); 583 CAL_DEBUG_LOG( 584 over_temp_cal->otc_debug_tag, 585 "Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET 586 ", " CAL_FORMAT_3DIGITS ", %" PRIu64, 587 over_temp_cal->otc_unit_tag, 588 CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3), 589 CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3), 590 CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3), 591 CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos); 592 #endif // OVERTEMPCAL_DBG_ENABLED 593 594 return; // Outlier detected: skips adding this offset to the model. 595 } else { 596 // Resets the count of rejected outliers. 597 over_temp_cal->num_outliers = 0; 598 } 599 } else { 600 // Resets the count of rejected outliers. 601 over_temp_cal->num_outliers = 0; 602 } 603 604 // Computes the temperature bin range data. 605 const int32_t bin_num = 606 CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin); 607 const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin; 608 const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin; 609 610 // The rules for accepting new offset estimates into the 'model_data' 611 // collection: 612 // 1) The temperature domain is divided into bins each spanning 613 // 'delta_temp_per_bin'. 614 // 2) Find and replace the i'th 'model_data' estimate data if: 615 // Let, bin_num = floor(temperature_celsius / delta_temp_per_bin) 616 // temp_lo_check = bin_num * delta_temp_per_bin 617 // temp_hi_check = (bin_num + 1) * delta_temp_per_bin 618 // Check condition: 619 // temp_lo_check <= model_data[i].offset_temp_celsius < temp_hi_check 620 bool replaced_one = false; 621 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 622 if (over_temp_cal->model_data[i].offset_temp_celsius < temp_hi_check && 623 over_temp_cal->model_data[i].offset_temp_celsius >= temp_lo_check) { 624 // NOTE - The pointer to the new model data point is set here; the offset 625 // data is set below in the call to 'setLatestEstimate'. 626 over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; 627 replaced_one = true; 628 break; 629 } 630 } 631 632 // NOTE - The pointer to the new model data point is set here; the offset 633 // data is set below in the call to 'setLatestEstimate'. 634 if (!replaced_one) { 635 if (over_temp_cal->num_model_pts < OTC_MODEL_SIZE) { 636 // 3) If nothing was replaced, and the 'model_data' buffer is not full 637 // then add the estimate data to the array. 638 over_temp_cal->latest_offset = 639 &over_temp_cal->model_data[over_temp_cal->num_model_pts]; 640 over_temp_cal->num_model_pts++; 641 } else { 642 // 4) Otherwise (nothing was replaced and buffer is full), replace the 643 // oldest data with the incoming one. 644 over_temp_cal->latest_offset = &over_temp_cal->model_data[0]; 645 for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) { 646 if (over_temp_cal->latest_offset->offset_age_nanos < 647 over_temp_cal->model_data[i].offset_age_nanos) { 648 over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; 649 } 650 } 651 } 652 } 653 654 // Updates the latest model estimate data. 655 setLatestEstimate(over_temp_cal, offset, temperature_celsius); 656 657 // The latest offset estimate is the nearest temperature offset. 658 over_temp_cal->nearest_offset = over_temp_cal->latest_offset; 659 660 // The rules for determining whether a new model fit is computed are: 661 // 1) A minimum number of data points must have been collected: 662 // num_model_pts >= min_num_model_pts 663 // NOTE: Collecting 'num_model_pts' and given that only one point is 664 // kept per temperature bin (spanning a thermal range specified by 665 // 'delta_temp_per_bin') implies that model data covers at least, 666 // model_temperature_span >= 'num_model_pts' * delta_temp_per_bin 667 // 2) ...shown in 'computeModelUpdate'. 668 if (over_temp_cal->num_model_pts >= over_temp_cal->min_num_model_pts) { 669 computeModelUpdate(over_temp_cal, timestamp_nanos); 670 } 671 672 // Updates the current over-temperature compensated offset estimate. 673 updateCalOffset(over_temp_cal, timestamp_nanos, temperature_celsius); 674 675 #ifdef OVERTEMPCAL_DBG_ENABLED 676 // Updates the total number of received sensor offset estimates. 677 over_temp_cal->debug_num_estimates++; 678 679 // Triggers a log printout to show the updated sensor offset estimate. 680 updateDebugData(over_temp_cal); 681 #endif // OVERTEMPCAL_DBG_ENABLED 682 } 683 684 void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, 685 uint64_t timestamp_nanos, 686 float temperature_celsius) { 687 ASSERT_NOT_NULL(over_temp_cal); 688 689 #ifdef OVERTEMPCAL_DBG_ENABLED 690 #ifdef OVERTEMPCAL_DBG_LOG_TEMP 691 // Prints the sensor temperature trajectory for debugging purposes. This 692 // throttles the print statements (1Hz). 693 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 694 timestamp_nanos, over_temp_cal->temperature_print_timer, 695 OTC_PRINT_TEMP_NANOS)) { 696 over_temp_cal->temperature_print_timer = 697 timestamp_nanos; // Starts the wait timer. 698 699 // Prints out temperature and the current timestamp. 700 createDebugTag(over_temp_cal, ":TEMP]"); 701 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 702 "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS 703 ", %" PRIu64, 704 CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos); 705 } 706 #endif // OVERTEMPCAL_DBG_LOG_TEMP 707 #endif // OVERTEMPCAL_DBG_ENABLED 708 709 // Updates the age of each OTC model data point. 710 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 711 timestamp_nanos, over_temp_cal->last_age_update_nanos, 712 OTC_MODEL_AGE_UPDATE_NANOS)) { 713 modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos); 714 } 715 716 // This check throttles new OTC offset compensation updates so that high data 717 // rate temperature samples do not cause excessive computational burden. Note, 718 // temperature sensor updates are expected to potentially increase the data 719 // processing load, however, computational load from new offset estimates is 720 // not a concern as they are a typically provided at a very low rate (< 1 Hz). 721 if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 722 timestamp_nanos, over_temp_cal->last_offset_update_nanos, 723 over_temp_cal->min_temp_update_period_nanos)) { 724 return; // Time interval too short, skip further data processing. 725 } 726 727 // Checks that the offset temperature is within a valid range, saturates if 728 // outside. 729 checkAndEnforceTemperatureRange(&temperature_celsius); 730 731 // Searches for the sensor offset estimate closest to the current temperature 732 // when the temperature has changed by more than +/-10% of the 733 // 'delta_temp_per_bin'. 734 if (over_temp_cal->num_model_pts > 0) { 735 if (NANO_ABS(over_temp_cal->last_temp_check_celsius - temperature_celsius) > 736 0.1f * over_temp_cal->delta_temp_per_bin) { 737 findNearestEstimate(over_temp_cal, temperature_celsius); 738 over_temp_cal->last_temp_check_celsius = temperature_celsius; 739 } 740 } 741 742 // Updates the current over-temperature compensated offset estimate. 743 updateCalOffset(over_temp_cal, timestamp_nanos, temperature_celsius); 744 745 // Sets the OTC offset compensation time check. 746 over_temp_cal->last_offset_update_nanos = timestamp_nanos; 747 } 748 749 void overTempGetModelError(const struct OverTempCal *over_temp_cal, 750 const float *temp_sensitivity, 751 const float *sensor_intercept, float *max_error) { 752 ASSERT_NOT_NULL(over_temp_cal); 753 ASSERT_NOT_NULL(temp_sensitivity); 754 ASSERT_NOT_NULL(sensor_intercept); 755 ASSERT_NOT_NULL(max_error); 756 757 float max_error_test; 758 memset(max_error, 0, 3 * sizeof(float)); 759 760 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 761 for (size_t j = 0; j < 3; j++) { 762 max_error_test = 763 NANO_ABS(over_temp_cal->model_data[i].offset[j] - 764 (temp_sensitivity[j] * 765 over_temp_cal->model_data[i].offset_temp_celsius + 766 sensor_intercept[j])); 767 if (max_error_test > max_error[j]) { 768 max_error[j] = max_error_test; 769 } 770 } 771 } 772 } 773 774 bool overTempValidateAndSetWeight( 775 struct OverTempCal *over_temp_cal, size_t index, 776 const struct OverTempCalWeight *new_otc_weight) { 777 ASSERT_NOT_NULL(over_temp_cal); 778 ASSERT_NOT_NULL(new_otc_weight); 779 780 // The input weighting coefficient must be positive. 781 if (new_otc_weight->weight <= 0.0f) { 782 #ifdef OVERTEMPCAL_DBG_ENABLED 783 createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]"); 784 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Invalid weight: Must > 0."); 785 #endif // OVERTEMPCAL_DBG_ENABLED 786 return false; 787 } 788 789 // Ensures that the 'index-1' weight's age is younger. 790 if (index == 0 || 791 over_temp_cal->weighting_function[index - 1].offset_age_nanos < 792 new_otc_weight->offset_age_nanos) { 793 over_temp_cal->weighting_function[index] = *new_otc_weight; 794 return true; 795 } 796 797 #ifdef OVERTEMPCAL_DBG_ENABLED 798 createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]"); 799 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Non monotonic weight age."); 800 #endif // OVERTEMPCAL_DBG_ENABLED 801 return false; 802 } 803 804 /////// LOCAL HELPER FUNCTION DEFINITIONS ///////////////////////////////////// 805 806 void compensateWithLinearModel(struct OverTempCal *over_temp_cal, 807 uint64_t timestamp_nanos, 808 float temperature_celsius) { 809 ASSERT_NOT_NULL(over_temp_cal); 810 811 // Defaults to using the current compensated offset value. 812 float compensated_offset[3]; 813 memcpy(compensated_offset, over_temp_cal->compensated_offset.offset, 814 sizeof(over_temp_cal->compensated_offset.offset)); 815 816 for (size_t index = 0; index < 3; index++) { 817 if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) { 818 // If a valid axis model is defined then the default compensation will 819 // use the linear model: 820 // compensated_offset = (temp_sensitivity * temperature + 821 // sensor_intercept) 822 compensated_offset[index] = 823 over_temp_cal->temp_sensitivity[index] * temperature_celsius + 824 over_temp_cal->sensor_intercept[index]; 825 } 826 } 827 828 // Sets the offset compensation vector, temperature, and timestamp. 829 setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos, 830 temperature_celsius); 831 } 832 833 void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal, 834 float *compensated_offset, 835 float delta_temp_celsius) { 836 ASSERT_NOT_NULL(over_temp_cal); 837 ASSERT_NOT_NULL(compensated_offset); 838 839 // Adds a delta term to the 'compensated_offset' using the temperature 840 // difference defined by 'delta_temp_celsius'. 841 for (size_t index = 0; index < 3; index++) { 842 if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) { 843 // If a valid axis model is defined, then use the linear model to assist 844 // with computing an extrapolated compensation term. 845 compensated_offset[index] += 846 over_temp_cal->temp_sensitivity[index] * delta_temp_celsius; 847 } 848 } 849 } 850 851 void compensateWithEstimate(struct OverTempCal *over_temp_cal, 852 uint64_t timestamp_nanos, 853 struct OverTempModelThreeAxis *estimate, 854 float temperature_celsius) { 855 ASSERT_NOT_NULL(over_temp_cal); 856 ASSERT_NOT_NULL(estimate); 857 858 // Uses the most recent offset estimate for offset compensation. 859 float compensated_offset[3]; 860 memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset)); 861 862 // Checks that the offset temperature is valid. 863 if (estimate->offset_temp_celsius > INVALID_TEMPERATURE_CELSIUS) { 864 const float delta_temp_celsius = 865 temperature_celsius - estimate->offset_temp_celsius; 866 867 // Adds a delta term to the compensated offset using the temperature 868 // difference defined by 'delta_temp_celsius'. 869 addLinearTemperatureExtrapolation(over_temp_cal, compensated_offset, 870 delta_temp_celsius); 871 } 872 873 // Sets the offset compensation vector, temperature, and timestamp. 874 setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos, 875 temperature_celsius); 876 } 877 878 void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal, 879 uint64_t timestamp_nanos, 880 float temperature_celsius, 881 bool compare_to_linear_model) { 882 ASSERT_NOT_NULL(over_temp_cal); 883 ASSERT_NOT_NULL(over_temp_cal->nearest_offset); 884 885 // The default compensated offset is the nearest-temperature offset vector. 886 float compensated_offset[3]; 887 memcpy(compensated_offset, over_temp_cal->nearest_offset->offset, 888 sizeof(compensated_offset)); 889 const float compensated_offset_temperature_celsius = 890 over_temp_cal->nearest_offset->offset_temp_celsius; 891 892 for (size_t index = 0; index < 3; index++) { 893 if (over_temp_cal->temp_sensitivity[index] < OTC_INITIAL_SENSITIVITY) { 894 // If a valid axis model is defined, then use the linear model to assist 895 // with computing an extrapolated compensation term. 896 float delta_temp_celsius = 897 temperature_celsius - compensated_offset_temperature_celsius; 898 compensated_offset[index] += 899 over_temp_cal->temp_sensitivity[index] * delta_temp_celsius; 900 901 // Computes the test offset (based on the linear model or current offset). 902 float test_offset; 903 if (compare_to_linear_model) { 904 test_offset = 905 over_temp_cal->temp_sensitivity[index] * temperature_celsius + 906 over_temp_cal->sensor_intercept[index]; 907 } else { 908 // Adds a delta term to the compensated offset using the temperature 909 // difference defined by 'delta_temp_celsius'. 910 if (over_temp_cal->compensated_offset.offset_temp_celsius <= 911 INVALID_TEMPERATURE_CELSIUS) { 912 // If temperature is invalid, then skip further processing. 913 break; 914 } 915 delta_temp_celsius = 916 temperature_celsius - 917 over_temp_cal->compensated_offset.offset_temp_celsius; 918 test_offset = 919 over_temp_cal->compensated_offset.offset[index] + 920 over_temp_cal->temp_sensitivity[index] * delta_temp_celsius; 921 } 922 923 // Checks for "jumps" in the candidate compensated offset. If detected, 924 // then 'test_offset' is used for the offset update. 925 if (NANO_ABS(test_offset - compensated_offset[index]) >= 926 over_temp_cal->jump_tolerance) { 927 compensated_offset[index] = test_offset; 928 } 929 } 930 } 931 932 // Sets the offset compensation vector, temperature, and timestamp. 933 setCompensatedOffset(over_temp_cal, compensated_offset, timestamp_nanos, 934 temperature_celsius); 935 } 936 937 void updateCalOffset(struct OverTempCal *over_temp_cal, 938 uint64_t timestamp_nanos, float temperature_celsius) { 939 ASSERT_NOT_NULL(over_temp_cal); 940 941 // If 'temperature_celsius' is invalid, then no changes to the compensated 942 // offset are computed. 943 if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) { 944 return; 945 } 946 947 // Removes very old data from the collected model estimates (i.e., 948 // eliminates drift-compromised data). Only does this when there is more 949 // than one estimate in the model (i.e., don't want to remove all data, even 950 // if it is very old [something is likely better than nothing]). 951 if ((timestamp_nanos >= 952 OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer_nanos) && 953 over_temp_cal->num_model_pts > 1) { 954 over_temp_cal->stale_data_timer_nanos = timestamp_nanos; // Resets timer. 955 removeStaleModelData(over_temp_cal, timestamp_nanos); 956 } 957 958 // Ensures that the most relevant model weighting is being used. 959 refreshOtcModel(over_temp_cal, timestamp_nanos); 960 961 // --------------------------------------------------------------------------- 962 // The following boolean expressions help determine how OTC offset updates 963 // are computed below. 964 965 // The nearest-temperature offset estimate is valid if the model data set is 966 // not empty. 967 const bool model_points_available = (over_temp_cal->num_model_pts > 0); 968 969 // To properly evaluate the logic paths that use the latest and nearest offset 970 // data below, the current age of the nearest and latest offset estimates are 971 // computed. 972 uint64_t latest_offset_age_nanos = 0; 973 if (over_temp_cal->latest_offset != NULL) { 974 latest_offset_age_nanos = 975 (over_temp_cal->last_age_update_nanos < timestamp_nanos) 976 ? over_temp_cal->latest_offset->offset_age_nanos + 977 timestamp_nanos - over_temp_cal->last_age_update_nanos 978 : over_temp_cal->latest_offset->offset_age_nanos; 979 } 980 981 uint64_t nearest_offset_age_nanos = 0; 982 if (over_temp_cal->nearest_offset != NULL) { 983 nearest_offset_age_nanos = 984 (over_temp_cal->last_age_update_nanos < timestamp_nanos) 985 ? over_temp_cal->nearest_offset->offset_age_nanos + 986 timestamp_nanos - over_temp_cal->last_age_update_nanos 987 : over_temp_cal->nearest_offset->offset_age_nanos; 988 } 989 990 // True when the latest offset estimate will be used to compute a sensor 991 // offset calibration estimate. 992 const bool use_latest_offset_compensation = 993 over_temp_cal->latest_offset != NULL && model_points_available && 994 latest_offset_age_nanos <= OTC_USE_RECENT_OFFSET_TIME_NANOS; 995 996 // True when the conditions are met to use the nearest-temperature offset to 997 // compute a sensor offset calibration estimate. 998 // The nearest-temperature offset: 999 // i. Must be defined. 1000 // ii. Offset temperature must be within a small neighborhood of the 1001 // current measured temperature (+/- 'delta_temp_per_bin'). 1002 const bool can_compensate_with_nearest = 1003 model_points_available && over_temp_cal->nearest_offset != NULL && 1004 NANO_ABS(temperature_celsius - 1005 over_temp_cal->nearest_offset->offset_temp_celsius) < 1006 over_temp_cal->delta_temp_per_bin; 1007 1008 // True if the last received sensor offset estimate is old or non-existent. 1009 const bool latest_model_point_not_relevant = 1010 (over_temp_cal->latest_offset == NULL) || 1011 (over_temp_cal->latest_offset != NULL && 1012 latest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS); 1013 1014 // True if the nearest-temperature offset estimate is old or non-existent. 1015 const bool nearest_model_point_not_relevant = 1016 (over_temp_cal->nearest_offset == NULL) || 1017 (over_temp_cal->nearest_offset != NULL && 1018 nearest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS); 1019 1020 // --------------------------------------------------------------------------- 1021 // The following conditional expressions govern new OTC offset updates. 1022 1023 if (!model_points_available) { 1024 // Computes the compensation using just the linear model if available, 1025 // otherwise the current compensated offset vector will be kept. 1026 compensateWithLinearModel(over_temp_cal, timestamp_nanos, 1027 temperature_celsius); 1028 return; // no further calculations, exit early. 1029 } 1030 1031 if (use_latest_offset_compensation) { 1032 // Computes the compensation using the latest received offset estimate plus 1033 // a term based on linear extrapolation from the offset temperature to the 1034 // current measured temperature (if a linear model is defined). 1035 compensateWithEstimate(over_temp_cal, timestamp_nanos, 1036 over_temp_cal->latest_offset, temperature_celsius); 1037 return; // no further calculations, exit early. 1038 } 1039 1040 if (can_compensate_with_nearest) { 1041 // Evaluates the nearest-temperature compensation (with a linear 1042 // extrapolation term), and compares it with the compensation due to just 1043 // the linear model, when 'compare_with_linear_model' is true. Otherwise, 1044 // the comparison will be made with an extrapolated version of the current 1045 // compensation value. The comparison determines whether the 1046 // nearest-temperature estimate deviates from the linear-model (or 1047 // current-compensated) value by more than 'jump_tolerance'. If a "jump" is 1048 // detected, then it keeps the linear-model (or current-compensated) value. 1049 const bool compare_with_linear_model = nearest_model_point_not_relevant; 1050 compareAndCompensateWithNearest(over_temp_cal, timestamp_nanos, 1051 temperature_celsius, 1052 compare_with_linear_model); 1053 } else { 1054 if (latest_model_point_not_relevant) { 1055 // If the nearest-temperature offset can't be used for compensation and 1056 // the latest offset is stale (in this case, the overall model trend may 1057 // be more useful for compensation than extending the most recent vector), 1058 // then this resorts to using only the linear model (if defined). 1059 compensateWithLinearModel(over_temp_cal, timestamp_nanos, 1060 temperature_celsius); 1061 } else { 1062 // If the nearest-temperature offset can't be used for compensation and 1063 // the latest offset is fairly recent, then the compensated offset is 1064 // based on the linear extrapolation of the current compensation vector. 1065 compensateWithEstimate(over_temp_cal, timestamp_nanos, 1066 &over_temp_cal->compensated_offset, 1067 temperature_celsius); 1068 } 1069 } 1070 } 1071 1072 void setCompensatedOffset(struct OverTempCal *over_temp_cal, 1073 const float *compensated_offset, 1074 uint64_t timestamp_nanos, float temperature_celsius) { 1075 ASSERT_NOT_NULL(over_temp_cal); 1076 ASSERT_NOT_NULL(compensated_offset); 1077 1078 // If the 'compensated_offset' value has changed significantly, then set 1079 // 'new_overtemp_offset_available' true. 1080 bool new_overtemp_offset_available = false; 1081 for (size_t i = 0; i < 3; i++) { 1082 if (NANO_ABS(over_temp_cal->compensated_offset.offset[i] - 1083 compensated_offset[i]) >= 1084 over_temp_cal->significant_offset_change) { 1085 new_overtemp_offset_available |= true; 1086 break; 1087 } 1088 } 1089 over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available; 1090 1091 // If the offset has changed significantly, then the offset compensation 1092 // vector is updated. 1093 if (new_overtemp_offset_available) { 1094 memcpy(over_temp_cal->compensated_offset.offset, compensated_offset, 1095 sizeof(over_temp_cal->compensated_offset.offset)); 1096 over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius; 1097 } 1098 } 1099 1100 void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, 1101 float offset_temp_celsius) { 1102 ASSERT_NOT_NULL(over_temp_cal); 1103 ASSERT_NOT_NULL(offset); 1104 1105 if (over_temp_cal->latest_offset != NULL) { 1106 // Sets the latest over-temp calibration estimate. 1107 memcpy(over_temp_cal->latest_offset->offset, offset, 1108 sizeof(over_temp_cal->latest_offset->offset)); 1109 over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius; 1110 over_temp_cal->latest_offset->offset_age_nanos = 0; 1111 } 1112 } 1113 1114 void refreshOtcModel(struct OverTempCal *over_temp_cal, 1115 uint64_t timestamp_nanos) { 1116 ASSERT_NOT_NULL(over_temp_cal); 1117 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 1118 timestamp_nanos, over_temp_cal->last_model_update_nanos, 1119 OTC_REFRESH_MODEL_NANOS)) { 1120 // Checks the time since the last computed model and recalculates the model 1121 // if necessary. This ensures that waking up after a long period of time 1122 // allows the properly weighted OTC model to be used. As the estimates age, 1123 // the weighting will become more uniform and the model will fit the whole 1124 // set uniformly as a better approximation to the expected temperature 1125 // sensitivity; Younger estimates will fit tighter to emphasize a more 1126 // localized fit of the temp sensitivity function. 1127 computeModelUpdate(over_temp_cal, timestamp_nanos); 1128 over_temp_cal->last_model_update_nanos = timestamp_nanos; 1129 } 1130 } 1131 1132 void computeModelUpdate(struct OverTempCal *over_temp_cal, 1133 uint64_t timestamp_nanos) { 1134 ASSERT_NOT_NULL(over_temp_cal); 1135 1136 // Ensures that the minimum number of points required for a model fit has been 1137 // satisfied. 1138 if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) return; 1139 1140 // Updates the linear model fit. 1141 float temp_sensitivity[3]; 1142 float sensor_intercept[3]; 1143 updateModel(over_temp_cal, temp_sensitivity, sensor_intercept); 1144 1145 // 2) A new set of model parameters are accepted if: 1146 // i. The model fit parameters must be within certain absolute bounds: 1147 // a. |temp_sensitivity| < temp_sensitivity_limit 1148 // b. |sensor_intercept| < sensor_intercept_limit 1149 // NOTE: Model parameter updates are not qualified against model fit error 1150 // here to protect against the case where there is large change in the 1151 // temperature characteristic either during runtime (e.g., temperature 1152 // conditioning due to hysteresis) or as a result of loading a poor model data 1153 // set. Otherwise, a lockout condition could occur where the entire model 1154 // data set would need to be replaced in order to bring the model fit error 1155 // below the error limit and allow a successful model update. 1156 bool updated_one = false; 1157 for (size_t i = 0; i < 3; i++) { 1158 if (isValidOtcLinearModel(over_temp_cal, temp_sensitivity[i], 1159 sensor_intercept[i])) { 1160 over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; 1161 over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; 1162 updated_one = true; 1163 } else { 1164 #ifdef OVERTEMPCAL_DBG_ENABLED 1165 createDebugTag(over_temp_cal, ":REJECT]"); 1166 CAL_DEBUG_LOG( 1167 over_temp_cal->otc_debug_tag, 1168 "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS 1169 ", " CAL_FORMAT_3DIGITS ", %" PRIu64, 1170 kDebugAxisLabel[i], over_temp_cal->otc_unit_tag, 1171 over_temp_cal->otc_unit_tag, 1172 CAL_ENCODE_FLOAT( 1173 temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3), 1174 CAL_ENCODE_FLOAT( 1175 sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3), 1176 timestamp_nanos); 1177 #endif // OVERTEMPCAL_DBG_ENABLED 1178 } 1179 } 1180 1181 // If at least one axis updated, then consider this a valid model update. 1182 if (updated_one) { 1183 // Resets the OTC model compensation update time and sets the update flag. 1184 over_temp_cal->last_model_update_nanos = timestamp_nanos; 1185 over_temp_cal->new_overtemp_model_available = true; 1186 1187 #ifdef OVERTEMPCAL_DBG_ENABLED 1188 // Updates the total number of model updates. 1189 over_temp_cal->debug_num_model_updates++; 1190 #endif // OVERTEMPCAL_DBG_ENABLED 1191 } 1192 } 1193 1194 void findNearestEstimate(struct OverTempCal *over_temp_cal, 1195 float temperature_celsius) { 1196 ASSERT_NOT_NULL(over_temp_cal); 1197 1198 // If 'temperature_celsius' is invalid, then do not search. 1199 if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) { 1200 return; 1201 } 1202 1203 // Performs a brute force search for the estimate nearest 1204 // 'temperature_celsius'. 1205 float dtemp_new = 0.0f; 1206 float dtemp_old = FLT_MAX; 1207 over_temp_cal->nearest_offset = &over_temp_cal->model_data[0]; 1208 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 1209 dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp_celsius - 1210 temperature_celsius); 1211 if (dtemp_new < dtemp_old) { 1212 over_temp_cal->nearest_offset = &over_temp_cal->model_data[i]; 1213 dtemp_old = dtemp_new; 1214 } 1215 } 1216 } 1217 1218 void removeStaleModelData(struct OverTempCal *over_temp_cal, 1219 uint64_t timestamp_nanos) { 1220 ASSERT_NOT_NULL(over_temp_cal); 1221 1222 bool removed_one = false; 1223 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 1224 if (over_temp_cal->model_data[i].offset_age_nanos >= 1225 over_temp_cal->age_limit_nanos) { 1226 // If the latest offset was removed, then indicate this by setting it to 1227 // NULL. 1228 if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) { 1229 over_temp_cal->latest_offset = NULL; 1230 } 1231 removed_one |= removeModelDataByIndex(over_temp_cal, i); 1232 } 1233 } 1234 1235 if (removed_one) { 1236 // If anything was removed, then this attempts to recompute the model. 1237 computeModelUpdate(over_temp_cal, timestamp_nanos); 1238 1239 // Searches for the sensor offset estimate closest to the current 1240 // temperature. 1241 findNearestEstimate(over_temp_cal, 1242 over_temp_cal->compensated_offset.offset_temp_celsius); 1243 } 1244 } 1245 1246 bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, 1247 size_t model_index) { 1248 ASSERT_NOT_NULL(over_temp_cal); 1249 1250 // This function will not remove all of the model data. At least one model 1251 // sample will be left. 1252 if (over_temp_cal->num_model_pts <= 1) { 1253 return false; 1254 } 1255 1256 #ifdef OVERTEMPCAL_DBG_ENABLED 1257 createDebugTag(over_temp_cal, ":REMOVE]"); 1258 CAL_DEBUG_LOG( 1259 over_temp_cal->otc_debug_tag, 1260 "Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET 1261 ", " CAL_FORMAT_3DIGITS ", %" PRIu64, 1262 over_temp_cal->otc_unit_tag, 1263 CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] * 1264 over_temp_cal->otc_unit_conversion, 1265 3), 1266 CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] * 1267 over_temp_cal->otc_unit_conversion, 1268 3), 1269 CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[1] * 1270 over_temp_cal->otc_unit_conversion, 1271 3), 1272 CAL_ENCODE_FLOAT( 1273 over_temp_cal->model_data[model_index].offset_temp_celsius, 3), 1274 over_temp_cal->model_data[model_index].offset_age_nanos); 1275 #endif // OVERTEMPCAL_DBG_ENABLED 1276 1277 // Remove the model data at 'model_index'. 1278 for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) { 1279 memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1], 1280 sizeof(struct OverTempModelThreeAxis)); 1281 } 1282 over_temp_cal->num_model_pts--; 1283 1284 return true; 1285 } 1286 1287 bool jumpStartModelData(struct OverTempCal *over_temp_cal) { 1288 ASSERT_NOT_NULL(over_temp_cal); 1289 ASSERT(over_temp_cal->delta_temp_per_bin > 0); 1290 1291 // Prevent a divide by zero below. 1292 if (over_temp_cal->delta_temp_per_bin <= 0) { 1293 return false; 1294 } 1295 1296 // In normal operation the offset estimates enter into the 'model_data' array 1297 // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart 1298 // data produced here requires that the model parameters have all been fully 1299 // defined and are all within the valid range. 1300 for (size_t i = 0; i < 3; i++) { 1301 if (!isValidOtcLinearModel(over_temp_cal, 1302 over_temp_cal->temp_sensitivity[i], 1303 over_temp_cal->sensor_intercept[i])) { 1304 return false; 1305 } 1306 } 1307 1308 // Any pre-existing model data points will be overwritten. 1309 over_temp_cal->num_model_pts = 0; 1310 1311 // This defines the minimum contiguous set of points to allow a model update 1312 // when the next offset estimate is received. They are placed at a common 1313 // temperature range that is likely to get replaced with actual data soon. 1314 const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS / 1315 over_temp_cal->delta_temp_per_bin); 1316 float offset_temp_celsius = 1317 (start_bin_num + 0.5f) * over_temp_cal->delta_temp_per_bin; 1318 1319 for (size_t i = 0; i < over_temp_cal->min_num_model_pts; i++) { 1320 for (size_t j = 0; j < 3; j++) { 1321 over_temp_cal->model_data[i].offset[j] = 1322 over_temp_cal->temp_sensitivity[j] * offset_temp_celsius + 1323 over_temp_cal->sensor_intercept[j]; 1324 } 1325 over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius; 1326 over_temp_cal->model_data[i].offset_age_nanos = 0; 1327 1328 offset_temp_celsius += over_temp_cal->delta_temp_per_bin; 1329 over_temp_cal->num_model_pts++; 1330 } 1331 1332 #ifdef OVERTEMPCAL_DBG_ENABLED 1333 createDebugTag(over_temp_cal, ":INIT]"); 1334 if (over_temp_cal->num_model_pts > 0) { 1335 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 1336 "Model Jump-Start: #Points = %zu.", 1337 over_temp_cal->num_model_pts); 1338 } 1339 #endif // OVERTEMPCAL_DBG_ENABLED 1340 1341 return (over_temp_cal->num_model_pts > 0); 1342 } 1343 1344 void updateModel(const struct OverTempCal *over_temp_cal, 1345 float *temp_sensitivity, float *sensor_intercept) { 1346 ASSERT_NOT_NULL(over_temp_cal); 1347 ASSERT_NOT_NULL(temp_sensitivity); 1348 ASSERT_NOT_NULL(sensor_intercept); 1349 ASSERT(over_temp_cal->num_model_pts > 0); 1350 1351 float sw = 0.0f; 1352 float st = 0.0f, stt = 0.0f; 1353 float sx = 0.0f, stsx = 0.0f; 1354 float sy = 0.0f, stsy = 0.0f; 1355 float sz = 0.0f, stsz = 0.0f; 1356 float weight = 1.0f; 1357 1358 // First pass computes the weighted mean values. 1359 const size_t n = over_temp_cal->num_model_pts; 1360 for (size_t i = 0; i < n; ++i) { 1361 weight = evaluateWeightingFunction( 1362 over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos); 1363 1364 sw += weight; 1365 st += over_temp_cal->model_data[i].offset_temp_celsius * weight; 1366 sx += over_temp_cal->model_data[i].offset[0] * weight; 1367 sy += over_temp_cal->model_data[i].offset[1] * weight; 1368 sz += over_temp_cal->model_data[i].offset[2] * weight; 1369 } 1370 1371 // Second pass computes the mean corrected second moment values. 1372 ASSERT(sw > 0.0f); 1373 const float inv_sw = 1.0f / sw; 1374 for (size_t i = 0; i < n; ++i) { 1375 weight = evaluateWeightingFunction( 1376 over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos); 1377 1378 const float t = 1379 over_temp_cal->model_data[i].offset_temp_celsius - st * inv_sw; 1380 stt += weight * t * t; 1381 stsx += t * over_temp_cal->model_data[i].offset[0] * weight; 1382 stsy += t * over_temp_cal->model_data[i].offset[1] * weight; 1383 stsz += t * over_temp_cal->model_data[i].offset[2] * weight; 1384 } 1385 1386 // Calculates the linear model fit parameters. 1387 ASSERT(stt > 0.0f); 1388 const float inv_stt = 1.0f / stt; 1389 temp_sensitivity[0] = stsx * inv_stt; 1390 sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_sw; 1391 temp_sensitivity[1] = stsy * inv_stt; 1392 sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_sw; 1393 temp_sensitivity[2] = stsz * inv_stt; 1394 sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_sw; 1395 } 1396 1397 bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset, 1398 size_t axis_index, float temperature_celsius) { 1399 ASSERT_NOT_NULL(over_temp_cal); 1400 ASSERT_NOT_NULL(offset); 1401 1402 // If a model has been defined, then check to see if this offset could be a 1403 // potential outlier: 1404 if (over_temp_cal->temp_sensitivity[axis_index] < OTC_INITIAL_SENSITIVITY) { 1405 const float outlier_test = NANO_ABS( 1406 offset[axis_index] - 1407 (over_temp_cal->temp_sensitivity[axis_index] * temperature_celsius + 1408 over_temp_cal->sensor_intercept[axis_index])); 1409 1410 if (outlier_test > over_temp_cal->outlier_limit) { 1411 return true; 1412 } 1413 } 1414 1415 return false; 1416 } 1417 1418 void resetOtcLinearModel(struct OverTempCal *over_temp_cal) { 1419 ASSERT_NOT_NULL(over_temp_cal); 1420 1421 // Sets the temperature sensitivity model parameters to 1422 // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial" 1423 // state. 1424 over_temp_cal->temp_sensitivity[0] = OTC_INITIAL_SENSITIVITY; 1425 over_temp_cal->temp_sensitivity[1] = OTC_INITIAL_SENSITIVITY; 1426 over_temp_cal->temp_sensitivity[2] = OTC_INITIAL_SENSITIVITY; 1427 memset(over_temp_cal->sensor_intercept, 0, 1428 sizeof(over_temp_cal->sensor_intercept)); 1429 } 1430 1431 bool checkAndEnforceTemperatureRange(float *temperature_celsius) { 1432 if (*temperature_celsius > OTC_TEMP_MAX_CELSIUS) { 1433 *temperature_celsius = OTC_TEMP_MAX_CELSIUS; 1434 return false; 1435 } 1436 if (*temperature_celsius < OTC_TEMP_MIN_CELSIUS) { 1437 *temperature_celsius = OTC_TEMP_MIN_CELSIUS; 1438 return false; 1439 } 1440 return true; 1441 } 1442 1443 bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal, 1444 float temp_sensitivity, float sensor_intercept) { 1445 ASSERT_NOT_NULL(over_temp_cal); 1446 1447 return NANO_ABS(temp_sensitivity) < over_temp_cal->temp_sensitivity_limit && 1448 NANO_ABS(sensor_intercept) < over_temp_cal->sensor_intercept_limit && 1449 NANO_ABS(temp_sensitivity) > OTC_MODELDATA_NEAR_ZERO_TOL && 1450 NANO_ABS(sensor_intercept) > OTC_MODELDATA_NEAR_ZERO_TOL; 1451 } 1452 1453 bool isValidOtcOffset(const float *offset, float offset_temp_celsius) { 1454 ASSERT_NOT_NULL(offset); 1455 1456 // Simple check to ensure that: 1457 // 1. All of the input data is non "zero". 1458 // 2. The offset temperature is within the valid range. 1459 if (NANO_ABS(offset[0]) < OTC_MODELDATA_NEAR_ZERO_TOL && 1460 NANO_ABS(offset[1]) < OTC_MODELDATA_NEAR_ZERO_TOL && 1461 NANO_ABS(offset[2]) < OTC_MODELDATA_NEAR_ZERO_TOL && 1462 NANO_ABS(offset_temp_celsius) < OTC_MODELDATA_NEAR_ZERO_TOL) { 1463 return false; 1464 } 1465 1466 // Only returns the "check" result. Don't care about coercion. 1467 return checkAndEnforceTemperatureRange(&offset_temp_celsius); 1468 } 1469 1470 float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, 1471 uint64_t offset_age_nanos) { 1472 ASSERT_NOT_NULL(over_temp_cal); 1473 for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) { 1474 if (offset_age_nanos <= 1475 over_temp_cal->weighting_function[i].offset_age_nanos) { 1476 return over_temp_cal->weighting_function[i].weight; 1477 } 1478 } 1479 1480 // Returning the default weight for all older offsets. 1481 return OTC_MIN_WEIGHT_VALUE; 1482 } 1483 1484 void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal, 1485 uint64_t timestamp_nanos) { 1486 ASSERT_NOT_NULL(over_temp_cal); 1487 if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) { 1488 // Age updates must be monotonic. 1489 return; 1490 } 1491 1492 uint64_t age_increment_nanos = 1493 timestamp_nanos - over_temp_cal->last_age_update_nanos; 1494 1495 // Resets the age update counter. 1496 over_temp_cal->last_age_update_nanos = timestamp_nanos; 1497 1498 // Updates the model dataset ages. 1499 for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { 1500 over_temp_cal->model_data[i].offset_age_nanos += age_increment_nanos; 1501 } 1502 } 1503 1504 /////// DEBUG FUNCTION DEFINITIONS //////////////////////////////////////////// 1505 1506 #ifdef OVERTEMPCAL_DBG_ENABLED 1507 void createDebugTag(struct OverTempCal *over_temp_cal, 1508 const char *new_debug_tag) { 1509 over_temp_cal->otc_debug_tag[0] = '['; 1510 memcpy(over_temp_cal->otc_debug_tag + 1, over_temp_cal->otc_sensor_tag, 1511 strlen(over_temp_cal->otc_sensor_tag)); 1512 memcpy( 1513 over_temp_cal->otc_debug_tag + strlen(over_temp_cal->otc_sensor_tag) + 1, 1514 new_debug_tag, strlen(new_debug_tag) + 1); 1515 } 1516 1517 void updateDebugData(struct OverTempCal *over_temp_cal) { 1518 ASSERT_NOT_NULL(over_temp_cal); 1519 1520 // Only update this data if debug printing is not currently in progress 1521 // (i.e., don't want to risk overwriting debug information that is actively 1522 // being reported). 1523 if (over_temp_cal->debug_state != OTC_IDLE) { 1524 return; 1525 } 1526 1527 // Triggers a debug log printout. 1528 over_temp_cal->debug_print_trigger = true; 1529 1530 // Initializes the debug data structure. 1531 memset(&over_temp_cal->debug_overtempcal, 0, sizeof(struct DebugOverTempCal)); 1532 1533 // Copies over the relevant data. 1534 for (size_t i = 0; i < 3; i++) { 1535 if (isValidOtcLinearModel(over_temp_cal, over_temp_cal->temp_sensitivity[i], 1536 over_temp_cal->sensor_intercept[i])) { 1537 over_temp_cal->debug_overtempcal.temp_sensitivity[i] = 1538 over_temp_cal->temp_sensitivity[i]; 1539 over_temp_cal->debug_overtempcal.sensor_intercept[i] = 1540 over_temp_cal->sensor_intercept[i]; 1541 } else { 1542 // If the model is not valid then just set the debug information so that 1543 // zeros are printed. 1544 over_temp_cal->debug_overtempcal.temp_sensitivity[i] = 0.0f; 1545 over_temp_cal->debug_overtempcal.sensor_intercept[i] = 0.0f; 1546 } 1547 } 1548 1549 // If 'latest_offset' is defined the copy the data for debug printing. 1550 // Otherwise, the current compensated offset will be printed. 1551 if (over_temp_cal->latest_offset != NULL) { 1552 memcpy(&over_temp_cal->debug_overtempcal.latest_offset, 1553 over_temp_cal->latest_offset, sizeof(struct OverTempModelThreeAxis)); 1554 } else { 1555 memcpy(&over_temp_cal->debug_overtempcal.latest_offset, 1556 &over_temp_cal->compensated_offset, 1557 sizeof(struct OverTempModelThreeAxis)); 1558 } 1559 1560 // Total number of OTC model data points. 1561 over_temp_cal->debug_overtempcal.num_model_pts = over_temp_cal->num_model_pts; 1562 1563 // Computes the maximum error over all of the model data. 1564 overTempGetModelError(over_temp_cal, 1565 over_temp_cal->debug_overtempcal.temp_sensitivity, 1566 over_temp_cal->debug_overtempcal.sensor_intercept, 1567 over_temp_cal->debug_overtempcal.max_error); 1568 } 1569 1570 void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 1571 uint64_t timestamp_nanos) { 1572 ASSERT_NOT_NULL(over_temp_cal); 1573 1574 // This is a state machine that controls the reporting out of debug data. 1575 createDebugTag(over_temp_cal, ":REPORT]"); 1576 switch (over_temp_cal->debug_state) { 1577 case OTC_IDLE: 1578 // Wait for a trigger and start the debug printout sequence. 1579 if (over_temp_cal->debug_print_trigger) { 1580 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, ""); 1581 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Debug Version: %s", 1582 OTC_DEBUG_VERSION_STRING); 1583 over_temp_cal->debug_print_trigger = false; // Resets trigger. 1584 over_temp_cal->debug_state = OTC_PRINT_OFFSET; 1585 } else { 1586 over_temp_cal->debug_state = OTC_IDLE; 1587 } 1588 break; 1589 1590 case OTC_WAIT_STATE: 1591 // This helps throttle the print statements. 1592 if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( 1593 timestamp_nanos, over_temp_cal->wait_timer_nanos, 1594 OTC_WAIT_TIME_NANOS)) { 1595 over_temp_cal->debug_state = over_temp_cal->next_state; 1596 } 1597 break; 1598 1599 case OTC_PRINT_OFFSET: 1600 // Prints out the latest offset estimate (input data). 1601 CAL_DEBUG_LOG( 1602 over_temp_cal->otc_debug_tag, 1603 "Cal#|Offset|Temp|Age [%s|C|nsec]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET 1604 ", " CAL_FORMAT_3DIGITS ", %" PRIu64, 1605 over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates, 1606 CAL_ENCODE_FLOAT( 1607 over_temp_cal->debug_overtempcal.latest_offset.offset[0] * 1608 over_temp_cal->otc_unit_conversion, 1609 3), 1610 CAL_ENCODE_FLOAT( 1611 over_temp_cal->debug_overtempcal.latest_offset.offset[1] * 1612 over_temp_cal->otc_unit_conversion, 1613 3), 1614 CAL_ENCODE_FLOAT( 1615 over_temp_cal->debug_overtempcal.latest_offset.offset[2] * 1616 over_temp_cal->otc_unit_conversion, 1617 3), 1618 CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset 1619 .offset_temp_celsius, 1620 3), 1621 over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos); 1622 1623 // clang-format off 1624 over_temp_cal->wait_timer_nanos = 1625 timestamp_nanos; // Starts the wait timer. 1626 over_temp_cal->next_state = 1627 OTC_PRINT_MODEL_PARAMETERS; // Sets the next state. 1628 over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. 1629 // clang-format on 1630 break; 1631 1632 case OTC_PRINT_MODEL_PARAMETERS: 1633 // Prints out the model parameters. 1634 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 1635 "Cal#|Sensitivity [%s/C]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET, 1636 over_temp_cal->otc_unit_tag, 1637 over_temp_cal->debug_num_estimates, 1638 CAL_ENCODE_FLOAT( 1639 over_temp_cal->debug_overtempcal.temp_sensitivity[0] * 1640 over_temp_cal->otc_unit_conversion, 1641 3), 1642 CAL_ENCODE_FLOAT( 1643 over_temp_cal->debug_overtempcal.temp_sensitivity[1] * 1644 over_temp_cal->otc_unit_conversion, 1645 3), 1646 CAL_ENCODE_FLOAT( 1647 over_temp_cal->debug_overtempcal.temp_sensitivity[2] * 1648 over_temp_cal->otc_unit_conversion, 1649 3)); 1650 1651 CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, 1652 "Cal#|Intercept [%s]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET, 1653 over_temp_cal->otc_unit_tag, 1654 over_temp_cal->debug_num_estimates, 1655 CAL_ENCODE_FLOAT( 1656 over_temp_cal->debug_overtempcal.sensor_intercept[0] * 1657 over_temp_cal->otc_unit_conversion, 1658 3), 1659 CAL_ENCODE_FLOAT( 1660 over_temp_cal->debug_overtempcal.sensor_intercept[1] * 1661 over_temp_cal->otc_unit_conversion, 1662 3), 1663 CAL_ENCODE_FLOAT( 1664 over_temp_cal->debug_overtempcal.sensor_intercept[2] * 1665 over_temp_cal->otc_unit_conversion, 1666 3)); 1667 1668 over_temp_cal->wait_timer_nanos = 1669 timestamp_nanos; // Starts the wait timer. 1670 over_temp_cal->next_state = 1671 OTC_PRINT_MODEL_ERROR; // Sets the next state. 1672 over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. 1673 break; 1674 1675 case OTC_PRINT_MODEL_ERROR: 1676 // Computes the maximum error over all of the model data. 1677 CAL_DEBUG_LOG( 1678 over_temp_cal->otc_debug_tag, 1679 "Cal#|#Updates|#ModelPts|Model Error [%s]: %zu, " 1680 "%zu, %zu, " CAL_FORMAT_3DIGITS_TRIPLET, 1681 over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates, 1682 over_temp_cal->debug_num_model_updates, 1683 over_temp_cal->debug_overtempcal.num_model_pts, 1684 CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] * 1685 over_temp_cal->otc_unit_conversion, 1686 3), 1687 CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[1] * 1688 over_temp_cal->otc_unit_conversion, 1689 3), 1690 CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[2] * 1691 over_temp_cal->otc_unit_conversion, 1692 3)); 1693 1694 over_temp_cal->model_counter = 0; // Resets the model data print counter. 1695 over_temp_cal->wait_timer_nanos = 1696 timestamp_nanos; // Starts the wait timer. 1697 over_temp_cal->next_state = OTC_PRINT_MODEL_DATA; // Sets the next state. 1698 over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. 1699 break; 1700 1701 case OTC_PRINT_MODEL_DATA: 1702 // Prints out all of the model data. 1703 if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) { 1704 CAL_DEBUG_LOG( 1705 over_temp_cal->otc_debug_tag, 1706 " Model[%zu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET 1707 ", " CAL_FORMAT_3DIGITS ", %" PRIu64, 1708 over_temp_cal->model_counter, over_temp_cal->otc_unit_tag, 1709 CAL_ENCODE_FLOAT( 1710 over_temp_cal->model_data[over_temp_cal->model_counter] 1711 .offset[0] * 1712 over_temp_cal->otc_unit_conversion, 1713 3), 1714 CAL_ENCODE_FLOAT( 1715 over_temp_cal->model_data[over_temp_cal->model_counter] 1716 .offset[1] * 1717 over_temp_cal->otc_unit_conversion, 1718 3), 1719 CAL_ENCODE_FLOAT( 1720 over_temp_cal->model_data[over_temp_cal->model_counter] 1721 .offset[2] * 1722 over_temp_cal->otc_unit_conversion, 1723 3), 1724 CAL_ENCODE_FLOAT( 1725 over_temp_cal->model_data[over_temp_cal->model_counter] 1726 .offset_temp_celsius, 1727 3), 1728 over_temp_cal->model_data[over_temp_cal->model_counter] 1729 .offset_age_nanos); 1730 1731 over_temp_cal->model_counter++; 1732 over_temp_cal->wait_timer_nanos = 1733 timestamp_nanos; // Starts the wait timer. 1734 over_temp_cal->next_state = 1735 OTC_PRINT_MODEL_DATA; // Sets the next state. 1736 over_temp_cal->debug_state = 1737 OTC_WAIT_STATE; // First, go to wait state. 1738 } else { 1739 // Sends this state machine to its idle state. 1740 over_temp_cal->wait_timer_nanos = 1741 timestamp_nanos; // Starts the wait timer. 1742 over_temp_cal->next_state = OTC_IDLE; // Sets the next state. 1743 over_temp_cal->debug_state = 1744 OTC_WAIT_STATE; // First, go to wait state. 1745 } 1746 break; 1747 1748 default: 1749 // Sends this state machine to its idle state. 1750 over_temp_cal->wait_timer_nanos = 1751 timestamp_nanos; // Starts the wait timer. 1752 over_temp_cal->next_state = OTC_IDLE; // Sets the next state. 1753 over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. 1754 } 1755 } 1756 1757 void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal, 1758 const char *otc_sensor_tag, 1759 const char *otc_unit_tag, 1760 float otc_unit_conversion) { 1761 ASSERT_NOT_NULL(over_temp_cal); 1762 ASSERT_NOT_NULL(otc_sensor_tag); 1763 ASSERT_NOT_NULL(otc_unit_tag); 1764 1765 // Sets the sensor descriptor, displayed units, and unit conversion factor. 1766 strcpy(over_temp_cal->otc_sensor_tag, otc_sensor_tag); 1767 strcpy(over_temp_cal->otc_unit_tag, otc_unit_tag); 1768 over_temp_cal->otc_unit_conversion = otc_unit_conversion; 1769 } 1770 1771 #endif // OVERTEMPCAL_DBG_ENABLED 1772
