[IMU_Cal] Fixes Runtime Calibration Data Storage
- Corrects NanoSensorCal calibration parameter storage to
cal.bin/txt.
- Syncs minor code improvements from Google3.
Tip of G3 CL: 197510514
Bug: 79481105
Test: ./load_app.sh and verified calibrations on device.
Change-Id: I8688f88a62aebdbdd92fc37e07108a627167219b
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 837a4c0..1f17254 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
* - Temperature [Celsius]
*
* #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * data to assist in tuning the GyroCal parameters.
*/
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index d57d990..0df2ae6 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -54,7 +54,8 @@
accel_cal_ = accel_cal;
if (accel_cal_ != nullptr) {
if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
- LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_, kAccelTag);
+ LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
+ &accel_cal_update_flags_, kAccelTag);
NANO_CAL_LOGI(kAccelTag,
"Accelerometer runtime calibration initialized.");
} else {
@@ -66,7 +67,8 @@
gyro_cal_ = gyro_cal;
if (gyro_cal_ != nullptr) {
if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
- LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, kGyroTag);
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_,
+ &gyro_cal_update_flags_, kGyroTag);
NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
} else {
gyro_cal_ = nullptr;
@@ -77,7 +79,8 @@
mag_cal_ = mag_cal;
if (mag_cal != nullptr) {
if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
- LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_, kMagTag);
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
+ &mag_cal_update_flags_, kMagTag);
NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
} else {
mag_cal_ = nullptr;
@@ -151,47 +154,47 @@
// Sends a new sensor sample to each active calibration algorithm and sends
// out notifications for new calibration updates.
if (accel_cal_ != nullptr) {
- const CalibrationTypeFlags accel_cal_flags =
+ const CalibrationTypeFlags new_cal_flags =
accel_cal_->SetMeasurement(sample);
- if (accel_cal_flags != CalibrationTypeFlags::NONE) {
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ accel_cal_update_flags_ |= new_cal_flags;
NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
- accel_cal_->GetSensorCalibration(), accel_cal_flags,
- kAccelTag);
- PrintCalibration(accel_cal_->GetSensorCalibration(), accel_cal_flags,
- kAccelTag);
+ accel_cal_->GetSensorCalibration(),
+ accel_cal_update_flags_, kAccelTag);
+ PrintCalibration(accel_cal_->GetSensorCalibration(),
+ accel_cal_update_flags_, kAccelTag);
}
}
if (gyro_cal_ != nullptr) {
- const CalibrationTypeFlags gyro_cal_flags =
+ const CalibrationTypeFlags new_cal_flags =
gyro_cal_->SetMeasurement(sample);
- if (gyro_cal_flags != CalibrationTypeFlags::NONE) {
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ gyro_cal_update_flags_ |= new_cal_flags;
if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
gyro_cal_->GetSensorCalibration(),
- gyro_cal_flags, kGyroTag)) {
+ gyro_cal_update_flags_, kGyroTag)) {
// Limits the log messaging update rate for the gyro calibrations since
// these can occur frequently with rapid temperature changes.
if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
sample.timestamp_nanos, gyro_notification_time_nanos_,
kNanoSensorCalMessageIntervalNanos)) {
gyro_notification_time_nanos_ = sample.timestamp_nanos;
- PrintCalibration(
- gyro_cal_->GetSensorCalibration(),
- // Ensures that both bias and over-temp parameters are printed.
- CalibrationTypeFlags::BIAS | CalibrationTypeFlags::OVER_TEMP,
- kGyroTag);
+ PrintCalibration(gyro_cal_->GetSensorCalibration(),
+ gyro_cal_update_flags_, kGyroTag);
}
}
}
}
if (mag_cal_ != nullptr) {
- const CalibrationTypeFlags mag_cal_flags = mag_cal_->SetMeasurement(sample);
- if (mag_cal_flags != CalibrationTypeFlags::NONE) {
+ const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample);
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ mag_cal_update_flags_ |= new_cal_flags;
NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
- mag_cal_->GetSensorCalibration(), mag_cal_flags,
- kMagTag);
- PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_flags,
+ mag_cal_->GetSensorCalibration(),
+ mag_cal_update_flags_, kMagTag);
+ PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_,
kMagTag);
}
}
@@ -232,8 +235,8 @@
return false;
}
- // Uses the ASH API to store ONLY the algorithm calibration parameters that
- // have been modified by the calibration algorithm.
+ // Uses the ASH API to store all calibration parameters relevant to a given
+ // algorithm as indicated by the input calibration type flags.
ashCalParams ash_cal_parameters;
memset(&ash_cal_parameters, 0, sizeof(ashCalParams));
if (flags & CalibrationTypeFlags::BIAS) {
@@ -263,14 +266,14 @@
bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
OnlineCalibrationThreeAxis *online_cal,
+ CalibrationTypeFlags* flags,
const char *sensor_tag) {
ashCalParams recalled_ash_cal_parameters;
if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
&recalled_ash_cal_parameters)) {
// Checks whether a valid set of runtime calibration parameters was received
// and can be used for initialization.
- CalibrationTypeFlags flags = CalibrationTypeFlags::NONE;
- if (DetectRuntimeCalibration(chreSensorType, sensor_tag, &flags,
+ if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags,
&recalled_ash_cal_parameters)) {
CalibrationDataThreeAxis cal_data;
cal_data.type = online_cal->get_sensor_type();
@@ -278,14 +281,14 @@
// Analyzes the calibration flags and sets only the runtime calibration
// values that were received.
- if (flags & CalibrationTypeFlags::BIAS) {
+ if (*flags & CalibrationTypeFlags::BIAS) {
cal_data.offset_temp_celsius =
recalled_ash_cal_parameters.offsetTempCelsius;
memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
sizeof(cal_data.offset));
}
- if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ if (*flags & CalibrationTypeFlags::OVER_TEMP) {
memcpy(cal_data.temp_sensitivity,
recalled_ash_cal_parameters.tempSensitivity,
sizeof(cal_data.temp_sensitivity));
@@ -298,7 +301,7 @@
// the recalled calibration data.
if (online_cal->SetInitialCalibration(cal_data)) {
return NotifyAshCalibration(chreSensorType,
- online_cal->GetSensorCalibration(), flags,
+ online_cal->GetSensorCalibration(), *flags,
sensor_tag);
} else {
NANO_CAL_LOGE(sensor_tag,
@@ -319,6 +322,7 @@
// Analyzes calibration source flags to determine whether runtime
// calibration values have been loaded and may be used for initialization. A
// valid runtime calibration source will include at least an offset.
+ *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags.
// Uses the ASH calibration source flags to set the appropriate
// CalibrationTypeFlags. These will be used to determine which values to copy
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
index 6b19f7e..d56d034 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -102,20 +102,25 @@
// Loads runtime calibration data using the Android Sensor Hub API. Returns
// 'true' when runtime calibration values were successfully recalled and used
// for algorithm initialization. 'sensor_tag' is a string that identifies a
- // sensor-specific identifier for log meassages.
+ // sensor-specific identifier for log messages. Updates 'flags' to indicate
+ // which runtime calibration parameters were recalled.
bool LoadAshCalibration(uint8_t chreSensorType,
OnlineCalibrationThreeAxis *online_cal,
+ online_calibration::CalibrationTypeFlags* flags,
const char *sensor_tag);
- // Provides sensor calibration updates using the ASH API. Returns 'true' with
- // a successful ASH update.
+ // Provides sensor calibration updates using the ASH API for the specified
+ // sensor type. 'cal_data' contains the new calibration data. 'flags' is used
+ // to indicate all of the valid calibration values that should be provided
+ // with the update. Returns 'true' with a successful ASH update.
bool NotifyAshCalibration(
uint8_t chreSensorType,
const online_calibration::CalibrationDataThreeAxis &cal_data,
online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
// Checks whether 'ash_cal_parameters' is a valid set of runtime calibration
- // data and can be used for algorithm initialization.
+ // data and can be used for algorithm initialization. Updates 'flags' to
+ // indicate which runtime calibration parameters were detected.
bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag,
online_calibration::CalibrationTypeFlags *flags,
ashCalParams *ash_cal_parameters);
@@ -140,6 +145,19 @@
// Pointer to the magnetometer runtime calibration object.
OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
+
+ // Flags that determine which calibration elements are updated with the ASH
+ // API. These are reset during initialization, and latched when a particular
+ // calibration update is detected upon a valid recall of parameters and/or
+ // during runtime. The latching behavior is used to start sending calibration
+ // values of a given type (e.g., bias, over-temp model, etc.) once they are
+ // detected and thereafter.
+ online_calibration::CalibrationTypeFlags accel_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
+ online_calibration::CalibrationTypeFlags gyro_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
+ online_calibration::CalibrationTypeFlags mag_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
};
} // namespace nano_calibration
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
index 915554f..1331153 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -26,10 +26,13 @@
#include <string.h>
#include <sys/types.h>
+#include "common/math/macros.h"
+
namespace online_calibration {
-// Defines an invalid or uninitialized temperature value.
-constexpr float kInvalidTemperatureCelsius = -274.0f;
+// 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; }
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index 6b64cb8..04a9c92 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -283,7 +283,7 @@
// Initializes the over-temperature compensated offset temperature.
over_temp_cal->compensated_offset.offset_temp_celsius =
- OTC_TEMP_INVALID_CELSIUS;
+ INVALID_TEMPERATURE_CELSIUS;
#ifdef OVERTEMPCAL_DBG_ENABLED
// Sets the default sensor descriptors for debugging.
@@ -860,7 +860,7 @@
memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
// Checks that the offset temperature is valid.
- if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
+ if (estimate->offset_temp_celsius > INVALID_TEMPERATURE_CELSIUS) {
const float delta_temp_celsius =
temperature_celsius - estimate->offset_temp_celsius;
@@ -908,7 +908,7 @@
// Adds a delta term to the compensated offset using the temperature
// difference defined by 'delta_temp_celsius'.
if (over_temp_cal->compensated_offset.offset_temp_celsius <=
- OTC_TEMP_INVALID_CELSIUS) {
+ INVALID_TEMPERATURE_CELSIUS) {
// If temperature is invalid, then skip further processing.
break;
}
@@ -940,7 +940,7 @@
// If 'temperature_celsius' is invalid, then no changes to the compensated
// offset are computed.
- if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+ if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
return;
}
@@ -1196,7 +1196,7 @@
ASSERT_NOT_NULL(over_temp_cal);
// If 'temperature_celsius' is invalid, then do not search.
- if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+ if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
return;
}
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 4cfbfc1..8f6f0a4 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -107,9 +107,6 @@
extern "C" {
#endif
-// Defines the maximum size of the 'model_data' array.
-#define OTC_MODEL_SIZE (40)
-
// A common sensor operating temperature at which to begin the model jump-start
// data.
#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
@@ -125,9 +122,6 @@
#define OTC_TEMP_MIN_CELSIUS (-40.0f)
#define OTC_TEMP_MAX_CELSIUS (85.0f)
-// Invalid sensor temperature.
-#define OTC_TEMP_INVALID_CELSIUS (-274.0f)
-
// Number of time-interval levels used to define the least-squares weighting
// function.
#define OTC_NUM_WEIGHT_LEVELS (2)
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_model.h b/firmware/os/algos/calibration/over_temp/over_temp_model.h
index 1f84944..f359c3b 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_model.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h
@@ -23,6 +23,9 @@
extern "C" {
#endif
+// Defines the maximum size of the OverTempCal 'model_data' array.
+#define OTC_MODEL_SIZE (40)
+
/*
* Over-temperature data structures that contain a modeled sensor offset
* estimate, an associated temperature, and the age of the data point since it
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index 1d785d1..cb75595 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -14,15 +14,16 @@
* limitations under the License.
*/
-// This file contains helper macros and definitions.
+// This file contains frequently used constants and helper macros.
#include <stdint.h>
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
-// Mathematical constants.
-#define NANO_PI (3.14159265359f)
+// Constants.
+#define NANO_PI (3.14159265359f)
+#define INVALID_TEMPERATURE_CELSIUS (-274.0f)
// Common math operations.
#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
@@ -75,7 +76,7 @@
// This conversion function may be necessary for embedded hardware that can't
// cast a uint64_t to a float directly. This conversion function was taken from:
-// /third_party/contexthub/firmware/core/floatRt.c
+// [android]//device/google/contexthub/firmware/os/core/floatRt.c
static inline float floatFromUint64(uint64_t v) {
uint32_t hi = v >> 32;
uint32_t lo = (uint32_t) v;
