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