diff options
Diffstat (limited to 'firmware/os/algos/calibration/over_temp')
| -rw-r--r-- | firmware/os/algos/calibration/over_temp/over_temp_cal.c | 940 | ||||
| -rw-r--r-- | firmware/os/algos/calibration/over_temp/over_temp_cal.h | 327 |
2 files changed, 0 insertions, 1267 deletions
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c deleted file mode 100644 index 0b8b6c73..00000000 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c +++ /dev/null @@ -1,940 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "calibration/over_temp/over_temp_cal.h" - -#include <float.h> -#include <math.h> -#include <string.h> - -#include "calibration/util/cal_log.h" -#include "common/math/vec.h" -#include "util/nano_assert.h" - -/////// DEFINITIONS AND MACROS //////////////////////////////////////////////// - -// The 'temp_sensitivity' parameters are set to this value to indicate that the -// model is in its initial state. -#define MODEL_INITIAL_STATE (1e6f) - -// Rate-limits the search for the nearest offset estimate to every 10 seconds -// when no model has been updated yet. -#define OVERTEMPCAL_NEAREST_NANOS (10000000000) - -// Rate-limits the check of old data to every 2 hours. -#define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000) - -// A common sensor operating temperature at which to start producing the model -// jump-start data. -#define JUMPSTART_START_TEMP_CELSIUS (30.0f) - -#ifdef OVERTEMPCAL_DBG_ENABLED -// A debug version label to help with tracking results. -#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Dec 12, 2016]" - -// The time value used to throttle debug messaging. -#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000) - -// Debug log tag string used to identify debug report output data. -#define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]" - -// Converts units of radians to milli-degrees. -#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI) - -// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z. -static const char kDebugAxisLabel[3] = "XYZ"; -#endif // OVERTEMPCAL_DBG_ENABLED - -/////// FORWARD DECLARATIONS ////////////////////////////////////////////////// - -// Updates the most recent model estimate data. -static void setLatestEstimate(struct OverTempCal *over_temp_cal, - const float *offset, float offset_temp, - uint64_t timestamp_nanos); - -/* - * Determines if a new over-temperature model fit should be performed, and then - * updates the model as needed. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * timestamp_nanos: Current timestamp for the model update. - */ -static void computeModelUpdate(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos); - -/* - * Searches 'model_data' for the sensor offset estimate closest to the current - * temperature. Sets the 'latest_offset' pointer to the result. - */ -static void setNearestEstimate(struct OverTempCal *over_temp_cal); - -/* - * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the - * drift-compromised data). Returns 'true' if any data was removed. - */ -static bool removeStaleModelData(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos); - -/* - * Removes the offset estimates from 'model_data' at index, 'model_index'. - * Returns 'true' if data was removed. - */ -static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, - size_t model_index); - -/* - * Since it may take a while for an empty model to build up enough data to start - * producing new model parameter updates, the model collection can be - * jump-started by using the new model parameters to insert fake data in place - * of actual sensor offset data. - */ -static bool jumpStartModelData(struct OverTempCal *over_temp_cal); - -/* - * Provides updated model parameters for the over-temperature model data. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * OUTPUTS: - * temp_sensitivity: Updated modeled temperature sensitivity (array). - * sensor_intercept: Updated model intercept (array). - * - * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - * - * Reference: "Comparing two ways to fit a line to data", John D. Cook. - * http://www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/ - */ -static void updateModel(const struct OverTempCal *over_temp_cal, - float *temp_sensitivity, float *sensor_intercept); - -/* - * Removes the over-temp compensated offset from the input sensor data. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * vi: Single axis sensor data to be compensated. - * index: Index for model parameter compensation (0=x, 1=y, 2=z). - * OUTPUTS: - * vo: Single axis sensor data that has been compensated. - */ -static void removeSensorOffset(const struct OverTempCal *over_temp_cal, - float vi, size_t index, float *vo); - -/* - * Computes the over-temperature compensated sensor offset estimate based on the - * input model parameters. Note, this is a single axis calculation. - * comp_offset = (temp_sensitivity * temperature + sensor_intercept) - * - * INPUTS: - * temperature: Temperature value at which to compute the estimate. - * temp_sensitivity: Temperature sensitivity model parameter. - * sensor_intercept: Sensor intercept model parameter. - * RETURNS: - * comp_offset: Over-temperature compensated sensor offset estimate. - */ -static float getCompensatedOffset(float temperature, float temp_sensitivity, - float sensor_intercept); - -/////// FUNCTION DEFINITIONS ////////////////////////////////////////////////// - -void overTempCalInit(struct OverTempCal *over_temp_cal, - size_t min_num_model_pts, - uint64_t min_update_interval_nanos, - float delta_temp_per_bin, float max_error_limit, - uint64_t age_limit_nanos, float temp_sensitivity_limit, - float sensor_intercept_limit, bool over_temp_enable) { - ASSERT_NOT_NULL(over_temp_cal); - - // Clears OverTempCal memory. - memset(over_temp_cal, 0, sizeof(struct OverTempCal)); - - // Initializes the pointer to the most recent sensor offset estimate. Sets it - // as the first element in 'model_data'. - over_temp_cal->latest_offset = &over_temp_cal->model_data[0]; - - // Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to - // indicate that the model is in an "initial" state. - over_temp_cal->temp_sensitivity[0] = MODEL_INITIAL_STATE; - over_temp_cal->temp_sensitivity[1] = MODEL_INITIAL_STATE; - over_temp_cal->temp_sensitivity[2] = MODEL_INITIAL_STATE; - - // Initializes the model identification parameters. - over_temp_cal->new_overtemp_model_available = false; - over_temp_cal->min_num_model_pts = min_num_model_pts; - over_temp_cal->min_update_interval_nanos = min_update_interval_nanos; - over_temp_cal->delta_temp_per_bin = delta_temp_per_bin; - over_temp_cal->max_error_limit = max_error_limit; - over_temp_cal->age_limit_nanos = age_limit_nanos; - over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit; - over_temp_cal->sensor_intercept_limit = sensor_intercept_limit; - over_temp_cal->over_temp_enable = over_temp_enable; - -#ifdef OVERTEMPCAL_DBG_ENABLED - CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu", - (unsigned long int)sizeof(struct OverTempCal)); - CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Over-Temp Cal: Initialized."); - -#endif // OVERTEMPCAL_DBG_ENABLED -} - -void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, - float offset_temp, uint64_t timestamp_nanos, - const float *temp_sensitivity, - const float *sensor_intercept, bool jump_start_model) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); - - // Sets the model parameters if they are within the acceptable limits. - size_t i; - for (i = 0; i < 3; i++) { - if (NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit && - NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) { - over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; - over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; - } - } - - // Sets the model update time to the current timestamp. - over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos; - - // Model "Jump-Start". - const bool model_jump_started = - (jump_start_model) ? jumpStartModelData(over_temp_cal) : false; - - if (!model_jump_started) { - // Sets the initial over-temp calibration estimate and model data. - setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos); - - // Now there is one offset estimate in the model. - over_temp_cal->num_model_pts = 1; - } - -#ifdef OVERTEMPCAL_DBG_ENABLED - // Prints the updated model data. - CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]", - "Model parameters recalled from memory."); - - // Trigger a debug print out to view the new model parameters. - over_temp_cal->debug_print_trigger = true; -#endif // OVERTEMPCAL_DBG_ENABLED -} - -void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, - float *offset_temp, uint64_t *timestamp_nanos, - float *temp_sensitivity, float *sensor_intercept) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - ASSERT_NOT_NULL(offset); - ASSERT_NOT_NULL(offset_temp); - ASSERT_NOT_NULL(timestamp_nanos); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); - - // Updates the latest offset so that it is the one nearest to the current - // temperature. - setNearestEstimate(over_temp_cal); - - // Gets the over-temp calibration estimate and model data. - memcpy(offset, over_temp_cal->latest_offset->offset, 3 * sizeof(float)); - memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float)); - memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float)); - *offset_temp = over_temp_cal->latest_offset->offset_temp; - *timestamp_nanos = over_temp_cal->latest_offset->timestamp_nanos; -} - -void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, float xi, float yi, - float zi, float *xo, float *yo, float *zo) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - ASSERT_NOT_NULL(xo); - ASSERT_NOT_NULL(yo); - ASSERT_NOT_NULL(zo); - - // Determines whether over-temp compensation will be applied. - if (!over_temp_cal->over_temp_enable) { - return; - } - - // Removes very old data from the collected model estimates (eliminates - // drift-compromised data). Only does this when there is more than one - // estimate in the model (i.e., don't want to remove all data, even if it is - // very old [something is likely better than nothing]). - if ((timestamp_nanos - over_temp_cal->stale_data_timer) >= - OVERTEMPCAL_STALE_CHECK_TIME_NANOS && - over_temp_cal->num_model_pts > 1) { - over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer. - - if (removeStaleModelData(over_temp_cal, timestamp_nanos)) { - // If anything was removed, then this attempts to recompute the model. - computeModelUpdate(over_temp_cal, timestamp_nanos); - } - } - - // Removes the over-temperature compensated offset from the input sensor data. - removeSensorOffset(over_temp_cal, xi, 0, xo); - removeSensorOffset(over_temp_cal, yi, 1, yo); - removeSensorOffset(over_temp_cal, zi, 2, zo); -} - -bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); - const bool update_available = over_temp_cal->new_overtemp_model_available && - over_temp_cal->over_temp_enable; - - // The 'new_overtemp_model_available' flag is reset when it is read here. - over_temp_cal->new_overtemp_model_available = false; - - return update_available; -} - -void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, - const float *offset, - float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - ASSERT_NOT_NULL(offset); - ASSERT(over_temp_cal->delta_temp_per_bin > 0); - - // Prevent a divide by zero below. - if (over_temp_cal->delta_temp_per_bin <= 0) { - return; - } - -#ifdef OVERTEMPCAL_DBG_ENABLED - // Updates the total count of offset estimates. - over_temp_cal->debug_num_estimates++; - - // If there are fewer than the minimum number of points to produce a model, - // then trigger a debug printout to view the model building process. - over_temp_cal->debug_print_trigger |= - (over_temp_cal->num_model_pts <= over_temp_cal->min_num_model_pts); -#endif // OVERTEMPCAL_DBG_ENABLED - - // Provides an early escape if this is the first model estimate. - if (over_temp_cal->num_model_pts == 0) { - setLatestEstimate(over_temp_cal, offset, temperature_celsius, - timestamp_nanos); - over_temp_cal->num_model_pts = 1; // one estimate was added above. - return; - } - - // Computes the temperature bin range data. - const int32_t bin_num = - CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin); - const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin; - const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin; - - // The rules for accepting new offset estimates into the 'model_data' - // collection: - // 1) The temperature domain is divided into bins each spanning - // 'delta_temp_per_bin'. - // 2) Find and replace the i'th 'model_data' estimate data if: - // Let, bin_num = floor(temperature_celsius / delta_temp_per_bin) - // temp_lo_check = bin_num * delta_temp_per_bin - // temp_hi_check = (bin_num + 1) * delta_temp_per_bin - // Check condition: - // temp_lo_check <= model_data[i].offset_temp < temp_hi_check - bool replaced_one = false; - size_t i = 0; - for (i = 0; i < over_temp_cal->num_model_pts; i++) { - if (over_temp_cal->model_data[i].offset_temp < temp_hi_check && - over_temp_cal->model_data[i].offset_temp >= temp_lo_check) { - // NOTE - the pointer to the estimate that is getting replaced is set - // here; the offset values are set below in the call to - // 'setLatestEstimate'. - over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; - replaced_one = true; - break; - } - } - - // 3) If nothing was replaced, and the 'model_data' buffer is not full - // then add the estimate data to the array. - // 4) Otherwise (nothing was replaced and buffer is full), replace the - // 'latest_offset' with the incoming one. This is done below. - if (!replaced_one && over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) { - // NOTE - the pointer to the next available array location is set here; - // the offset values are set below in the call to 'setLatestEstimate'. - over_temp_cal->latest_offset = - &over_temp_cal->model_data[over_temp_cal->num_model_pts]; - over_temp_cal->num_model_pts++; - } - - // Updates the latest model estimate data. - setLatestEstimate(over_temp_cal, offset, temperature_celsius, - timestamp_nanos); - - // Conditionally updates the over-temp model. See 'computeModelUpdate' for - // update conditions. - computeModelUpdate(over_temp_cal, timestamp_nanos); - -} - -void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, - float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - -#ifdef OVERTEMPCAL_DBG_ENABLED -#ifdef OVERTEMPCAL_DBG_LOG_TEMP - static uint64_t wait_timer = 0; - // Prints the sensor temperature trajectory for debugging purposes. - // This throttles the print statements. - if ((timestamp_nanos - wait_timer) >= 1000000000) { - wait_timer = timestamp_nanos; // Starts the wait timer. - - // Prints out temperature and the current timestamp. - CAL_DEBUG_LOG("[OVER_TEMP_CAL:TEMP]", - "Temperature|Time [C|nsec] = %s%d.%06d, %llu", - CAL_ENCODE_FLOAT(temperature_celsius, 6), - (unsigned long long int)timestamp_nanos); - } -#endif // OVERTEMPCAL_DBG_LOG_TEMP -#endif // OVERTEMPCAL_DBG_ENABLED - - // Updates the sensor temperature. - over_temp_cal->temperature_celsius = temperature_celsius; - - // If any of the models for the sensor axes are in an initial state, then - // this searches for the sensor offset estimate closest to the current - // temperature. A timer is used to limit the rate at which this search is - // performed. - if (over_temp_cal->num_model_pts > 1 && - (over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE || - over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE || - over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) && - (timestamp_nanos - over_temp_cal->nearest_search_timer) >= - OVERTEMPCAL_NEAREST_NANOS) { - setNearestEstimate(over_temp_cal); - over_temp_cal->nearest_search_timer = timestamp_nanos; // Reset timer. - } -} - -void getModelError(const struct OverTempCal *over_temp_cal, - const float *temp_sensitivity, const float *sensor_intercept, - float *max_error) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); - ASSERT_NOT_NULL(max_error); - - size_t i; - size_t j; - float max_error_test; - memset(max_error, 0, 3 * sizeof(float)); - - for (i = 0; i < over_temp_cal->num_model_pts; i++) { - for (j = 0; j < 3; j++) { - max_error_test = - NANO_ABS(over_temp_cal->model_data[i].offset[j] - - getCompensatedOffset(over_temp_cal->model_data[i].offset_temp, - temp_sensitivity[j], sensor_intercept[j])); - if (max_error_test > max_error[j]) { - max_error[j] = max_error_test; - } - } - } -} - -/////// LOCAL HELPER FUNCTION DEFINITIONS ///////////////////////////////////// - -void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, - float offset_temp, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - - // Sets the latest over-temp calibration estimate. - over_temp_cal->latest_offset->offset[0] = offset[0]; - over_temp_cal->latest_offset->offset[1] = offset[1]; - over_temp_cal->latest_offset->offset[2] = offset[2]; - over_temp_cal->latest_offset->offset_temp = offset_temp; - over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos; -} - -void computeModelUpdate(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); - - // The rules for determining whether a new model fit is computed are: - // 1) A minimum number of data points must have been collected: - // num_model_pts >= min_num_model_pts - // NOTE: Collecting 'num_model_pts' and given that only one point is - // kept per temperature bin (spanning a thermal range specified by - // 'delta_temp_per_bin'), implies that model data covers at least, - // model_temp_span >= 'num_model_pts' * delta_temp_per_bin - // 2) New model updates will not occur for intervals less than: - // (current_timestamp_nanos - modelupdate_timestamp_nanos) < - // min_update_interval_nanos - if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts || - (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) < - over_temp_cal->min_update_interval_nanos) { - return; - } - - // Updates the linear model fit. - float temp_sensitivity[3]; - float sensor_intercept[3]; - updateModel(over_temp_cal, temp_sensitivity, sensor_intercept); - - // Computes the maximum error over all of the model data. - float max_error[3]; - getModelError(over_temp_cal, temp_sensitivity, sensor_intercept, max_error); - - // 3) A new set of model parameters are accepted if: - // i. The model fit error is less than, 'max_error_limit'. See - // getModelError() for error metric description. - // ii. The model fit parameters must be within certain absolute - // bounds: - // a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit - // b. NANO_ABS(sensor_intercept) < sensor_intercept_limit - size_t i; - for (i = 0; i < 3; i++) { - if (max_error[i] < over_temp_cal->max_error_limit && - NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit && - NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) { - over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; - over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; - } else { -#ifdef OVERTEMPCAL_DBG_ENABLED - CAL_DEBUG_LOG( - "[OVER_TEMP_CAL:REJECT]", - "Rejected %c-Axis Parameters|Max Error [mdps/C|mdps|mdps] = " - "%s%d.%06d, %s%d.%06d, %s%d.%06d", - kDebugAxisLabel[i], - CAL_ENCODE_FLOAT(temp_sensitivity[i] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(sensor_intercept[i] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6)); -#endif // OVERTEMPCAL_DBG_ENABLED - } - } - - // Resets the timer and sets the update flag. - over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos; - over_temp_cal->new_overtemp_model_available = true; - - // Track the total number of model updates, and set trigger to print log data. -#ifdef OVERTEMPCAL_DBG_ENABLED - over_temp_cal->debug_num_model_updates++; - over_temp_cal->debug_print_trigger |= true; -#endif // OVERTEMPCAL_DBG_ENABLED -} - -void setNearestEstimate(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); - - size_t i = 0; - float dtemp_new = 0.0f; - float dtemp_old = FLT_MAX; - struct OverTempCalDataPt *nearest_estimate = &over_temp_cal->model_data[0]; - for (i = 1; i < over_temp_cal->num_model_pts; i++) { - dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp - - over_temp_cal->temperature_celsius); - if (dtemp_new < dtemp_old) { - nearest_estimate = &over_temp_cal->model_data[i]; - dtemp_old = dtemp_new; - } - } - - // Set the 'latest_offset' to the estimate nearest the current temperature. - over_temp_cal->latest_offset = nearest_estimate; -} - -bool removeStaleModelData(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); - - size_t i; - bool removed_one = false; - for (i = 0; i < over_temp_cal->num_model_pts; i++) { - if ((timestamp_nanos - over_temp_cal->model_data[i].timestamp_nanos) > - over_temp_cal->age_limit_nanos) { - removed_one |= removeModelDataByIndex(over_temp_cal, i); - } - } - - // Updates the latest offset so that it is the one nearest to the current - // temperature. - setNearestEstimate(over_temp_cal); - - return removed_one; -} - -bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, - size_t model_index) { - ASSERT_NOT_NULL(over_temp_cal); - - // This function will not remove all of the model data. At least one model - // sample will be left. - if (over_temp_cal->num_model_pts <= 1) { - return false; - } - - // Remove the model data at 'model_index'. - size_t i; - for (i = model_index; i < over_temp_cal->num_model_pts - 1; i++) { - memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1], - sizeof(struct OverTempCalDataPt)); - } - over_temp_cal->num_model_pts--; - -#ifdef OVERTEMPCAL_DBG_ENABLED - CAL_DEBUG_LOG("[OVER_TEMP_CAL:REMOVE]", - "Removed Stale Data: Model Index = %lu", - (unsigned long int)model_index); -#endif // OVERTEMPCAL_DBG_ENABLED - - return true; -} - -bool jumpStartModelData(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT(over_temp_cal->delta_temp_per_bin > 0); - - // Prevent a divide by zero below. - if (over_temp_cal->delta_temp_per_bin <= 0) { - return false; - } - - // In normal operation the offset estimates enter into the 'model_data' array - // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart - // data produced here requires that the model parameters have all been fully - // defined (i.e., no models in an initial state) and are all within the valid - // range (this is assumed to have been checked prior to this function). There - // must also be no preexisting model data; that is, this function will not - // replace any actual offset estimates already buffered. - if (over_temp_cal->num_model_pts > 0 || - over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE || - over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE || - over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) { - return false; - } - - // This defines the minimum contiguous set of points to allow a model update - // when the next offset estimate is received. They are placed at a common - // temperature range that is likely to get replaced with actual data soon. - const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS / - over_temp_cal->delta_temp_per_bin); - float offset_temp = - start_bin_num * (1.5f * over_temp_cal->delta_temp_per_bin); - - size_t i; - size_t j; - for (i = 0; i < over_temp_cal->min_num_model_pts; i++) { - float offset[3]; - const uint64_t timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos; - for (j = 0; j < 3; j++) { - offset[j] = - getCompensatedOffset(offset_temp, over_temp_cal->temp_sensitivity[j], - over_temp_cal->sensor_intercept[j]); - } - over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; - setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos); - offset_temp += over_temp_cal->delta_temp_per_bin; - over_temp_cal->num_model_pts++; - } - -#ifdef OVERTEMPCAL_DBG_ENABLED - if (over_temp_cal->min_num_model_pts > 0) { - CAL_DEBUG_LOG("[OVER_TEMP_CAL]", "Jump-Started Model: #Points = %lu.", - (unsigned long int)over_temp_cal->min_num_model_pts); - } -#endif // OVERTEMPCAL_DBG_ENABLED - - return (over_temp_cal->min_num_model_pts > 0); -} - -void updateModel(const struct OverTempCal *over_temp_cal, - float *temp_sensitivity, float *sensor_intercept) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); - ASSERT(over_temp_cal->num_model_pts > 0); - - float st = 0.0f, stt = 0.0f; - float sx = 0.0f, stsx = 0.0f; - float sy = 0.0f, stsy = 0.0f; - float sz = 0.0f, stsz = 0.0f; - const size_t n = over_temp_cal->num_model_pts; - size_t i = 0; - - // First pass computes the mean values. - for (i = 0; i < n; ++i) { - st += over_temp_cal->model_data[i].offset_temp; - sx += over_temp_cal->model_data[i].offset[0]; - sy += over_temp_cal->model_data[i].offset[1]; - sz += over_temp_cal->model_data[i].offset[2]; - } - - // Second pass computes the mean corrected second moment values. - const float inv_n = 1.0f / n; - for (i = 0; i < n; ++i) { - const float t = over_temp_cal->model_data[i].offset_temp - st * inv_n; - stt += t * t; - stsx += t * over_temp_cal->model_data[i].offset[0]; - stsy += t * over_temp_cal->model_data[i].offset[1]; - stsz += t * over_temp_cal->model_data[i].offset[2]; - } - - // Calculates the linear model fit parameters. - ASSERT(stt > 0); - const float inv_stt = 1.0f / stt; - temp_sensitivity[0] = stsx * inv_stt; - sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_n; - temp_sensitivity[1] = stsy * inv_stt; - sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_n; - temp_sensitivity[2] = stsz * inv_stt; - sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n; -} - -void removeSensorOffset(const struct OverTempCal *over_temp_cal, float vi, - size_t index, float *vo) { - // Removes the over-temperature compensated offset from the input sensor data. - if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE) { - // If this axis is in its initial state, use the nearest estimate to perform - // the compensation (in this case the latest estimate will be the nearest): - // sensor_out = sensor_in - nearest_offset - *vo = vi - over_temp_cal->latest_offset->offset[index]; - } else { - // sensor_out = sensor_in - compensated_offset - // Where, - // compensated_offset = (temp_sensitivity * temp_meas + sensor_intercept) - *vo = vi - getCompensatedOffset(over_temp_cal->temperature_celsius, - over_temp_cal->temp_sensitivity[index], - over_temp_cal->sensor_intercept[index]); - } -} - -float getCompensatedOffset(float temperature, float temp_sensitivity, - float sensor_intercept) { - return temp_sensitivity * temperature + sensor_intercept; -} - -/////// DEBUG FUNCTION DEFINITIONS //////////////////////////////////////////// - -#ifdef OVERTEMPCAL_DBG_ENABLED -// Debug printout state enumeration. -enum DebugState { - IDLE = 0, - WAIT_STATE, - PRINT_HEADER, - PRINT_OFFSET, - PRINT_SENSITIVITY, - PRINT_INTERCEPT, - PRINT_ERROR, - PRINT_MODEL_PTS, - PRINT_MODEL_DATA -}; - -void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->latest_offset); - - static enum DebugState debug_state = IDLE; - static enum DebugState next_state = 0; - static uint64_t wait_timer = 0; - static size_t i = 0; // Counter. - - // NOTE - The un-initialized model state is indicated by - // temp_sensitivity=MODEL_INITIAL_STATE. The following filters out this - // condition for the data printout below. - float compensated_offset[3]; - float temp_sensitivity[3]; - float max_error[3]; - size_t j; - for (j = 0; j < 3; j++) { - temp_sensitivity[j] = - (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE) - ? 0.0f - : over_temp_cal->temp_sensitivity[j]; - } - - // This is a state machine that controls the reporting out of debug data. - switch (debug_state) { - case IDLE: - // Wait for a trigger and start the debug printout sequence. - if (over_temp_cal->debug_print_trigger) { - debug_state = PRINT_HEADER; - CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, ""); - over_temp_cal->debug_print_trigger = false; // Resets trigger. - } else { - debug_state = IDLE; - } - break; - - case PRINT_HEADER: - // Print out header. - CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s", - OVERTEMPCAL_DEBUG_VERSION_STRING); - - // Prints out number of offsets. - CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Total Offsets = %lu", - (unsigned long int)over_temp_cal->debug_num_estimates); - - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_OFFSET; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case PRINT_OFFSET: - // Computes the compensated sensor offset based on the current - // temperature. - for (j = 0; j < 3; j++) { - compensated_offset[j] = - (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE) - ? over_temp_cal->latest_offset->offset[j] - : getCompensatedOffset(over_temp_cal->temperature_celsius, - over_temp_cal->temp_sensitivity[j], - over_temp_cal->sensor_intercept[j]); - } - - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, - "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, " - "%s%d.%06d, %s%d.%06d, %llu", - CAL_ENCODE_FLOAT(compensated_offset[0] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(compensated_offset[1] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(compensated_offset[2] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(over_temp_cal->temperature_celsius, 6), - (unsigned long long int) - over_temp_cal->latest_offset->timestamp_nanos); - - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_SENSITIVITY; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case WAIT_STATE: - // This helps throttle the print statements. - if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) { - debug_state = next_state; - } - break; - - case PRINT_SENSITIVITY: - // Prints out the modeled temperature sensitivity. - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, - "Modeled Temperature Sensitivity [mdps/C] = %s%d.%06d, %s%d.%06d, " - "%s%d.%06d", - CAL_ENCODE_FLOAT(temp_sensitivity[0] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(temp_sensitivity[1] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(temp_sensitivity[2] * RAD_TO_MILLI_DEGREES, 6)); - - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_INTERCEPT; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case PRINT_INTERCEPT: - // Prints out the modeled temperature intercept. - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, - "Modeled Temperature Intercept [mdps] = %s%d.%06d, %s%d.%06d, " - "%s%d.%06d", - CAL_ENCODE_FLOAT( - over_temp_cal->sensor_intercept[0] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT( - over_temp_cal->sensor_intercept[1] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT( - over_temp_cal->sensor_intercept[2] * RAD_TO_MILLI_DEGREES, 6)); - - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_ERROR; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case PRINT_ERROR: - // Computes the maximum error over all of the model data. - if (over_temp_cal->num_model_pts > 0) { - getModelError(over_temp_cal, temp_sensitivity, - over_temp_cal->sensor_intercept, max_error); - - // Reports the resulting model error. - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, - "Model Error [mdps] = %s%d.%06d, %s%d.%06d, %s%d.%06d", - CAL_ENCODE_FLOAT(max_error[0] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(max_error[1] * RAD_TO_MILLI_DEGREES, 6), - CAL_ENCODE_FLOAT(max_error[2] * RAD_TO_MILLI_DEGREES, 6)); - } - - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_MODEL_PTS; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case PRINT_MODEL_PTS: - // Prints out the number of model points/updates. - CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Points = %lu", - (unsigned long int)over_temp_cal->num_model_pts); - - CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Updates = %lu", - (unsigned long int)over_temp_cal->debug_num_model_updates); - - i = 0; // Resets the counter. - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_MODEL_DATA; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - break; - - case PRINT_MODEL_DATA: - // Prints out all of the model data. - if (i < over_temp_cal->num_model_pts) { - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, - " Model[%lu] [mdps|C] = %s%d.%06d, %s%d.%06d, %s%d.%06d, " - "%s%d.%03d ", - (unsigned long int)i, - CAL_ENCODE_FLOAT( - over_temp_cal->model_data[i].offset[0] * RAD_TO_MILLI_DEGREES, - 6), - CAL_ENCODE_FLOAT( - over_temp_cal->model_data[i].offset[1] * RAD_TO_MILLI_DEGREES, - 6), - CAL_ENCODE_FLOAT( - over_temp_cal->model_data[i].offset[2] * RAD_TO_MILLI_DEGREES, - 6), - CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp, 3)); - - i++; - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = PRINT_MODEL_DATA; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - } else { - debug_state = IDLE; // Goes to idle state. - CAL_DEBUG_LOG( - OVERTEMPCAL_REPORT_TAG, "Last Model Update [nsec] = %llu", - (unsigned long long int)over_temp_cal->modelupdate_timestamp_nanos); - } - break; - - default: - // Sends this state machine to its idle state. - wait_timer = timestamp_nanos; // Starts the wait timer. - next_state = IDLE; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. - } -} - -#endif // OVERTEMPCAL_DBG_ENABLED diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h deleted file mode 100644 index 928576f0..00000000 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h +++ /dev/null @@ -1,327 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* - * This module provides an online algorithm for compensating a 3-axis sensor's - * offset over its operating temperature: - * - * 1) Estimates of sensor offset with associated temperature are consumed, - * {offset, offset_temperature}. - * 2) A temperature dependence model is extracted from the collected set of - * data pairs. - * 3) Until a "complete" model has been built and a model equation has been - * computed, the compensation will use the collected offset nearest in - * temperature. If a model is available, then the compensation will take - * the form of: - * - * Linear Compensation Model Equation: - * sensor_out = sensor_in - compensated_offset - * Where, - * compensated_offset = (temp_sensitivity * current_temp + sensor_intercept) - * - * NOTE - 'current_temp' is the current measured temperature. 'temp_sensitivity' - * is the modeled temperature sensitivity (i.e., linear slope). - * 'sensor_intercept' is linear model intercept. - * - * Assumptions: - * - * 1) Sensor hysteresis is negligible. - * 2) Sensor offset temperature dependence is sufficiently "linear". - * 3) The impact of long-term offset drift/aging compared to the magnitude of - * deviation resulting from the thermal sensitivity of the offset is - * relatively small. - * - * Sensor Input and Units: - * - General 3-axis sensor data. - * - Temperature measurements [Celsius]. - * - * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - * - * #define OVERTEMPCAL_DBG_ENABLED to enable debug printout statements. - * #define OVERTEMPCAL_DBG_LOG_TEMP to periodically printout sensor temperature. - */ - -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ - -#include <stdbool.h> -#include <stddef.h> -#include <stdint.h> - -#ifdef __cplusplus -extern "C" { -#endif - -// Defines the maximum size of the 'model_data' array. -#define OVERTEMPCAL_MODEL_SIZE (20) - -// Over-temperature sensor offset estimate structure. -struct OverTempCalDataPt { - // Sensor offset estimate, temperature, and timestamp. - float offset[3]; - float offset_temp; // [Celsius] - uint64_t timestamp_nanos; // [nanoseconds] - // TODO(davejacobs) - Design option: add variance to provide weighting info. -}; - -// The following data structure contains all of the necessary components for -// modeling a sensor's temperature dependency and providing over-temperature -// offset corrections. -struct OverTempCal { - // Storage for over-temperature model data. - struct OverTempCalDataPt model_data[OVERTEMPCAL_MODEL_SIZE]; - - // Total number of model data points collected. - size_t num_model_pts; - - // Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius]. - float temp_sensitivity[3]; - - // Sensor model equation intercept [sensor_units]. - float sensor_intercept[3]; - - // Timestamp of the last model update. - uint64_t modelupdate_timestamp_nanos; // [nanoseconds] - - // The temperature at which the offset compensation is performed. - float temperature_celsius; - - // Pointer to the most recent sensor offset estimate. This is also updated - // periodically to point to the offset estimate closest to the current sensor - // temperature. - struct OverTempCalDataPt *latest_offset; - - ///// Online Model Identification Parameters //////////////////////////////// - // - // The rules for determining whether a new model fit is computed and the - // resulting fit parameters are accepted are: - // 1) A minimum number of data points must have been collected: - // num_model_pts >= min_num_model_pts - // NOTE: Collecting 'num_model_pts' and given that only one point is - // kept per temperature bin (spanning a thermal range specified by - // 'delta_temp_per_bin'), implies that model data covers at least, - // model_temp_span >= 'num_model_pts' * delta_temp_per_bin - // 2) New model updates will not occur for intervals less than: - // (current_timestamp_nanos - modelupdate_timestamp_nanos) < - // min_update_interval_nanos - // 3) A new set of model parameters are accepted if: - // i. The model fit error is less than, 'max_error_limit'. See - // getModelError() for error metric description. - // ii. The model fit parameters must be within certain absolute - // bounds: - // a. ABS(temp_sensitivity) < temp_sensitivity_limit - // b. ABS(sensor_intercept) < sensor_intercept_limit - size_t min_num_model_pts; - uint64_t min_update_interval_nanos; // [nanoseconds] - float max_error_limit; // [sensor units] - float temp_sensitivity_limit; // [sensor units/Celsius] - float sensor_intercept_limit; // [sensor units] - - // The rules for accepting new offset estimates into the 'model_data' - // collection: - // 1) The temperature domain is divided into bins each spanning - // 'delta_temp_per_bin'. - // 2) Find and replace the i'th 'model_data' estimate data if: - // Let, bin_num = floor(current_temp / delta_temp_per_bin) - // temp_lo_check = bin_num * delta_temp_per_bin - // temp_hi_check = (bin_num + 1) * delta_temp_per_bin - // Check condition: - // temp_lo_check <= model_data[i].offset_temp < temp_hi_check - // 3) If nothing was replaced, and the 'model_data' buffer is not full then - // add the sensor offset estimate to the array. - // 4) Otherwise (nothing was replaced and buffer is full), replace the - // 'latest_offset' with the incoming one. - // This approach ensures a uniform spread of collected data, keeps the most - // recent estimates in cases where they arrive frequently near a given - // temperature, and prevents model oversampling (i.e., dominance of estimates - // concentrated at given set of temperatures). - float delta_temp_per_bin; // [Celsius/bin] - - // Timer used to limit the rate at which a search for the nearest offset - // estimate is performed. - uint64_t nearest_search_timer; // [nanoseconds] - - // Timer used to limit the rate at which old estimates are removed from - // the 'model_data' collection. - uint64_t stale_data_timer; // [nanoseconds] - - // Duration beyond which data will be removed to avoid corrupting the model - // with drift-compromised data. - uint64_t age_limit_nanos; // [nanoseconds] - - // Flag set by user to control whether over-temp compensation is used. - bool over_temp_enable; - - // True when new compensation model values have been computed; and reset when - // overTempCalNewModelUpdateAvailable() is called. This variable indicates - // that the following should be stored/updated in persistent system memory: - // 1) 'temp_sensitivity' and 'sensor_intercept'. - // 2) The sensor offset data pointed to by 'latest_offset' - // (saving timestamp information is not required). - bool new_overtemp_model_available; - -#ifdef OVERTEMPCAL_DBG_ENABLED - size_t debug_num_model_updates; // Total number of model updates. - size_t debug_num_estimates; // Total number of offset estimates. - bool debug_print_trigger; // Flag used to trigger data printout. -#endif // OVERTEMPCAL_DBG_ENABLED -}; - -/////// FUNCTION PROTOTYPES /////////////////////////////////////////////////// - -/* - * Initializes the over-temp calibration model identification parameters. - * - * INPUTS: - * over_temp_cal: Over-temp main data structure. - * min_num_model_pts: Minimum number of model points per model - * calculation update. - * min_update_interval_nanos: Minimum model update interval. - * delta_temp_per_bin: Temperature span that defines the spacing of - * collected model estimates. - * max_error_limit: Model acceptance fit error tolerance. - * age_limit_nanos: Sets the age limit beyond which a offset - * estimate is removed from 'model_data'. - * temp_sensitivity_limit: Values that define the upper limits for the - * sensor_intercept_limit: model parameters. The acceptance of new model - * parameters must satisfy: - * i. ABS(temp_sensitivity) < temp_sensitivity_limit - * ii. ABS(sensor_intercept) < sensor_intercept_limit - * over_temp_enable: Flag that determines whether over-temp sensor - * offset compensation is applied. - */ -void overTempCalInit(struct OverTempCal *over_temp_cal, - size_t min_num_model_pts, - uint64_t min_update_interval_nanos, - float delta_temp_per_bin, float max_error_limit, - uint64_t age_limit_nanos, float temp_sensitivity_limit, - float sensor_intercept_limit, bool over_temp_enable); - -/* - * Sets the over-temp calibration model parameters. - * - * INPUTS: - * over_temp_cal: Over-temp main data structure. - * offset: Update values for the latest offset estimate (array). - * offset_temp: Measured temperature for the offset estimate. - * timestamp_nanos: Timestamp for the offset estimate [nanoseconds]. - * temp_sensitivity: Modeled temperature sensitivity (array). - * sensor_intercept: Linear model intercept for the over-temp model (array). - * jump_start_model: When 'true' populates an empty 'model_data' array using - * valid input model parameters. - * - * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - */ -void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, - float offset_temp, uint64_t timestamp_nanos, - const float *temp_sensitivity, - const float *sensor_intercept, bool jump_start_model); - -/* - * Gets the over-temp calibration model parameters. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * OUTPUTS: - * offset: Offset values for the latest offset estimate (array). - * offset_temp: Measured temperature for the offset estimate. - * timestamp_nanos: Timestamp for the offset estimate [nanoseconds]. - * temp_sensitivity: Modeled temperature sensitivity (array). - * sensor_intercept: Linear model intercept for the over-temp model (array). - * - * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - */ -void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, - float *offset_temp, uint64_t *timestamp_nanos, - float *temp_sensitivity, float *sensor_intercept); - -/* - * Removes the over-temp compensated offset from the input sensor data. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * timestamp_nanos: Timestamp of the sensor estimate update. - * xi, yi, zi: 3-axis sensor data to be compensated. - * OUTPUTS: - * xo, yo, zo: 3-axis sensor data that has been compensated. - */ -void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, float xi, float yi, - float zi, float *xo, float *yo, float *zo); - -// Returns true when a new over-temp model update is available; and the -// 'new_overtemp_model_available' flag is reset. -bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal); - -/* - * Updates the sensor's offset estimate and conditionally assimilates it into - * the over-temp model data set, 'model_data'. - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * timestamp_nanos: Timestamp of the sensor estimate update. - * offset: 3-axis sensor data to be compensated (array). - * temperature_celsius: Measured temperature for the new sensor estimate. - * - * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - */ -void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, - const float *offset, - float temperature_celsius); - -// Updates the temperature at which the offset compensation is performed (i.e., -// the current measured temperature value). This function is provided mainly for -// flexibility since temperature updates may come in from a source other than -// the sensor itself, and at a different rate. -void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, - float temperature_celsius); - -/* - * Computes the maximum absolute error between the 'model_data' estimates and - * the estimate determined by the input model parameters. - * max_error (over all i) - * |model_data[i]->offset_xyz - - * getCompensatedOffset(model_data[i]->offset_temp, temp_sensitivity, - * sensor_intercept)| - * - * INPUTS: - * over_temp_cal: Over-temp data structure. - * temp_sensitivity: Model temperature sensitivity to test (array). - * sensor_intercept: Model intercept to test (array). - * OUTPUTS: - * max_error: Maximum absolute error for the candidate model (array). - * - * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. - * NOTE 2: This function is provided for testing purposes. - */ -void getModelError(const struct OverTempCal *over_temp_cal, - const float *temp_sensitivity, const float *sensor_intercept, - float *max_error); - -#ifdef OVERTEMPCAL_DBG_ENABLED -// This debug printout function assumes the input sensor data is a gyroscope -// [rad/sec]. -void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos); -#endif // OVERTEMPCAL_DBG_ENABLED - -#ifdef __cplusplus -} -#endif - -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ |