[IMU_Cal] Runtime sensor calibration code update.
This code updates all of the refactored changes developed and
tested in Google3. This update includes setting calibration
accuracy level.
Tip of G3 CL: 195047378
Bug: 75333210 (refactor updates)
Bug: 75335561 (calibration accuracy)
Test: ./load_app.sh and verified calibrations on device.
Change-Id: I48bd61b6610a7cc7ce9a88bfdde51552c509d763
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index 3c3160f..99e96ef 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -17,6 +17,7 @@
#include "calibration/accelerometer/accel_cal.h"
#include <errno.h>
+#include <inttypes.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
@@ -458,12 +459,13 @@
#ifdef IMU_TEMP_DBG_ENABLED
if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
CAL_DEBUG_LOG("IMU Temp Data: ",
- ", %s%d.%02d, %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n",
- CAL_ENCODE_FLOAT(temp, 2),
- (unsigned long long int)sample_time_nanos,
- CAL_ENCODE_FLOAT(acc->x_bias_new, 5),
- CAL_ENCODE_FLOAT(acc->y_bias_new, 5),
- CAL_ENCODE_FLOAT(acc->z_bias_new, 5));
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64
+ ", " CAL_FORMAT_6DIGITS_TRIPLET " \n",
+ CAL_ENCODE_FLOAT(temp, 3),
+ sample_time_nanos,
+ CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias_new, 6));
acc->temp_time_nanos = sample_time_nanos;
}
#endif
@@ -548,231 +550,216 @@
}
#ifdef ACCEL_CAL_DBG_ENABLED
+
+// Local helper macro for printing log messages.
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_ACCEL_HISTORY \
+ "%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d," \
+ "%s%d.%06d,%s%d.%06d,%s%d.%06d"
+#else
+#define CAL_FORMAT_ACCEL_HISTORY \
+ "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
+
// Debug Print Output
void accelCalDebPrint(struct AccelCal *acc, float temp) {
static int32_t kk = 0;
if (++kk == 1000) {
// X offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,11,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(x_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{11," CAL_FORMAT_ACCEL_HISTORY "}(x_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
// Y offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,12,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(y_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{12," CAL_FORMAT_ACCEL_HISTORY "}(y_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
// Z offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,13,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(z_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{13," CAL_FORMAT_ACCEL_HISTORY "}(z_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
// Temp history variation VAR of offset.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,14,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(VAR temp history)\n",
- CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{14," CAL_FORMAT_ACCEL_HISTORY "}(VAR temp history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
// Temp mean history of offset.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,15,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(MEAN Temp history)\n",
- CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{15," CAL_FORMAT_ACCEL_HISTORY "}(MEAN Temp history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
// KASA radius history.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,16,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(radius)\n",
- CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{16," CAL_FORMAT_ACCEL_HISTORY "}(radius)\n",
+ CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
kk = 0;
}
if (kk == 750) {
// Eigen Vector X.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "7,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen x)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 7," CAL_FORMAT_ACCEL_HISTORY "}(eigen x)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
// Y.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "8,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen y)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 8," CAL_FORMAT_ACCEL_HISTORY "}(eigen y)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
// Z.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "9,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen z)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 9," CAL_FORMAT_ACCEL_HISTORY "}(eigen z)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
// Accel Time in ns.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,10,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,}("
- "timestamp ns)\n",
- acc->adf.cal_time[0], acc->adf.cal_time[1], acc->adf.cal_time[2],
- acc->adf.cal_time[3], acc->adf.cal_time[4], acc->adf.cal_time[5],
- acc->adf.cal_time[6], acc->adf.cal_time[7], acc->adf.cal_time[8],
- acc->adf.cal_time[9]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{10,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+ ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+ "}(timestamp ns)\n",
+ acc->adf.cal_time[0], acc->adf.cal_time[1],
+ acc->adf.cal_time[2], acc->adf.cal_time[3],
+ acc->adf.cal_time[4], acc->adf.cal_time[5],
+ acc->adf.cal_time[6], acc->adf.cal_time[7],
+ acc->adf.cal_time[8], acc->adf.cal_time[9]);
}
if (kk == 500) {
// Total bucket count.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d,}(Total Bucket #)\n",
- (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, (unsigned)acc->adf.nty,
- (unsigned)acc->adf.ntyb, (unsigned)acc->adf.ntz,
- (unsigned)acc->adf.ntzb, (unsigned)acc->adf.ntle);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d}(Total Bucket #)\n",
+ (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb,
+ (unsigned)acc->adf.nty, (unsigned)acc->adf.ntyb,
+ (unsigned)acc->adf.ntz, (unsigned)acc->adf.ntzb,
+ (unsigned)acc->adf.ntle);
// Live bucket count lower.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
- "lower)\n",
- (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
- (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
- (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
- (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+ "lower)\n",
+ (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
+ (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
+ (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
+ (unsigned)acc->ac1[0].agd.nle,
+ (unsigned)acc->ac1[0].akf.nsamples);
// Live bucket count hogher.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
- "higher)\n",
- (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
- (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
- (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
- (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+ "higher)\n",
+ (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
+ (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
+ (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
+ (unsigned)acc->ac1[1].agd.nle,
+ (unsigned)acc->ac1[1].akf.nsamples);
// Offset used.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 3,%s%d.%06d, %s%d.%06d, %s%d.%06d, %2d,}(updated offset "
- "x,y,z, total # of offsets)\n",
- CAL_ENCODE_FLOAT(acc->x_bias, 6), CAL_ENCODE_FLOAT(acc->y_bias, 6),
- CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 3,"CAL_FORMAT_6DIGITS_TRIPLET", %2d}(updated offset "
+ "x,y,z, total # of offsets)\n",
+ CAL_ENCODE_FLOAT(acc->x_bias, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
// Offset New.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 4,%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d,}(New offset "
- "x,y,z, live temp)\n",
- CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
- CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
- CAL_ENCODE_FLOAT(acc->z_bias_new, 6), CAL_ENCODE_FLOAT(temp, 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 4," CAL_FORMAT_6DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS
+ "}(New offset x,y,z, live temp)\n",
+ CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias_new, 6),
+ CAL_ENCODE_FLOAT(temp, 6));
// Temp Histogram.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
- "%7d, %7d,}(temp histo)\n",
- (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
- (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
- (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
- (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
- (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
- (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
- (unsigned)acc->adf.t_hist[12]);
- CAL_DEBUG_LOG(
- "[BMI160]",
- "M{K_ACCEL, 6,%7d, %7d, %7d,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
- "%7d,}(temp histo)\n",
- (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
- (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
- (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
- (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
- (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
- (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+ "%7d, %7d}(temp histo)\n",
+ (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
+ (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
+ (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
+ (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
+ (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
+ (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
+ (unsigned)acc->adf.t_hist[12]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 6,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+ "%7d}(temp histo)\n",
+ (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
+ (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
+ (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
+ (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
+ (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
+ (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
}
}
#endif
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 9da7aa4..90b2544 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -17,6 +17,7 @@
#include "calibration/gyroscope/gyro_cal.h"
#include <float.h>
+#include <inttypes.h>
#include <math.h>
#include <string.h>
@@ -222,12 +223,12 @@
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
"Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
CAL_ENCODE_FLOAT(bias_x * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(bias_y * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(bias_z * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(temperature_celsius, 3),
- (unsigned long long int)calibration_time_nanos);
+ calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
}
@@ -471,7 +472,7 @@
CAL_DEBUG_LOG(
"[GYRO_CAL:REJECT]",
"Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
CAL_ENCODE_FLOAT(
gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(
@@ -479,7 +480,7 @@
CAL_ENCODE_FLOAT(
gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
- (unsigned long long int)calibration_time_nanos);
+ calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Outside of range. Ignore, reset, and continue.
@@ -540,21 +541,17 @@
#ifdef GYRO_CAL_DBG_ENABLED
gyro_cal->debug_watchdog_count++;
if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:WATCHDOG]",
- "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu",
- (unsigned long int)gyro_cal->debug_watchdog_count,
- (unsigned long long int)sample_time_nanos,
- (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos -
- sample_time_nanos));
+ CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+ "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64
+ ", -%" PRIu64,
+ gyro_cal->debug_watchdog_count, sample_time_nanos,
+ gyro_cal->gyro_watchdog_start_nanos - sample_time_nanos);
} else {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:WATCHDOG]",
- "Total#, Timestamp | Delta [nsec]: %lu, %llu, %llu",
- (unsigned long int)gyro_cal->debug_watchdog_count,
- (unsigned long long int)sample_time_nanos,
- (unsigned long long int)(sample_time_nanos -
- gyro_cal->gyro_watchdog_start_nanos));
+ CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+ "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64
+ ", %" PRIu64,
+ gyro_cal->debug_watchdog_count, sample_time_nanos,
+ sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos);
}
#endif // GYRO_CAL_DBG_ENABLED
@@ -884,8 +881,9 @@
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
- (unsigned long int)gyro_cal->debug_calibration_count,
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+ ", %" PRIu64,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(
gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(
@@ -894,8 +892,7 @@
gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
3),
- (unsigned long long int)
- gyro_cal->debug_gyro_cal.end_still_time_nanos);
+ gyro_cal->debug_gyro_cal.end_still_time_nanos);
break;
case STILLNESS_DATA:
@@ -904,13 +901,11 @@
: -1.0f; // Signals that magnetometer was not used.
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Stillness|Confidence [nsec]: %lu, "
- "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
- (unsigned long long int)(gyro_cal->debug_gyro_cal
- .end_still_time_nanos -
- gyro_cal->debug_gyro_cal
- .start_still_time_nanos),
+ "Cal#|Stillness|Confidence [nsec]: %zu, "
+ "%" PRIu64 ", " CAL_FORMAT_3DIGITS_TRIPLET,
+ gyro_cal->debug_calibration_count,
+ gyro_cal->debug_gyro_cal.end_still_time_nanos -
+ gyro_cal->debug_gyro_cal.start_still_time_nanos,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
CAL_ENCODE_FLOAT(mag_data, 3));
@@ -920,9 +915,9 @@
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
", " CAL_FORMAT_3DIGITS,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3),
@@ -939,8 +934,8 @@
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Gyro Peak Stillness Variation [mDPS]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
RAD_TO_MDEG,
@@ -958,9 +953,9 @@
case ACCEL_STATS:
CAL_DEBUG_LOG(debug_tag,
"Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_6DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
@@ -972,9 +967,9 @@
case GYRO_STATS:
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
@@ -996,9 +991,9 @@
if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "Cal#|Mag Mean|Var [uT|uT^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_6DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
@@ -1007,9 +1002,9 @@
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
} else {
CAL_DEBUG_LOG(debug_tag,
- "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
+ "Cal#|Mag Mean|Var [uT|uT^2]: %zu, 0, 0, 0, -1.0, -1.0, "
"-1.0",
- (unsigned long int)gyro_cal->debug_calibration_count);
+ gyro_cal->debug_calibration_count);
}
break;
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index deb26b3..bb2063b 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -15,6 +15,7 @@
*/
#include "calibration/gyroscope/gyro_stillness_detect.h"
+
#include <string.h>
/////// FORWARD DECLARATIONS /////////////////////////////////////////
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
similarity index 79%
rename from firmware/os/algos/calibration/magnetometer/mag_cal.c
rename to firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
index 40749e1..c3f12ae 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -14,23 +14,30 @@
* limitations under the License.
*/
-#include "calibration/magnetometer/mag_cal.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
#include <errno.h>
+#include <inttypes.h>
#include <string.h>
#include "calibration/util/cal_log.h"
-// clang-format off
-#ifdef MAG_CAL_ORIGINAL_TUNING
-#define MAX_EIGEN_RATIO 25.0f
-#define MAX_EIGEN_MAG 80.0f // uT
-#define MIN_EIGEN_MAG 10.0f // uT
-#define MAX_FIT_MAG 80.0f
-#define MIN_FIT_MAG 10.0f
-#define MAX_BATCH_WINDOW 15000000UL // 15 sec
-#define MIN_BATCH_SIZE 25 // samples
+// Local helper macro for printing log messages.
+#ifdef MAG_CAL_DEBUG_ENABLE
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_MAG_MEMORY \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d"
#else
+#define CAL_FORMAT_MAG_MEMORY \
+ "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, " \
+ "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
+#endif // MAG_CAL_DEBUG_ENABLE
+
+// clang-format off
#define MAX_EIGEN_RATIO 15.0f
#define MAX_EIGEN_MAG 70.0f // uT
#define MIN_EIGEN_MAG 20.0f // uT
@@ -38,23 +45,12 @@
#define MIN_FIT_MAG 20.0f
#define MAX_BATCH_WINDOW 15000000UL // 15 sec
#define MIN_BATCH_SIZE 25 // samples
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
#define MAX_DISTANCE_VIOLATIONS 2
-#ifdef SPHERE_FIT_ENABLED
-#define MAX_ITERATIONS 30
-#define INITIAL_U_SCALE 1.0e-4f
-#define GRADIENT_THRESHOLD 1.0e-16f
-#define RELATIVE_STEP_THRESHOLD 1.0e-7f
-#define FROM_MICRO_SEC_TO_SEC 1.0e-6f
-#endif
-#endif
// clang-format
-// eigen value magnitude and ratio test
+// eigen value magnitude and ratio test.
static int moc_eigen_test(struct KasaFit *kasa) {
- // covariance matrix
+ // covariance matrix.
struct Mat33 S;
S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
@@ -86,9 +82,7 @@
void magCalReset(struct MagCal *moc) {
kasaReset(&moc->kasa);
-#ifdef DIVERSITY_CHECK_ENABLED
diversityCheckerReset(&moc->diversity_checker);
-#endif
moc->start_time = 0;
moc->kasa_batching = false;
}
@@ -110,12 +104,8 @@
}
void initMagCal(struct MagCal *moc,
- const struct MagCalParameters *mag_cal_parameters
-#ifdef DIVERSITY_CHECK_ENABLED
- ,
- const struct DiversityCheckerParameters *diverse_parameters
-#endif
-) {
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters) {
magCalReset(moc);
moc->update_time = 0;
moc->min_batch_window_in_micros =
@@ -139,12 +129,10 @@
#ifdef MAG_CAL_DEBUG_ENABLE
moc->mag_dbg.mag_trigger_count = 0;
moc->mag_dbg.kasa_count = 0;
-#endif
+#endif // MAG_CAL_DEBUG_ENABLE
-#ifdef DIVERSITY_CHECK_ENABLED
// Diversity Checker
diversityCheckerInit(&moc->diversity_checker, diverse_parameters);
-#endif
}
void magCalDestroy(struct MagCal *moc) { (void)moc; }
@@ -153,10 +141,8 @@
float x, float y, float z) {
enum MagUpdate new_bias = NO_UPDATE;
-#ifdef DIVERSITY_CHECK_ENABLED
// Diversity Checker Update.
diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
-#endif
// 1. run accumulators
kasaAccumulate(&moc->kasa, x, y, z);
@@ -178,15 +164,13 @@
if (kasaFit(&moc->kasa, &bias, &radius, MAX_FIT_MAG, MIN_FIT_MAG)) {
#ifdef MAG_CAL_DEBUG_ENABLE
moc->mag_dbg.kasa_count++;
- CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :,",
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %lu, %lu",
+ CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32,
CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
- (unsigned long int)moc->mag_dbg.kasa_count,
- (unsigned long int)moc->mag_dbg.mag_trigger_count);
-#endif
+ moc->mag_dbg.kasa_count, moc->mag_dbg.mag_trigger_count);
+#endif // MAG_CAL_DEBUG_ENABLE
-#ifdef DIVERSITY_CHECK_ENABLED
// Update the local field.
diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius);
@@ -198,14 +182,13 @@
// DEBUG PRINT OUT.
#ifdef MAG_CAL_DEBUG_ENABLE
moc->mag_dbg.mag_trigger_count++;
-#ifdef DIVERSE_DEBUG_ENABLE
moc->diversity_checker.diversity_dbg.new_trigger = 1;
CAL_DEBUG_LOG(
- "[MAG_CAL:BIAS UPDATE] :, ",
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d,"
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %zu, %s%d.%03d, "
- "%s%d.%03d, %lu, %lu, %llu, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %llu",
+ "[MAG_CAL:BIAS UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET ", "
+ CAL_FORMAT_3DIGITS ", " CAL_FORMAT_6DIGITS ", "
+ CAL_FORMAT_3DIGITS_TRIPLET ", %zu, " CAL_FORMAT_3DIGITS ", "
+ CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32 ", %" PRIu64 ", "
+ CAL_FORMAT_3DIGITS_TRIPLET ", %" PRIu64 "",
CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.var_log, 6),
@@ -216,16 +199,14 @@
moc->diversity_checker.num_points,
CAL_ENCODE_FLOAT(moc->diversity_checker.threshold, 3),
CAL_ENCODE_FLOAT(moc->diversity_checker.max_distance, 3),
- (unsigned long int)moc->mag_dbg.mag_trigger_count,
- (unsigned long int)moc->mag_dbg.kasa_count,
- (unsigned long long int)sample_time_us,
+ moc->mag_dbg.mag_trigger_count,
+ moc->mag_dbg.kasa_count,
+ sample_time_us,
CAL_ENCODE_FLOAT(moc->x_bias, 3),
CAL_ENCODE_FLOAT(moc->y_bias, 3),
CAL_ENCODE_FLOAT(moc->z_bias, 3),
- (unsigned long long int)moc->update_time);
-#endif
-#endif
-#endif
+ moc->update_time);
+#endif // MAG_CAL_DEBUG_ENABLE
moc->x_bias = bias.x;
moc->y_bias = bias.y;
moc->z_bias = bias.z;
@@ -234,13 +215,9 @@
moc->update_time = sample_time_us;
new_bias = UPDATE_BIAS;
-
-#ifdef DIVERSITY_CHECK_ENABLED
}
-#endif
}
}
-
// 5. reset for next batch
magCalReset(moc);
}
@@ -248,7 +225,7 @@
return new_bias;
}
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) {
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z) {
*x = moc->x_bias;
*y = moc->y_bias;
*z = moc->z_bias;
@@ -288,7 +265,7 @@
*zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
}
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSE_DEBUG_ENABLE
+#if defined MAG_CAL_DEBUG_ENABLE
// This function prints every second sample parts of the dbg diverse_data_log,
// which ensures that all the messages get printed into the log file.
void magLogPrint(struct DiversityChecker *diverse_data, float temp) {
@@ -299,13 +276,9 @@
sample_counter++;
if (sample_counter == 2) {
CAL_DEBUG_LOG(
- "[MAG_CAL:MEMORY X] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d",
- (unsigned long int)diverse_data->diversity_dbg.diversity_count,
+ "[MAG_CAL:MEMORY X] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY ", "
+ CAL_FORMAT_3DIGITS,
+ diverse_data->diversity_dbg.diversity_count,
CAL_ENCODE_FLOAT(data_log_ptr[0 * 3], 3),
CAL_ENCODE_FLOAT(data_log_ptr[1 * 3], 3),
CAL_ENCODE_FLOAT(data_log_ptr[2 * 3], 3),
@@ -330,12 +303,8 @@
if (sample_counter == 4) {
CAL_DEBUG_LOG(
- "[MAG_CAL:MEMORY Y] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
- (unsigned long int)diverse_data->diversity_dbg.diversity_count,
+ "[MAG_CAL:MEMORY Y] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+ diverse_data->diversity_dbg.diversity_count,
CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 1], 3),
CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 1], 3),
CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 1], 3),
@@ -359,12 +328,8 @@
}
if (sample_counter == 6) {
CAL_DEBUG_LOG(
- "[MAG_CAL:MEMORY Z] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
- (unsigned long int)diverse_data->diversity_dbg.diversity_count,
+ "[MAG_CAL:MEMORY Z] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+ diverse_data->diversity_dbg.diversity_count,
CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 2], 3),
CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 2], 3),
CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 2], 3),
@@ -390,4 +355,4 @@
}
}
}
-#endif
+#endif // MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
similarity index 85%
rename from firmware/os/algos/calibration/magnetometer/mag_cal.h
rename to firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
index 381a925..bbc079c 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
@@ -14,21 +14,19 @@
* limitations under the License.
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
-#ifdef SPHERE_FIT_ENABLED
-#ifndef DIVERSITY_CHECK_ENABLED
-#define DIVERSITY_CHECK_ENABLED
-#endif
-#endif
+#ifdef MAG_CAL_DEBUG_ENABLE
+// Enables diversity debug messages if mag_cal debugging is enabled.
+#define DIVERSE_DEBUG_ENABLE
+#endif // MAG_CAL_DEBUG_ENABLE
#include <stdbool.h>
#include <stdint.h>
#include <sys/types.h>
-#ifdef DIVERSITY_CHECK_ENABLED
+
#include "calibration/diversity_checker/diversity_checker.h"
-#endif
#include "common/math/kasa.h"
#include "common/math/mat.h"
#include "common/math/vec.h"
@@ -71,9 +69,7 @@
};
struct MagCal {
-#ifdef DIVERSITY_CHECK_ENABLED
struct DiversityChecker diversity_checker;
-#endif
struct KasaFit kasa;
uint64_t start_time; // [micro-seconds]
@@ -89,21 +85,16 @@
#endif
};
-#ifdef DIVERSITY_CHECK_ENABLED
void initMagCal(struct MagCal *moc,
const struct MagCalParameters *mag_cal_parameters,
const struct DiversityCheckerParameters *diverse_parameters);
-#else
-void initMagCal(struct MagCal *moc,
- const struct MagCalParameters *mag_cal_parameters);
-#endif
void magCalDestroy(struct MagCal *moc);
enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
float x, float y, float z);
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z);
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z);
void magCalAddBias(struct MagCal *moc, float x, float y, float z);
@@ -119,7 +110,7 @@
void magCalReset(struct MagCal *moc);
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSITY_CHECK_ENABLED
+#if defined MAG_CAL_DEBUG_ENABLE
void magLogPrint(struct DiversityChecker *moc, float temp);
#endif
@@ -127,4 +118,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
similarity index 98%
rename from firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
rename to firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
index 3e23dcf..93c2ac6 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
@@ -14,13 +14,11 @@
* limitations under the License.
*/
-#include "calibration/magnetometer/mag_sphere_fit.h"
+#include "calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h"
#include <errno.h>
#include <string.h>
-#include "calibration/util/cal_log.h"
-
#define MAX_ITERATIONS 30
#define INITIAL_U_SCALE 1.0e-4f
#define GRADIENT_THRESHOLD 1.0e-16f
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
similarity index 91%
rename from firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
rename to firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
index 7c2fc86..3ae4687 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
@@ -14,10 +14,10 @@
* limitations under the License.
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
-#include "calibration/magnetometer/mag_cal.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
#include "calibration/sphere_fit/sphere_fit_calibration.h"
#define NUM_SPHERE_FIT_DATA 50
@@ -69,4 +69,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
index 88557ad..12b8470 100644
--- a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
+++ b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
@@ -28,7 +28,7 @@
#ifdef MAG_CAL_ENABLED
#include "calibration/diversity_checker/diversity_checker.h"
-#include "calibration/magnetometer/mag_cal.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
#endif // MAG_CAL_ENABLED
#include "common/math/macros.h"
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
index 68b477b..a5de5d7 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -39,10 +39,10 @@
#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__)
#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__)
#else
-#define NANO_CAL_LOGD(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
-#define NANO_CAL_LOGI(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
-#define NANO_CAL_LOGW(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
-#define NANO_CAL_LOGE(tag, format, ...) chreLogNull(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
#endif // NANO_SENSOR_CAL_DBG_ENABLED
} // namespace
@@ -54,11 +54,7 @@
accel_cal_ = accel_cal;
if (accel_cal_ != nullptr) {
if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
- if (LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
- kAccelTag)) {
- PrintCalibration(accel_cal_->GetSensorCalibration(),
- CalibrationTypeFlags::BIAS, kAccelTag);
- }
+ LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_, kAccelTag);
NANO_CAL_LOGI(kAccelTag,
"Accelerometer runtime calibration initialized.");
} else {
@@ -70,12 +66,7 @@
gyro_cal_ = gyro_cal;
if (gyro_cal_ != nullptr) {
if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
- if (LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, kGyroTag)) {
- PrintCalibration(
- gyro_cal_->GetSensorCalibration(),
- CalibrationTypeFlags::BIAS | CalibrationTypeFlags::OVER_TEMP,
- kGyroTag);
- }
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, kGyroTag);
NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
} else {
gyro_cal_ = nullptr;
@@ -86,11 +77,7 @@
mag_cal_ = mag_cal;
if (mag_cal != nullptr) {
if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
- if (LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
- kMagTag)) {
- PrintCalibration(mag_cal_->GetSensorCalibration(),
- CalibrationTypeFlags::BIAS, kMagTag);
- }
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_, kMagTag);
NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
} else {
mag_cal_ = nullptr;
@@ -188,8 +175,11 @@
sample.timestamp_nanos, gyro_notification_time_nanos_,
kNanoSensorCalMessageIntervalNanos)) {
gyro_notification_time_nanos_ = sample.timestamp_nanos;
- PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_flags,
- kGyroTag);
+ PrintCalibration(
+ gyro_cal_->GetSensorCalibration(),
+ // Ensures that both bias and over-temp parameters are printed.
+ CalibrationTypeFlags::BIAS | CalibrationTypeFlags::OVER_TEMP,
+ kGyroTag);
}
}
}
@@ -217,7 +207,23 @@
ash_cal_info.compMatrix[4] = 1.0f;
ash_cal_info.compMatrix[8] = 1.0f;
memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias));
- ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
+
+ // Sets the appropriate calibration accuracy level.
+ switch (cal_data.calibration_quality.level) {
+ case online_calibration::CalibrationQualityLevel::HIGH_QUALITY:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
+ break;
+
+ case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM;
+ break;
+
+ case online_calibration::CalibrationQualityLevel::LOW_QUALITY:
+ // FALLTHROUGH_INTENTIONAL.
+ default:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW;
+ break;
+ }
if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
@@ -307,56 +313,38 @@
const char *sensor_tag,
CalibrationTypeFlags *flags,
ashCalParams *ash_cal_parameters) {
- // Analyzes calibration source flags to determine whether factory calibration
- // data was received. A valid factory calibration source will include at least
- // an offset.
- bool factory_cal_detected =
- ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_NONE &&
- ash_cal_parameters->offsetTempCelsiusSource ==
- ASH_CAL_PARAMS_SOURCE_FACTORY;
+ // 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.
+ // Uses the ASH calibration source flags to set the appropriate
+ // CalibrationTypeFlags. These will be used to determine which values to copy
+ // from 'ash_cal_parameters' and provide to the calibration algorithms for
+ // initialization.
bool runtime_cal_detected = false;
- if (factory_cal_detected) {
- // Prints the retrieved factory calibration data.
- NANO_CAL_LOGI(sensor_tag, "Factory calibration detected.");
+ if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->offsetTempCelsiusSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ runtime_cal_detected = true;
+ *flags = CalibrationTypeFlags::BIAS;
+ }
+
+ if (ash_cal_parameters->tempSensitivitySource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->tempInterceptSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ *flags |= CalibrationTypeFlags::OVER_TEMP;
+ }
+
+ if (runtime_cal_detected) {
+ // Prints the retrieved runtime calibration data.
+ NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected.");
PrintAshCalParams(*ash_cal_parameters, sensor_tag);
-
- // Since the factory calibration is applied lower in the sensor framework,
- // initialization using these parameters must be skipped to avoid the
- // possibility of double calibration. Returns 'false' to avoid further
- // initialization. Note, the runtime calibration algorithms dynamically
- // correct the residuals that the factory calibration doesn't or can't
- // account for (e.g., aging induced error charateristics).
- return false;
} else {
- // 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.
-
- // Converts the ASH calibration source flags to CalibrationTypeFlags. These
- // will be used to determine which values to copy from 'ash_cal_parameters'
- // and provide to the calibration algorithms for initialization.
- if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
- ash_cal_parameters->offsetTempCelsiusSource ==
- ASH_CAL_PARAMS_SOURCE_RUNTIME) {
- runtime_cal_detected = true;
- *flags = CalibrationTypeFlags::BIAS;
- }
-
- if (ash_cal_parameters->tempSensitivitySource ==
- ASH_CAL_PARAMS_SOURCE_RUNTIME &&
- ash_cal_parameters->tempInterceptSource ==
- ASH_CAL_PARAMS_SOURCE_RUNTIME) {
- runtime_cal_detected = true;
- *flags |= CalibrationTypeFlags::OVER_TEMP;
- }
-
- if (!runtime_cal_detected) {
- // This is a warning (not an error) since the runtime algorithms will
- // function correctly with no recalled calibration values. They will
- // eventually trigger and update the system with valid calibration data.
- NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
- }
+ // This is a warning (not an error) since the runtime algorithms will
+ // function correctly with no recalled calibration values. They will
+ // eventually trigger and update the system with valid calibration data.
+ NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
}
return runtime_cal_detected;
@@ -365,26 +353,37 @@
// Helper functions for logging calibration information.
void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params,
const char *sensor_tag) {
- NANO_CAL_LOGI(sensor_tag, "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
- cal_params.offset[0], cal_params.offset[1],
- cal_params.offset[2], cal_params.offsetTempCelsius);
+ if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+ cal_params.offset[0], cal_params.offset[1],
+ cal_params.offset[2], cal_params.offsetTempCelsius);
+ }
- NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
- cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
- cal_params.tempSensitivity[2]);
+ if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
+ cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
+ cal_params.tempSensitivity[2]);
+ }
- NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
- cal_params.tempIntercept[0], cal_params.tempIntercept[1],
- cal_params.tempIntercept[2]);
+ if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
+ cal_params.tempIntercept[0], cal_params.tempIntercept[1],
+ cal_params.tempIntercept[2]);
+ }
- NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
- cal_params.scaleFactor[0], cal_params.scaleFactor[1],
- cal_params.scaleFactor[2]);
+ if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
+ cal_params.scaleFactor[0], cal_params.scaleFactor[1],
+ cal_params.scaleFactor[2]);
+ }
- NANO_CAL_LOGI(sensor_tag,
- "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
- cal_params.crossAxis[0], cal_params.crossAxis[1],
- cal_params.crossAxis[2]);
+ if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
+ cal_params.crossAxis[0], cal_params.crossAxis[1],
+ cal_params.crossAxis[2]);
+ }
}
void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
index 5985054..6b19f7e 100644
--- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -81,8 +81,8 @@
NanoSensorCal() = default;
// Sets the sensor calibration object pointers and initializes the algorithms
- // using factory or runtime values recalled using Android Sensor Hub (ASH). A
- // nullptr may be passed in to disable a particular sensor calibration.
+ // using runtime values recalled using Android Sensor Hub (ASH). A nullptr may
+ // be passed in to disable a particular sensor calibration.
void Initialize(OnlineCalibrationThreeAxis *accel_cal,
OnlineCalibrationThreeAxis *gyro_cal,
OnlineCalibrationThreeAxis *mag_cal);
@@ -99,10 +99,10 @@
// Passes sensor data to the runtime calibration algorithms.
void ProcessSample(const online_calibration::SensorData &sample);
- // Loads factory and 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.
+ // 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.
bool LoadAshCalibration(uint8_t chreSensorType,
OnlineCalibrationThreeAxis *online_cal,
const char *sensor_tag);
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
index 3958ae8..e9def4e 100644
--- 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
@@ -1,3 +1,19 @@
+/*
+ * 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"
@@ -40,6 +56,8 @@
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;
@@ -65,6 +83,10 @@
// 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;
}
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
index d2ec494..0256495 100644
--- 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
@@ -1,3 +1,19 @@
+/*
+ * 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_
@@ -11,6 +27,17 @@
/*
* 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> {
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
index 96c061f..62bc443 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
@@ -1,4 +1,20 @@
/*
+ * 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.
*/
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
index 23ebe4f..8f1470a 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
@@ -1,3 +1,19 @@
+/*
+ * 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 {
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
index 8b95ef7..0404936 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
@@ -1,4 +1,20 @@
/*
+ * 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.
@@ -11,6 +27,7 @@
#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"
@@ -21,14 +38,16 @@
* 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).
+ * 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,
@@ -36,6 +55,7 @@
SCALE_FACTOR = 0x02,
CROSS_AXIS = 0x04,
OVER_TEMP = 0x08,
+ QUALITY_DEGRADED = 0x10,
ALL = 0xFF
};
@@ -102,11 +122,16 @@
// 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 precendence
+ // 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;
@@ -115,6 +140,7 @@
// 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;
@@ -168,11 +194,16 @@
// 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 precendence
+ // 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;
@@ -181,6 +212,7 @@
// 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;
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 0000000..d6475c7
--- /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
index bf348ac..c893065 100644
--- a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
+++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
@@ -1,3 +1,19 @@
+/*
+ * 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_
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 9849ad2..915554f 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
@@ -1,4 +1,20 @@
/*
+ * 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.
*/
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
index 67ea497..26eef57 100644
--- 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
@@ -1,9 +1,29 @@
+/*
+ * 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);
@@ -87,9 +107,11 @@
cal_data_.temp_sensitivity, cal_data_.temp_intercept);
}
- // Sets the new-calibration polling flag, and notifies a calibration callback
- // listener of the new update.
+ // 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);
}
@@ -128,6 +150,10 @@
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);
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
index 74ba982..e21007b 100644
--- 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
@@ -1,3 +1,19 @@
+/*
+ * 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_
@@ -13,10 +29,23 @@
/*
* 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;
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
index a963dcd..a4b4dfd 100644
--- 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
@@ -1,9 +1,29 @@
+/*
+ * 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) {
@@ -36,6 +56,8 @@
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;
@@ -62,6 +84,10 @@
// 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 = kLowQualityUt;
+
return true;
}
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
index 23ac3c4..212ffeb 100644
--- 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
@@ -1,8 +1,24 @@
+/*
+ * 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.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"
@@ -13,9 +29,22 @@
/*
* 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::LOW_QUALITY
+ * CalibrationQuality.value = kLowQualityUt
+ * 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.
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 908d383..6b64cb8 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -17,6 +17,7 @@
#include "calibration/over_temp/over_temp_cal.h"
#include <float.h>
+#include <inttypes.h>
#include <math.h>
#include <string.h>
@@ -582,13 +583,12 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3),
- CAL_ENCODE_FLOAT(temperature_celsius, 3),
- (unsigned long long int)timestamp_nanos);
+ CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
return; // Outlier detected: skips adding this offset to the model.
@@ -699,9 +699,9 @@
// Prints out temperature and the current timestamp.
createDebugTag(over_temp_cal, ":TEMP]");
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
- CAL_ENCODE_FLOAT(temperature_celsius, 3),
- (unsigned long long int)timestamp_nanos);
+ "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS
+ ", %" PRIu64,
+ CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
}
#endif // OVERTEMPCAL_DBG_LOG_TEMP
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -969,16 +969,23 @@
// To properly evaluate the logic paths that use the latest and nearest offset
// data below, the current age of the nearest and latest offset estimates are
// computed.
- const uint64_t latest_offset_age_nanos =
- (over_temp_cal->latest_offset != NULL)
- ? over_temp_cal->latest_offset->offset_age_nanos + timestamp_nanos -
- over_temp_cal->last_age_update_nanos
- : 0;
- const uint64_t nearest_offset_age_nanos =
- (over_temp_cal->nearest_offset != NULL)
- ? over_temp_cal->nearest_offset->offset_age_nanos + timestamp_nanos -
- over_temp_cal->last_age_update_nanos
- : 0;
+ uint64_t latest_offset_age_nanos = 0;
+ if (over_temp_cal->latest_offset != NULL) {
+ latest_offset_age_nanos =
+ (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+ ? over_temp_cal->latest_offset->offset_age_nanos +
+ timestamp_nanos - over_temp_cal->last_age_update_nanos
+ : over_temp_cal->latest_offset->offset_age_nanos;
+ }
+
+ uint64_t nearest_offset_age_nanos = 0;
+ if (over_temp_cal->nearest_offset != NULL) {
+ nearest_offset_age_nanos =
+ (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+ ? over_temp_cal->nearest_offset->offset_age_nanos +
+ timestamp_nanos - over_temp_cal->last_age_update_nanos
+ : over_temp_cal->nearest_offset->offset_age_nanos;
+ }
// True when the latest offset estimate will be used to compute a sensor
// offset calibration estimate.
@@ -1159,14 +1166,14 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(
sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3),
- (unsigned long long int)timestamp_nanos);
+ timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
}
}
@@ -1251,7 +1258,7 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
over_temp_cal->otc_unit_conversion,
@@ -1264,8 +1271,7 @@
3),
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
- (unsigned long long int)over_temp_cal->model_data[model_index]
- .offset_age_nanos);
+ over_temp_cal->model_data[model_index].offset_age_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
// Remove the model data at 'model_index'.
@@ -1327,8 +1333,8 @@
createDebugTag(over_temp_cal, ":INIT]");
if (over_temp_cal->num_model_pts > 0) {
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Model Jump-Start: #Points = %lu.",
- (unsigned long int)over_temp_cal->num_model_pts);
+ "Model Jump-Start: #Points = %zu.",
+ over_temp_cal->num_model_pts);
}
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -1478,6 +1484,11 @@
void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
+ if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) {
+ // Age updates must be monotonic.
+ return;
+ }
+
uint64_t age_increment_nanos =
timestamp_nanos - over_temp_cal->last_age_update_nanos;
@@ -1589,10 +1600,9 @@
// Prints out the latest offset estimate (input data).
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|Offset|Temp|Age [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
+ "Cal#|Offset|Temp|Age [%s|C|nsec]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+ over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
over_temp_cal->debug_overtempcal.latest_offset.offset[0] *
over_temp_cal->otc_unit_conversion,
@@ -1608,8 +1618,7 @@
CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset
.offset_temp_celsius,
3),
- (unsigned long long int)
- over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos);
+ over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos);
// clang-format off
over_temp_cal->wait_timer_nanos =
@@ -1623,9 +1632,9 @@
case OTC_PRINT_MODEL_PARAMETERS:
// Prints out the model parameters.
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ "Cal#|Sensitivity [%s/C]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
+ over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
over_temp_cal->otc_unit_conversion,
@@ -1640,9 +1649,9 @@
3));
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ "Cal#|Intercept [%s]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
+ over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
over_temp_cal->debug_overtempcal.sensor_intercept[0] *
over_temp_cal->otc_unit_conversion,
@@ -1667,12 +1676,11 @@
// Computes the maximum error over all of the model data.
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
- "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
- (unsigned long int)over_temp_cal->debug_num_model_updates,
- (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
+ "Cal#|#Updates|#ModelPts|Model Error [%s]: %zu, "
+ "%zu, %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
+ over_temp_cal->debug_num_model_updates,
+ over_temp_cal->debug_overtempcal.num_model_pts,
CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
over_temp_cal->otc_unit_conversion,
3),
@@ -1695,10 +1703,9 @@
if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- " Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
- (unsigned long int)over_temp_cal->model_counter,
- over_temp_cal->otc_unit_tag,
+ " Model[%zu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+ over_temp_cal->model_counter, over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[over_temp_cal->model_counter]
.offset[0] *
@@ -1718,8 +1725,7 @@
over_temp_cal->model_data[over_temp_cal->model_counter]
.offset_temp_celsius,
3),
- (unsigned long long int)over_temp_cal
- ->model_data[over_temp_cal->model_counter]
+ over_temp_cal->model_data[over_temp_cal->model_counter]
.offset_age_nanos);
over_temp_cal->model_counter++;
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 18e2bef..1f84944 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_model.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h
@@ -1,3 +1,19 @@
+/*
+ * 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_OVER_TEMP_OVER_TEMP_MODEL_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
index a501331..d1b6619 100644
--- a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
@@ -85,8 +85,8 @@
if (sample_rate_estimator->num_intervals_collected >
sample_rate_estimator->num_intervals_to_collect) {
sample_rate_estimator->mean_sampling_rate_estimate_hz =
- SEC_TO_NANOS(1) * sample_rate_estimator->num_intervals_collected /
- sample_rate_estimator->interval_accumulator_nanos;
+ sample_rate_estimator->num_intervals_collected *
+ (SEC_TO_NANOS(1) / sample_rate_estimator->interval_accumulator_nanos);
// Sets the polling flag to indicate that a new estimate is ready.
sample_rate_estimator->new_sampling_rate_estimate_ready = true;
diff --git a/firmware/os/algos/calibration/sphere_fit/calibration_data.c b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
new file mode 100644
index 0000000..2de4a8c
--- /dev/null
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
@@ -0,0 +1,52 @@
+/*
+ * 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/sphere_fit/calibration_data.h"
+
+#include <string.h>
+
+#include "common/math/vec.h"
+
+// FUNCTION IMPLEMENTATIONS
+//////////////////////////////////////////////////////////////////////////////
+
+// Set calibration data to identity scale factors, zero skew and
+// zero bias.
+void calDataReset(struct ThreeAxisCalData* calstruct) {
+ memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
+ calstruct->scale_factor_x = 1.0f;
+ calstruct->scale_factor_y = 1.0f;
+ calstruct->scale_factor_z = 1.0f;
+}
+
+void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
+ const float x_impaired[THREE_AXIS_DIM],
+ float* x_corrected) {
+ // x_temp = (x_impaired - bias).
+ float x_temp[THREE_AXIS_DIM];
+ vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
+
+ // x_corrected = scale_skew_mat * x_temp, where:
+ // scale_skew_mat = [scale_factor_x 0 0
+ // skew_yx scale_factor_y 0
+ // skew_zx skew_zy scale_factor_z].
+ x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
+ x_corrected[1] =
+ calstruct->skew_yx * x_temp[0] + calstruct->scale_factor_y * x_temp[1];
+ x_corrected[2] = calstruct->skew_zx * x_temp[0] +
+ calstruct->skew_zy * x_temp[1] +
+ calstruct->scale_factor_z * x_temp[2];
+}
diff --git a/firmware/os/algos/calibration/sphere_fit/calibration_data.h b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
new file mode 100644
index 0000000..405d0ac
--- /dev/null
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
@@ -0,0 +1,73 @@
+/*
+ * 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 contains a data structure and corresponding helper functions for
+ * a three-axis sensor calibration. The calibration consists of a bias vector,
+ * bias, and a lower-diagonal scaling and skew matrix, scale_skew_mat.
+ *
+ * The calibration is applied to impaired sensor data as follows:
+ *
+ * corrected_data = scale_skew_mat * (impaired_data - bias).
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define THREE_AXIS_DIM (3)
+
+// Calibration data structure.
+struct ThreeAxisCalData {
+ // Scale factor and skew terms. Used to construct the following lower
+ // diagonal scale_skew_mat:
+ // scale_skew_mat = [scale_factor_x 0 0
+ // skew_yx scale_factor_y 0
+ // skew_zx skew_zy scale_factor_z].
+ float scale_factor_x;
+ float scale_factor_y;
+ float scale_factor_z;
+ float skew_yx;
+ float skew_zx;
+ float skew_zy;
+
+ // Sensor bias offset.
+ float bias[THREE_AXIS_DIM];
+
+ // Calibration time.
+ uint64_t calibration_time_nanos;
+};
+
+// Set calibration data to identity scale factors, zero skew and
+// zero bias.
+void calDataReset(struct ThreeAxisCalData* calstruct);
+
+// Apply a stored calibration to correct a single sample of impaired sensor
+// data.
+void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
+ const float x_impaired[THREE_AXIS_DIM],
+ float* x_corrected);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
new file mode 100644
index 0000000..853a73d
--- /dev/null
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -0,0 +1,264 @@
+/*
+ * 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/sphere_fit/sphere_fit_calibration.h"
+
+#include <errno.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+#include "common/math/mat.h"
+#include "common/math/vec.h"
+
+// FORWARD DECLARATIONS
+///////////////////////////////////////////////////////////////////////////////
+// Utility for converting solver state to a calibration data structure.
+static void convertStateToCalStruct(const float x[SF_STATE_DIM],
+ struct ThreeAxisCalData *calstruct);
+
+static bool runCalibration(struct SphereFitCal *sphere_cal,
+ const struct SphereFitData *data,
+ uint64_t timestamp_nanos);
+
+#define MIN_VALID_DATA_NORM (1e-4f)
+
+// FUNCTION IMPLEMENTATIONS
+//////////////////////////////////////////////////////////////////////////////
+void sphereFitInit(struct SphereFitCal *sphere_cal,
+ const struct LmParams *lm_params,
+ const size_t min_num_points_for_cal) {
+ ASSERT_NOT_NULL(sphere_cal);
+ ASSERT_NOT_NULL(lm_params);
+
+ // Initialize LM solver.
+ lmSolverInit(&sphere_cal->lm_solver, lm_params,
+ &sphereFitResidAndJacobianFunc);
+
+ // Reset other parameters.
+ sphereFitReset(sphere_cal);
+
+ // Set num points for calibration, checking that it is above min.
+ if (min_num_points_for_cal < MIN_NUM_SPHERE_FIT_POINTS) {
+ sphere_cal->min_points_for_cal = MIN_NUM_SPHERE_FIT_POINTS;
+ } else {
+ sphere_cal->min_points_for_cal = min_num_points_for_cal;
+ }
+}
+
+void sphereFitReset(struct SphereFitCal *sphere_cal) {
+ ASSERT_NOT_NULL(sphere_cal);
+
+ // Set state to default (diagonal scale matrix and zero offset).
+ memset(&sphere_cal->x0[0], 0, sizeof(float) * SF_STATE_DIM);
+ sphere_cal->x0[eParamScaleMatrix11] = 1.f;
+ sphere_cal->x0[eParamScaleMatrix22] = 1.f;
+ sphere_cal->x0[eParamScaleMatrix33] = 1.f;
+ memcpy(sphere_cal->x, sphere_cal->x0, sizeof(sphere_cal->x));
+
+ // Reset time.
+ sphere_cal->estimate_time_nanos = 0;
+}
+
+void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
+ struct LmData *lm_data) {
+ ASSERT_NOT_NULL(sphere_cal);
+ ASSERT_NOT_NULL(lm_data);
+
+ // Set solver data.
+ lmSolverSetData(&sphere_cal->lm_solver, lm_data);
+}
+
+bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
+ const struct SphereFitData *data,
+ uint64_t timestamp_nanos) {
+ ASSERT_NOT_NULL(sphere_cal);
+ ASSERT_NOT_NULL(data);
+
+ // Run calibration if have enough points.
+ if (data->num_fit_points >= sphere_cal->min_points_for_cal) {
+ return runCalibration(sphere_cal, data, timestamp_nanos);
+ }
+
+ return false;
+}
+
+void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
+ const float initial_bias[THREE_AXIS_DIM]) {
+ ASSERT_NOT_NULL(sphere_cal);
+ sphere_cal->x0[eParamOffset1] = initial_bias[0];
+ sphere_cal->x0[eParamOffset2] = initial_bias[1];
+ sphere_cal->x0[eParamOffset3] = initial_bias[2];
+}
+
+void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
+ struct ThreeAxisCalData *cal_data) {
+ ASSERT_NOT_NULL(sphere_cal);
+ ASSERT_NOT_NULL(cal_data);
+ convertStateToCalStruct(sphere_cal->x, cal_data);
+ cal_data->calibration_time_nanos = sphere_cal->estimate_time_nanos;
+}
+
+void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
+ float *residual, float *jacobian) {
+ ASSERT_NOT_NULL(state);
+ ASSERT_NOT_NULL(f_data);
+ ASSERT_NOT_NULL(residual);
+
+ const struct SphereFitData *data = (const struct SphereFitData *)f_data;
+
+ // Verify that expected norm is non-zero, else use default of 1.0.
+ float expected_norm = 1.0;
+ ASSERT(data->expected_norm > MIN_VALID_DATA_NORM);
+ if (data->expected_norm > MIN_VALID_DATA_NORM) {
+ expected_norm = data->expected_norm;
+ }
+
+ // Convert parameters to calibration structure.
+ struct ThreeAxisCalData calstruct;
+ convertStateToCalStruct(state, &calstruct);
+
+ // Compute Jacobian helper matrix if Jacobian requested.
+ //
+ // J = d(||M(x_data - bias)|| - expected_norm)/dstate
+ // = (M(x_data - bias) / ||M(x_data - bias)||) * d(M(x_data - bias))/dstate
+ // = x_corr / ||x_corr|| * A
+ // A = d(M(x_data - bias))/dstate
+ // = [dy/dM11, dy/dM21, dy/dM22, dy/dM31, dy/dM32, dy/dM3,...
+ // dy/db1, dy/db2, dy/db3]'
+ // where:
+ // dy/dM11 = [x_data[0] - bias[0], 0, 0]
+ // dy/dM21 = [0, x_data[0] - bias[0], 0]
+ // dy/dM22 = [0, x_data[1] - bias[1], 0]
+ // dy/dM31 = [0, 0, x_data[0] - bias[0]]
+ // dy/dM32 = [0, 0, x_data[1] - bias[1]]
+ // dy/dM33 = [0, 0, x_data[2] - bias[2]]
+ // dy/db1 = [-scale_factor_x, 0, 0]
+ // dy/db2 = [0, -scale_factor_y, 0]
+ // dy/db3 = [0, 0, -scale_factor_z]
+ float A[SF_STATE_DIM * THREE_AXIS_DIM];
+ if (jacobian) {
+ memset(jacobian, 0, sizeof(float) * SF_STATE_DIM * data->num_fit_points);
+ memset(A, 0, sizeof(A));
+ A[0 * SF_STATE_DIM + eParamOffset1] = -calstruct.scale_factor_x;
+ A[1 * SF_STATE_DIM + eParamOffset2] = -calstruct.scale_factor_y;
+ A[2 * SF_STATE_DIM + eParamOffset3] = -calstruct.scale_factor_z;
+ }
+
+ // Loop over all data points to compute residual and Jacobian.
+ // TODO(dvitus): Use fit_data_std when available to weight residuals.
+ float x_corr[THREE_AXIS_DIM];
+ float x_bias_corr[THREE_AXIS_DIM];
+ size_t i;
+ for (i = 0; i < data->num_fit_points; ++i) {
+ const float *x_data = &data->fit_data[i * THREE_AXIS_DIM];
+
+ // Compute corrected sensor data
+ calDataCorrectData(&calstruct, x_data, x_corr);
+
+ // Compute norm of x_corr.
+ const float norm = vecNorm(x_corr, THREE_AXIS_DIM);
+
+ // Compute residual error: f_x = norm - exp_norm
+ residual[i] = norm - data->expected_norm;
+
+ // Compute Jacobian if valid pointer.
+ if (jacobian) {
+ if (norm < MIN_VALID_DATA_NORM) {
+ return;
+ }
+ const float scale = 1.f / norm;
+
+ // Compute bias corrected data.
+ vecSub(x_bias_corr, x_data, calstruct.bias, THREE_AXIS_DIM);
+
+ // Populate non-bias terms for A
+ A[0 + eParamScaleMatrix11] = x_bias_corr[0];
+ A[1 * SF_STATE_DIM + eParamScaleMatrix21] = x_bias_corr[0];
+ A[1 * SF_STATE_DIM + eParamScaleMatrix22] = x_bias_corr[1];
+ A[2 * SF_STATE_DIM + eParamScaleMatrix31] = x_bias_corr[0];
+ A[2 * SF_STATE_DIM + eParamScaleMatrix32] = x_bias_corr[1];
+ A[2 * SF_STATE_DIM + eParamScaleMatrix33] = x_bias_corr[2];
+
+ // Compute J = x_corr / ||x_corr|| * A
+ matTransposeMultiplyVec(&jacobian[i * SF_STATE_DIM], A, x_corr,
+ THREE_AXIS_DIM, SF_STATE_DIM);
+ vecScalarMulInPlace(&jacobian[i * SF_STATE_DIM], scale, SF_STATE_DIM);
+ }
+ }
+}
+
+void convertStateToCalStruct(const float x[SF_STATE_DIM],
+ struct ThreeAxisCalData *calstruct) {
+ memcpy(&calstruct->bias[0], &x[eParamOffset1],
+ sizeof(float) * THREE_AXIS_DIM);
+ calstruct->scale_factor_x = x[eParamScaleMatrix11];
+ calstruct->skew_yx = x[eParamScaleMatrix21];
+ calstruct->scale_factor_y = x[eParamScaleMatrix22];
+ calstruct->skew_zx = x[eParamScaleMatrix31];
+ calstruct->skew_zy = x[eParamScaleMatrix32];
+ calstruct->scale_factor_z = x[eParamScaleMatrix33];
+}
+
+bool runCalibration(struct SphereFitCal *sphere_cal,
+ const struct SphereFitData *data,
+ uint64_t timestamp_nanos) {
+ float x_sol[SF_STATE_DIM];
+
+ // Run calibration
+ const enum LmStatus status =
+ lmSolverSolve(&sphere_cal->lm_solver, sphere_cal->x0, (void *)data,
+ SF_STATE_DIM, data->num_fit_points, x_sol);
+
+ // Check if solver was successful
+ if (status == RELATIVE_STEP_SUFFICIENTLY_SMALL ||
+ status == GRADIENT_SUFFICIENTLY_SMALL) {
+ // TODO(dvitus): Check quality of new fit before using.
+ // Store new fit.
+#ifdef SPHERE_FIT_DBG_ENABLED
+ CAL_DEBUG_LOG("[SPHERE CAL]",
+ "Solution found in %d iterations with status %d.\n",
+ sphere_cal->lm_solver.num_iter, status);
+ CAL_DEBUG_LOG("[SPHERE CAL]", "Solution:\n {"
+ CAL_FORMAT_6DIGITS " [M(1,1)], "
+ CAL_FORMAT_6DIGITS " [M(2,1)], "
+ CAL_FORMAT_6DIGITS " [M(2,2)], \n"
+ CAL_FORMAT_6DIGITS " [M(3,1)], "
+ CAL_FORMAT_6DIGITS " [M(3,2)], "
+ CAL_FORMAT_6DIGITS " [M(3,3)], \n"
+ CAL_FORMAT_6DIGITS " [b(1)], "
+ CAL_FORMAT_6DIGITS " [b(2)], "
+ CAL_FORMAT_6DIGITS " [b(3)]}.",
+ CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
+ CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
+ CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
+ CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
+ CAL_ENCODE_FLOAT(x_sol[8], 6));
+#endif // SPHERE_FIT_DBG_ENABLED
+ memcpy(sphere_cal->x, x_sol, sizeof(x_sol));
+ sphere_cal->estimate_time_nanos = timestamp_nanos;
+ return true;
+ } else {
+#ifdef SPHERE_FIT_DBG_ENABLED
+ CAL_DEBUG_LOG("[SPHERE CAL]",
+ "Solution failed in %d iterations with status %d.\n",
+ sphere_cal->lm_solver.num_iter, status);
+#endif // SPHERE_FIT_DBG_ENABLED
+ }
+
+ return false;
+}
diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
new file mode 100644
index 0000000..d3bbf7f
--- /dev/null
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
@@ -0,0 +1,144 @@
+/*
+ * 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 contains an algorithm for performing a sphere fit calibration.
+ * A sphere fit calibration solves the following non-linear least squares
+ * problem:
+ *
+ * arg min || ||M(x - b)|| - exp_norm ||
+ * M,b
+ *
+ * where:
+ * x is a 3xN matrix containing N 3-dimensional uncalibrated data points,
+ * M is a 3x3 lower diagonal scaling matrix
+ * b is a 3x1 offset vector.
+ * exp_norm is the expected norm of an individual calibration data point.
+ * M and b are solved such that the norm of the calibrated data (M(x - b)) is
+ * near exp_norm.
+ *
+ * This module uses a Levenberg-Marquardt nonlinear least squares solver to find
+ * M and b. M is assumed to be a lower diagonal, consisting of 6 parameters.
+ *
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "calibration/sphere_fit/calibration_data.h"
+#include "common/math/levenberg_marquardt.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MIN_NUM_SPHERE_FIT_POINTS (14)
+
+// Enum defining the meaning of the state parameters. The 9-parameter
+// sphere fit calibration computes a lower-diagonal scaling matrix (M) and
+// an offset such that:
+// x_corrected = M * (x_impaired - offset)
+enum SphereFitParams {
+ eParamScaleMatrix11 = 0,
+ eParamScaleMatrix21,
+ eParamScaleMatrix22,
+ eParamScaleMatrix31,
+ eParamScaleMatrix32,
+ eParamScaleMatrix33,
+ eParamOffset1,
+ eParamOffset2,
+ eParamOffset3,
+ SF_STATE_DIM
+};
+
+// Structure containing the data to be used for the sphere fit calibration.
+struct SphereFitData {
+ // Data for fit (assumed to be a matrix of size num_fit_points x SF_DATA_DIM)
+ const float *fit_data;
+
+ // Pointer to standard deviations of the fit data, used to weight individual
+ // data points. Assumed to point to a matrix of dimensions
+ // num_fit_points x THREE_AXIS_DIM.
+ // If NULL, data will all be used with equal weighting in the fit.
+ const float *fit_data_std;
+
+ // Number of fit points.
+ size_t num_fit_points;
+
+ // Expected data norm.
+ float expected_norm;
+};
+
+// Structure for a sphere fit calibration, including a non-linear least squares
+// solver and the latest state estimate.
+struct SphereFitCal {
+ // Levenberg-Marquardt solver.
+ struct LmSolver lm_solver;
+
+ // Minimum number of points for computing a calibration.
+ size_t min_points_for_cal;
+
+ // State estimate.
+ float x[SF_STATE_DIM];
+ uint64_t estimate_time_nanos;
+
+ // Initial state for solver.
+ float x0[SF_STATE_DIM];
+};
+
+// Initialize sphere fit calibration structure with solver and fit params.
+void sphereFitInit(struct SphereFitCal *sphere_cal,
+ const struct LmParams *lm_params,
+ const size_t min_num_points_for_cal);
+
+// Clears state estimate and initial state.
+void sphereFitReset(struct SphereFitCal *sphere_cal);
+
+// Sets data pointer for single solve of the Levenberg-Marquardt solver.
+// Must be called before calling sphereFitRunCal().
+void sphereFitSetSolverData(struct SphereFitCal *sphere_cal,
+ struct LmData *lm_data);
+
+// Sends in a set of calibration data and attempts to run calibration.
+// Returns true if a calibration was successfully triggered with this data.
+bool sphereFitRunCal(struct SphereFitCal *sphere_cal,
+ const struct SphereFitData *data,
+ uint64_t timestamp_nanos);
+
+// Set an initial condition for the bias state.
+void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal,
+ const float initial_bias[THREE_AXIS_DIM]);
+
+// Returns the latest calibration data in a ThreeAxisCalData structure.
+void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal,
+ struct ThreeAxisCalData *cal_data);
+
+///////////////// TEST UTILITIES ///////////////////////////////////////////
+// The following functions are exposed in the header for testing only.
+
+// The ResidualAndJacobianFunction for sphere calibration in the
+// Levenberg-Marquardt solver.
+void sphereFitResidAndJacobianFunc(const float *state, const void *f_data,
+ float *residual, float *jacobian);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 593c087..46297db 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -16,7 +16,7 @@
///////////////////////////////////////////////////////////////
/*
- * Logging macros for printing formatted strings to Nanohub.
+ * Logging macros for printing user-debug messages.
*/
///////////////////////////////////////////////////////////////
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
@@ -36,10 +36,18 @@
LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
# define CAL_DEBUG_LOG(tag, fmt, ...) \
osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
-#else // _OS_BUILD_
+#elif NANOHUB_DEBUG_LOG
# include <chre.h>
# define CAL_DEBUG_LOG(tag, fmt, ...) \
chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
+#else
+// CHRE/SLPI Nanoapp Logging.
+# include "chre/util/nanoapp/log.h"
+# ifndef LOG_TAG
+# define LOG_TAG ""
+# endif // LOG_TAG
+# define CAL_DEBUG_LOG(tag, format, ...) \
+ LOGI("%s " format, tag, ##__VA_ARGS__)
#endif // GCC_DEBUG_LOG
// clang-format on
@@ -47,22 +55,42 @@
extern "C" {
#endif
-// Using this macro because floorf() is not currently implemented by the Nanohub
-// firmware.
+// Floor macro implementation for platforms that do not supply the standard
+// floorf() math function.
#define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT
+/*
+ * On some embedded software platforms numerical string formatting is not fully
+ * supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that
+ * prints floating-point values having a specified number of digits using the
+ * CAL_ENCODE_FLOAT macro.
+ * Examples:
+ * - Nanohub does not support floating-point format strings.
+ * - CHRE/SLPI allows %.Xf for printing float values.
+ */
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
// Macro used to print floating point numbers with a specified number of digits.
-#define CAL_ENCODE_FLOAT(x, num_digits) \
- ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)), \
- (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * \
+# define CAL_ENCODE_FLOAT(x, num_digits) \
+ ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)), \
+ (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * \
powf(10, num_digits)) // NOLINT
// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
// desired significant digits.
-#define CAL_FORMAT_3DIGITS "%s%d.%03d"
-#define CAL_FORMAT_6DIGITS "%s%d.%06d"
-#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
-#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+# define CAL_FORMAT_3DIGITS "%s%d.%03d"
+# define CAL_FORMAT_6DIGITS "%s%d.%06d"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+#else
+// Pass-through when float string formatting (e.g., %.6f) is available.
+# define CAL_ENCODE_FLOAT(x, num_digits) (x)
+
+// Float string formatting helpers.
+# define CAL_FORMAT_3DIGITS "%.3f"
+# define CAL_FORMAT_6DIGITS "%.6f"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%.3f, %.3f, %.3f"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%.6f, %.6f, %.6f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
#ifdef __cplusplus
}
