| /* |
| * 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 (40) |
| |
| // The maximum number of successive outliers that may be rejected. |
| #define OVERTEMPCAL_MAX_OUTLIER_COUNT (3) |
| |
| // Over-temperature sensor offset estimate structure. |
| struct OverTempCalDataPt { |
| // Sensor offset estimate, temperature, and timestamp. |
| float offset[3]; |
| float offset_temp_celsius; // [Celsius] |
| uint64_t timestamp_nanos; // [nanoseconds] |
| }; |
| |
| #ifdef OVERTEMPCAL_DBG_ENABLED |
| // Debug printout state enumeration. |
| enum OverTempCalDebugState { |
| OTC_IDLE = 0, |
| OTC_WAIT_STATE, |
| OTC_PRINT_OFFSET, |
| OTC_PRINT_MODEL_PARAMETERS, |
| OTC_PRINT_MODEL_ERROR, |
| OTC_PRINT_MODEL_DATA |
| }; |
| |
| // OverTempCal debug information/data tracking structure. |
| struct DebugOverTempCal { |
| uint64_t modelupdate_timestamp_nanos; |
| |
| // The most recent offset estimate received. |
| struct OverTempCalDataPt latest_offset; |
| |
| // The offset estimate nearest the current sensor temperature. |
| struct OverTempCalDataPt nearest_offset; |
| |
| // The maximum model error over all model_data points. |
| float max_error[3]; |
| |
| float temp_sensitivity[3]; |
| float sensor_intercept[3]; |
| float temperature_celsius; |
| size_t num_model_pts; |
| }; |
| #endif // OVERTEMPCAL_DBG_ENABLED |
| |
| // 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 offset estimate closest to the current sensor temperature. |
| struct OverTempCalDataPt *nearest_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 number of successive outliers rejected in a row. This is used to |
| // prevent the possibility of a bad state where an initial bad fit causes |
| // good data to be continually rejected. |
| size_t num_outliers; |
| |
| // 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_celsius < 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 |
| // oldest data 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 a 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 'nearest_offset' |
| // (saving timestamp information is not required). |
| bool new_overtemp_model_available; |
| |
| #ifdef OVERTEMPCAL_DBG_ENABLED |
| struct DebugOverTempCal debug_overtempcal; // Debug data structure. |
| enum OverTempCalDebugState debug_state; // Debug printout state machine. |
| 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_celsius: 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_celsius, 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_celsius: 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_celsius, 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_celsius, |
| * 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_ |
