diff options
Diffstat (limited to 'firmware/os/algos/calibration/online_calibration')
12 files changed, 1291 insertions, 0 deletions
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc new file mode 100644 index 00000000..e9def4e6 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2018 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/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +void AccelOffsetCal::Initialize( + const AccelCalParameters& accel_cal_parameters) { + accelCalInit(&accel_cal_, &accel_cal_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + switch (sample.type) { + case SensorType::kAccelerometerMps2: + accelCalRun(&accel_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis], // [m/sec^2] + temperature_celsius_); + +#ifdef ACCEL_CAL_DBG_ENABLED + // Prints debug data report. + accelCalDebPrint(&accel_cal_, temperature_celsius_); +#endif + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new offset estimate, and updates the calibration data. + if (accelCalNewBiasAvailable(&accel_cal_)) { + accelCalUpdateBias(&accel_cal_, &cal_data_.offset[0], &cal_data_.offset[1], + &cal_data_.offset[2]); + + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality; + cal_data_.offset_temp_celsius = temperature_celsius_; + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_polling_flags_ = CalibrationTypeFlags::BIAS; + OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS); + } + + return cal_update_polling_flags_; +} + +bool AccelOffsetCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[AccelOffsetCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sets the accelerometer algorithm's calibration data. + accelCalBiasSet(&accel_cal_, input_cal_data.offset[0], + input_cal_data.offset[1], input_cal_data.offset[2]); + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the calibration quality. + cal_data_.calibration_quality.level = CalibrationQualityLevel::LOW_QUALITY; + cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality; + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h new file mode 100644 index 00000000..0256495a --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2018 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. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ + +#include "calibration/accelerometer/accel_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This class is a wrapper for accelerometer offset calibration. + * + * NOTE: Calibration quality reporting (quantitative metric is not used): + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::LOW_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + */ +class AccelOffsetCal final + : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Default constructor. + AccelOffsetCal() = default; + + // Creates an AccelOffsetCal with specified algorithm parameters. + explicit AccelOffsetCal(const AccelCalParameters& accel_cal_parameters) { + Initialize(accel_cal_parameters); + } + + // Initializes with specified algorithm parameters, and resets the calibration + // data. + void Initialize(const AccelCalParameters& accel_cal_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kAccelerometerMps2; + }; + + private: + // Accelerometer offset calibration algorithm data structure. + AccelCal accel_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h new file mode 100644 index 00000000..62bc443d --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2018 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 the callback functionality employed by the online sensor + * calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ + +#include "calibration/online_calibration/common_data/calibration_data.h" + +namespace online_calibration { + +/* + * This codebase avoids Standard Template Library (STL) containers to maximize + * its usefullness on embedded hardware with basic C++ compiler support. The + * following uses type erasure to implement callback functionality to a user's + * desired class method. The idea here is to store an object pointer by + * instantiating a class template (Callback) which implements a virtual + * interface function (CallbackInterface::Call). + * + * USAGE: + * See unit testing for a simple example of how to use this for callback + * functionality. + */ + +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class CalibrationType> +class CallbackInterface { + public: + // Interface function that is defined to implement the desired callback. + virtual void Call(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags) = 0; + virtual ~CallbackInterface() {} +}; + +// ClassCallerType: Sets the object's class type for the callback. +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class ClassCallerType, class CalibrationType> +class Callback : public CallbackInterface<CalibrationType> { + public: + // Constructors. + Callback() = delete; + Callback(ClassCallerType* obj, + void (ClassCallerType::*func)(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags)) + : object_ptr_(obj), function_ptr_(func) {} + + // Implements the callback to the desired class-method. + void Call(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags) final { + (object_ptr_->*function_ptr_)(cal_data, cal_update_flags); + } + + private: + // Pointer to the class that owns the callback method. + ClassCallerType* object_ptr_; + + // Templated function pointer with the required function signature. + // Calibration callbacks must accept: + // 1. Constant reference to the calibration. + // 2. Bitmask indicating which calibration components have been updated. + void (ClassCallerType::*function_ptr_)(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags); +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc new file mode 100644 index 00000000..8f1470a8 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2018 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/online_calibration/common_data/calibration_data.h" + +namespace online_calibration { + +CalibrationTypeFlags operator|(CalibrationTypeFlags lhs, + CalibrationTypeFlags rhs) { + return static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) | + static_cast<char>(rhs)); +} + +bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs) { + return static_cast<char>(lhs) & static_cast<char>(rhs); +} + +CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs, + CalibrationTypeFlags rhs) { + lhs = static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) | + static_cast<char>(rhs)); + return lhs; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h new file mode 100644 index 00000000..0404936b --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h @@ -0,0 +1,228 @@ +/* + * Copyright (C) 2018 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 the component definitions used to represent sensor + * calibration data, labeled flags/enumerators, and the callback functionality + * employed by the online sensor calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ + +#include <stdint.h> +#include <string.h> +#include <sys/types.h> + +#include "calibration/online_calibration/common_data/calibration_quality.h" +#include "calibration/online_calibration/common_data/sensor_data.h" +#include "calibration/over_temp/over_temp_model.h" + +namespace online_calibration { + +/* + * Bitmask used to indicate which calibration values have changed for a given + * calibration update. + * + * [Bit Flag] | [Affected Sensor Calibration Value] + * BIAS - Quasi-static non-zero sensor output (offset), given + * conditions where the sensor should ideally output zero. + * Includes corrections for over-temperature compensation. + * SCALE_FACTOR - Sensor axis scaling (ideally unity). + * CROSS_AXIS - Output sensitivity due to variations of perpendicular + * sensing axes (ideally zero). + * OVER_TEMP - Model parameters that capture variations in sensor + * behavior with temperature (e.g., linear bias sensitivity + * model). + * QUALITY_DEGRADED - Indicates a degradation in calibration quality. + */ +enum class CalibrationTypeFlags : uint8_t { + NONE = 0x00, + BIAS = 0x01, + SCALE_FACTOR = 0x02, + CROSS_AXIS = 0x04, + OVER_TEMP = 0x08, + QUALITY_DEGRADED = 0x10, + ALL = 0xFF +}; + +// Logic operators to assist with common bitmask setting/checking. +CalibrationTypeFlags operator|(CalibrationTypeFlags lhs, + CalibrationTypeFlags rhs); + +bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs); + +CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs, + CalibrationTypeFlags rhs); + +/* + * Defines the calibration data specific to a prototypical three-axis sensing + * device (e.g., accelerometer, gyroscope, magnetometer). + * + * Calibration correction may be applied as: + * corrected_data = scale_skew_matrix * (input_data - offset); + * + * 'offset' is the sensor bias estimate (with temperature compensation applied + * when supported by the underlying calibration algorithm). + * + * The 'scale_skew_matrix' is assumed to be in lower diagonal form where the + * sensor frame reference definition is such that cross-axis sensitivities + * cross_axis_xy, cross_axis_xz, and cross_axis_yz are set to zero. + * + * scale_skew_matrix = [scale_factor_x 0 0 + * cross_axis_yx scale_factor_y 0 + * cross_axis_zx cross_axis_zy scale_factor_z] + * + * NOTE1: If over-temperature compensation is provided, then temperature + * compensation is already applied to the 'offset'. Coefficients representative + * of the sensor's temperature dependency model are provided, and may be used + * for model initialization after a system restart. + * + * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope). + * temp_intercept - Linear model intercept. + * + * The model equation for the over-temperature compensated offset: + * compensated_offset = temp_sensitivity * current_temp + temp_intercept + * + * NOTE2: Unless otherwise stated, 3-dimensional array indices: 0=x, 1=y, 2=z. + */ +struct CalibrationDataThreeAxis { + // Timestamp for the most recent calibration update. + uint64_t cal_update_time_nanos = 0; + + // The sensor's offset (i.e., bias) in the x-, y-, z-axes at + // 'offset_temp_celsius'. + float offset[3]; + + // The temperature associated with the sensor offset. + float offset_temp_celsius; + + // The temperature sensitivity of the offset. + float temp_sensitivity[3]; // [sensor_units/Celsius] + + // The sensor's offset at zero degrees Celsius. + float temp_intercept[3]; // [sensor_units] + + // The sensor's scale factor for each axis. + float scale_factor[3]; + + // The cross-axis factors in order: [0=yx, 1=zx, 2=zy]. + float cross_axis[3]; + + // Metrics used for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + CalibrationQuality calibration_quality; + + // Indicates the type of sensing device being calibrated. + SensorType type = SensorType::kUndefined; + + // Optional pointer to an array of over-temperature model data (null when not + // used). For initialization, populating a model dataset will take precedence + // over the linear model parameters provided in the calibration data. + OverTempModelThreeAxis* otc_model_data = nullptr; + int16_t num_model_pts = 0; + + // Helper function that resets the calibration data to a set of neutral + // reference values where no calibration correction would be applied if used. + void reset() { + otc_model_data = nullptr; + calibration_quality.reset(); + offset_temp_celsius = 0.0f; + scale_factor[0] = 1.0f; + scale_factor[1] = 1.0f; + scale_factor[2] = 1.0f; + memset(offset, 0, sizeof(offset)); + memset(temp_sensitivity, 0, sizeof(temp_sensitivity)); + memset(temp_intercept, 0, sizeof(temp_intercept)); + memset(cross_axis, 0, sizeof(cross_axis)); + } + + CalibrationDataThreeAxis() { reset(); } +}; + +/* + * Defines the calibration data for single dimensional sensing device (e.g., + * thermometer, barometer). + * + * Calibration correction may be applied as: + * corrected_data = scale_factor * (input_data - offset); + * + * 'offset' is the sensor bias estimate (with temperature compensation applied, + * if supported by the underlying calibration algorithm). + * + * NOTE: If over-temperature compensation is provided, then temperature + * compensation is already applied to the 'offset'. Coefficients representative + * of the sensor's temperature dependency model are provided, and may be used + * for model initialization after a system restart. + * + * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope). + * temp_intercept - Linear model intercept. + * + * The model equation for the over-temperature compensated offset: + * compensated_offset = temp_sensitivity * current_temp + temp_intercept + */ +struct CalibrationDataSingleAxis { + // Timestamp for the most recent calibration update. + uint64_t cal_update_time_nanos = 0; + + // The sensor's offset (i.e., bias) at temperature, 'offset_temp_celsius'. + float offset; + + // The temperature associated with the sensor offset. + float offset_temp_celsius; + + // The temperature sensitivity of the offset. + float temp_sensitivity; // [sensor_units/Celsius] + + // The sensor's offset at zero degrees Celsius. + float temp_intercept; // [sensor_units] + + // The sensor's scale factor. + float scale_factor; + + // Metrics used for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + CalibrationQuality calibration_quality; + + // Indicates the type of sensing device being calibrated. + SensorType type = SensorType::kUndefined; + + // Optional pointer to an array of over-temperature model data (null when not + // used). For initialization, populating a model dataset will take precedence + // over the linear model parameters provided in the calibration data. + OverTempModelSingleAxis* otc_model_data = nullptr; + int16_t num_model_pts = 0; + + // Helper function that resets the calibration data to a set of neutral + // reference values where no calibration correction would be applied if used. + void reset() { + otc_model_data = nullptr; + calibration_quality.reset(); + scale_factor = 1.0f; + offset_temp_celsius = 0.0f; + offset = 0.0f; + temp_sensitivity = 0.0f; + temp_intercept = 0.0f; + } + + CalibrationDataSingleAxis() { reset(); } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h new file mode 100644 index 00000000..d6475c78 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2018 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 quality definitions that represent the accuracy + * associated with calibration data. This information may be used to affect how + * a system uses available sensor calibration data. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ + +#include <float.h> +#include <stdint.h> + +namespace online_calibration { + +/* + * In general, calibration quality values may be thought of in terms of + * something akin to an error standard deviation. That is, it's a non-negative + * value where lower values imply higher calibration quality, and larger values + * indicate poorer quality. However, the units and numerical interpretation is + * ultimately dictated by the sensor type and calibration algorithm. Consult the + * calibration code implementation for the actual calibration quality metric + * details. + */ + +/* + * Bitmask used to provide a qualitative ranking of the calibration data's + * accuracy. Many systems use this sort of interpretation for calibration + * quality (e.g., Android). + * + * [Bit Flag] | [Qualitative Calibration Quality] + * UNDETERMINED - Calibration quality has not yet been determined (e.g., a + * calibration hasn't occurred, or a metric hasn't been + * established). + * LOW_QUALITY - Sensor calibration is needed. System properties have + * changed that may have affected the applicability of the + * current calibration (e.g., calibration data is old, anomaly + * detected, etc.). + * MEDIUM_QUALITY - The reported sensor calibration has an average level of + * accuracy, updated calibration may improve the readings. + * HIGH_QUALITY - The reported calibration has maximal accuracy. + */ +enum class CalibrationQualityLevel : uint8_t { + UNDETERMINED = 0x00, + LOW_QUALITY = 0x01, + MEDIUM_QUALITY = 0x02, + HIGH_QUALITY = 0x04, +}; + +// Sets the calibration quality value when this metric is either not +// implemented, or has not yet been determined (e.g., a calibration hasn't +// occurred). +constexpr float kUndeterminedCalibrationQuality = -1.0f; + +/* + * Calibration quality structure that contains a quantitative (float) and + * qualitative (enum) measure of a sensor's calibration accuracy. Both entries + * should be co-defined by the algorithm providing the calibration update. The + * enum sets the qualitative interpretation of the float value, this is often + * used in systems (Android, etc.) to label quality and affect the use of the + * calibration data. + */ +struct CalibrationQuality { + // Provides a qualitative measure for sensor calibration accuracy. + CalibrationQualityLevel level = CalibrationQualityLevel::UNDETERMINED; + + // Quantitative metric for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + float value = kUndeterminedCalibrationQuality; + + // Helper function that resets the calibration quality to an initial state. + void reset() { + level = CalibrationQualityLevel::UNDETERMINED; + value = kUndeterminedCalibrationQuality; + } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h new file mode 100644 index 00000000..59e26bae --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2018 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. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ + +#include <string.h> + +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This abstract base class provides a set of general interface functions for + * calibration algorithms. The data structures used are intended to be lean and + * portable to a wide variety of software and hardware systems. Algorithm + * wrappers may use this as a basis for providing the following functionality: + * + * SetMeasurement - Delivers new sensor data. + * SetInitialCalibration - Initializes the algorithm's calibration data. + * GetSensorCalibration - Retrieves the latest calibration data set. + * new_calibration_ready - Used to poll for new calibration updates. + * SetCalibrationCallback - User provides a pointer its own Callback object. + * UpdateDynamicSystemSettings - Provides feedback to adjust system behavior. + * get_sensor_type - Returns the sensor type which is being calibrated. + * + * NOTE 1: This class accomodates two methods of providing calibration updates. + * Either, or both, may be used depending on system requirements. 1) Polling can + * be achieved with new_calibration_ready/GetSensorCalibration functions. 2) + * Callback notification of new calibration updates can managed using the + * SetCalibrationCallback function. + * + * NOTE 2: This code implementation specifically avoids using standard template + * libraries (STL) and other external API’s since they may not be fully + * supported on embedded hardware targets. Only basic C/C++ support will be + * assumed. + */ + +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class CalibrationType> +class OnlineCalibration { + public: + // Virtual destructor. + virtual ~OnlineCalibration() {} + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0; + + // Sets the initial calibration data of the calibration algorithm. Returns + // "true" if set successfully. + virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0; + + // Polling Updates: New calibration updates are generated during + // SetMeasurement and the 'cal_update_polling_flags_' are set according to + // which calibration values have changed. To prevent missing updates in + // systems that use polling, this bitmask remains latched until the + // calibration data is retrieved with this function. + const CalibrationType& GetSensorCalibration() const { + cal_update_polling_flags_ = CalibrationTypeFlags::NONE; + return cal_data_; + } + + // Polling Updates: This function returns 'cal_update_polling_flags_' to + // indicate which calibration components have a pending update. The updated + // calibration data may be retrieved with GetSensorCalibration, and the + // 'cal_update_polling_flags_' will reset. + CalibrationTypeFlags new_calibration_ready() const { + return cal_update_polling_flags_; + } + + // Sets the pointer to the CallbackInterface object used for notification of + // new calibration updates. + void SetCalibrationCallback( + CallbackInterface<CalibrationType>* calibration_callback) { + calibration_callback_ = calibration_callback; + } + + // Returns the sensor-type this calibration algorithm provides updates for. + virtual SensorType get_sensor_type() const = 0; + + protected: + // Helper function that activates the registered callback. + void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const { + if (calibration_callback_ != nullptr) { + calibration_callback_->Call(cal_data_, cal_update_flags); + } + } + + // Helper function used to initialize the calibration data. + void InitializeCalData() { + cal_data_.reset(); + cal_data_.type = get_sensor_type(); + cal_update_polling_flags_ = CalibrationTypeFlags::NONE; + } + + // Stores the sensor calibration data. + CalibrationType cal_data_; + + // Tracks the most recent sensor temperature value. + float temperature_celsius_ = kInvalidTemperatureCelsius; + + // This bitmask indicates which subset of calibration parameters have changed + // and is used specifically for polling; the callback notification passes its + // own set of update flags which do not need this latching behavior. Marked + // mutable so that these flags may be reset when GetSensorCalibration is + // called. + mutable CalibrationTypeFlags cal_update_polling_flags_ = + CalibrationTypeFlags::NONE; + + private: + // Pointer to a callback object. + CallbackInterface<CalibrationType>* calibration_callback_ = nullptr; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h new file mode 100644 index 00000000..1331153e --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2018 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 the component definitions used to represent sensor + * data employed by the online sensor calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ + +#include <stdint.h> +#include <string.h> +#include <sys/types.h> + +#include "common/math/macros.h" + +namespace online_calibration { + +// Defines an invalid or uninitialized temperature value (referenced from +// common/math/macros.h). +constexpr float kInvalidTemperatureCelsius = INVALID_TEMPERATURE_CELSIUS; + +// Unit conversion from nanoseconds to microseconds. +constexpr uint64_t NanoToMicroseconds(uint64_t x) { return x / 1000; } + +// Identifies the various sensing devices used by the calibration algorithms. +enum class SensorType : int8_t { + kUndefined = 0, + kAccelerometerMps2 = 1, // 3-axis sensor (units = meter/sec^2). + kGyroscopeRps = 2, // 3-axis sensor (units = radian/sec). + kMagnetometerUt = 3, // 3-axis sensor (units = micro-Tesla). + kTemperatureCelsius = 4, // 1-axis sensor (units = degrees Celsius). + kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal). + kWifiM = 6 // 3-axis sensor (units = meter). +}; + +/* + * SensorData is a generalized data structure used to represent sensor samples + * produced by either a single- or three-axis device. Usage is implied through + * the sensor type (i.e., Gyroscope is a three-axis sensor and would therefore + * use all elements of 'data'; a pressure sensor is single-dimensional and would + * use 'data[SensorIndex::kSingleAxis]'). This arbitration is determined + * at the algorithm wrapper level where knowledge of a sensor's dimensionality + * is clearly understood. + * + * NOTE: The unified dimensional representation makes it convenient to pass + * either type of data into the interface functions defined in the + * OnlineCalibration. + */ + +// Axis index definitions for SensorData::data. +enum SensorIndex : int8_t { + kSingleAxis = 0, + kXAxis = kSingleAxis, + kYAxis = 1, + kZAxis = 2, +}; + +struct SensorData { + // Indicates the type of sensor this data originated from. + SensorType type; + + // Sensor sample timestamp. + uint64_t timestamp_nanos; + + // Generalized sensor sample (represents either single- or three-axis data). + float data[3]; + + SensorData() : type(SensorType::kUndefined), timestamp_nanos(0) { + memset(data, 0, sizeof(data)); + } + + SensorData(SensorType type, uint64_t ts, float x, float y, float z) + : type(type), timestamp_nanos(ts) { + data[SensorIndex::kXAxis] = x; + data[SensorIndex::kYAxis] = y; + data[SensorIndex::kZAxis] = z; + } + + SensorData(SensorType type, uint64_t ts, float single_axis_sample) + : type(type), timestamp_nanos(ts) { + data[SensorIndex::kSingleAxis] = single_axis_sample; + } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc new file mode 100644 index 00000000..26eef573 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2018 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/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +// Estimated upper bounds on gyro offset error (i.e., 3-sigma values). +constexpr float GyroOffsetOtcCal::kMediumQualityRps; +constexpr float GyroOffsetOtcCal::kHighQualityRps; + +void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters) { + gyroCalInit(&gyro_cal_, &gyro_cal_parameters); + overTempCalInit(&over_temp_cal_, &otc_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( + const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + switch (sample.type) { + case SensorType::kAccelerometerMps2: + gyroCalUpdateAccel(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); // [m/sec^2] + break; + + case SensorType::kGyroscopeRps: + gyroCalUpdateGyro(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis], // [rad/sec] + temperature_celsius_); + break; + + case SensorType::kMagnetometerUt: + gyroCalUpdateMag(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); // [micro-Tesla] + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + overTempCalSetTemperature(&over_temp_cal_, sample.timestamp_nanos, + temperature_celsius_); + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new calibration, and updates the OTC. + if (gyroCalNewBiasAvailable(&gyro_cal_)) { + float offset[3]; + float temperature_celsius = kInvalidTemperatureCelsius; + uint64_t calibration_time_nanos = 0; + gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2], + &temperature_celsius, &calibration_time_nanos); + overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos, + offset, temperature_celsius); + } + + // Checks the OTC for a new calibration model update. + const bool new_otc_model_update = + overTempCalNewModelUpdateAvailable(&over_temp_cal_); + + // Checks for a change in the temperature compensated offset estimate. + const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_); + + // Sets the new calibration data. + CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE; + if (new_otc_offset) { + overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius, + cal_data_.offset); + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_callback_flags |= CalibrationTypeFlags::BIAS; + } + + if (new_otc_model_update) { + // Sets the pointer to the OTC model dataset and the number of model points. + cal_data_.otc_model_data = over_temp_cal_.model_data; + cal_data_.num_model_pts = over_temp_cal_.num_model_pts; + + cal_update_callback_flags |= CalibrationTypeFlags::OVER_TEMP; + overTempCalGetModel(&over_temp_cal_, cal_data_.offset, + &cal_data_.offset_temp_celsius, + &cal_data_.cal_update_time_nanos, + cal_data_.temp_sensitivity, cal_data_.temp_intercept); + } + + // Sets the new calibration quality, polling flag, and notifies a calibration + // callback listener of the new update. + if (new_otc_model_update || new_otc_offset) { + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kHighQualityRps; + cal_update_polling_flags_ |= cal_update_callback_flags; + OnNotifyCalibrationUpdate(cal_update_callback_flags); + } + + // Print debug data reports. +#ifdef GYRO_CAL_DBG_ENABLED + gyroCalDebugPrint(&gyro_cal_, sample.timestamp_nanos); +#endif // GYRO_CAL_DBG_ENABLED +#ifdef OVERTEMPCAL_DBG_ENABLED + overTempCalDebugPrint(&over_temp_cal_, sample.timestamp_nanos); +#endif // OVERTEMPCAL_DBG_ENABLED + + return cal_update_polling_flags_; +} + +bool GyroOffsetOtcCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[GyroOffsetOtcCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the initial calibration data (offset and OTC model parameters). + gyroCalSetBias(&gyro_cal_, cal_data_.offset[0], cal_data_.offset[1], + cal_data_.offset[2], cal_data_.offset_temp_celsius, + cal_data_.cal_update_time_nanos); + + overTempCalSetModel(&over_temp_cal_, cal_data_.offset, + cal_data_.offset_temp_celsius, + cal_data_.cal_update_time_nanos, + cal_data_.temp_sensitivity, cal_data_.temp_intercept, + /*jump_start_model=*/false); + + // Sets the calibration quality. + cal_data_.calibration_quality.level = CalibrationQualityLevel::MEDIUM_QUALITY; + cal_data_.calibration_quality.value = kMediumQualityRps; + + const bool load_new_model_dataset = + (input_cal_data.otc_model_data != nullptr && + input_cal_data.num_model_pts > 0); + + if (load_new_model_dataset) { + // Loads the new model dataset and uses it to update the linear model + // parameters. + overTempCalSetModelData(&over_temp_cal_, input_cal_data.num_model_pts, + cal_data_.cal_update_time_nanos, + input_cal_data.otc_model_data); + } + + // Sets the pointer to the OTC model dataset and the number of model points. + cal_data_.otc_model_data = over_temp_cal_.model_data; + cal_data_.num_model_pts = over_temp_cal_.num_model_pts; + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h new file mode 100644 index 00000000..e21007b3 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2018 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. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ + +#include "calibration/gyroscope/gyro_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" +#include "calibration/over_temp/over_temp_cal.h" + +namespace online_calibration { + +/* + * This class is a wrapper for the gyroscope offset calibration with + * over-temperature compensation (OTC). + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::MEDIUM_QUALITY + * CalibrationQuality.value = kMediumQualityRps + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = kHighQualityRps + */ +class GyroOffsetOtcCal final + : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Estimated upper bounds on gyro offset error (i.e., 3-sigma values). + static constexpr float kMediumQualityRps = 5.23599e-3f; // 300 mDPS + static constexpr float kHighQualityRps = 1.04720e-3f; // 60 mDPS + + // Default constructor. + GyroOffsetOtcCal() = default; + + // Creates an GyroOffsetOtcCal with specified algorithm parameters. + GyroOffsetOtcCal(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters) { + Initialize(gyro_cal_parameters, otc_parameters); + } + + // Initializes with specified algorithm parameters, and resets the calibration + // data. + void Initialize(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kGyroscopeRps; + }; + + private: + // GyroCal algorithm data structure. + GyroCal gyro_cal_; + + // Over-temperature offset compensation algorithm data structure. + OverTempCal over_temp_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc new file mode 100644 index 00000000..fe787b1a --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2018 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/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +// Empirically estimated upper bounds on offset error. +constexpr float MagDiverseCal::kLowQualityUt; +constexpr float MagDiverseCal::kHighQualityUt; + +void MagDiverseCal::Initialize( + const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters) { + initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + MagUpdate new_calibration_update = MagUpdate::NO_UPDATE; + switch (sample.type) { + case SensorType::kMagnetometerUt: + new_calibration_update = magCalUpdate( + &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos), + sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new offset estimate, and updates the calibration data. + if (MagUpdate::UPDATE_BIAS & new_calibration_update) { + magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1], + &cal_data_.offset[2]); + + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kHighQualityUt; + cal_data_.offset_temp_celsius = temperature_celsius_; + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_polling_flags_ = CalibrationTypeFlags::BIAS; + OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS); + } + + return cal_update_polling_flags_; +} + +bool MagDiverseCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[MagDiverseCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sets the magnetometer algorithm's calibration data. + magCalReset(&mag_cal_); // Resets the magnetometer's offset vector. + magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1], + input_cal_data.offset[2]); + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the calibration quality to undetermined (uncertain magnetic history + // makes the usefulness of the input calibration value unknown). + cal_data_.calibration_quality.reset(); + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h new file mode 100644 index 00000000..11ede3d4 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2018 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. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ + +#include "calibration/diversity_checker/diversity_checker.h" +#include "calibration/magnetometer/mag_cal/mag_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This class is a wrapper for the magnetometer offset calibration with + * diversity checking. + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = kHighQualityUt + */ +class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Empirically estimated upper bounds on offset error. + static constexpr float kLowQualityUt = 1000.0f; // Units of micro Tesla + static constexpr float kHighQualityUt = 5.0f; // Units of micro Tesla + + MagDiverseCal() = default; + + // Creates an MagDiverseCal with specified algorithm parameters. + MagDiverseCal(const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters) { + Initialize(mag_cal_parameters, diversity_parameters); + } + + // Initializes with specified algorithm parameters. + void Initialize(const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kMagnetometerUt; + }; + + private: + // MagCal algorithm data structure. + MagCal mag_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ |