diff options
| author | David Jacobs <davejacobs@google.com> | 2018-04-17 16:55:46 -0400 |
|---|---|---|
| committer | David Jacobs <davejacobs@google.com> | 2018-05-02 06:16:58 +0000 |
| commit | ce2dd9290fce64fad2ad6cf8a8342de40e6cbcaa (patch) | |
| tree | c8037e3620298f6ea23a6ba7969e3be71032e8b2 | |
| parent | b4766557d14eef22527ca54417a158a2cccb7648 (diff) | |
| download | contexthub-ce2dd9290fce64fad2ad6cf8a8342de40e6cbcaa.tar.gz | |
[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
30 files changed, 1384 insertions, 514 deletions
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index 3c3160f5..99e96ef7 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 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, #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 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, } #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 9da7aa45..90b25446 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 @@ void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, #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 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { 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 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { 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 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { #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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, : -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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, 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 deb26b3f..bb2063b6 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 index 40749e1a..c3f12ae6 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 @@ static int moc_eigen_test(struct KasaFit *kasa) { 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 @@ static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { } 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 @@ void initMagCal(struct MagCal *moc, #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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, // 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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 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 @@ enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 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 @@ void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, *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 @@ void magLogPrint(struct DiversityChecker *diverse_data, float temp) { 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 @@ void magLogPrint(struct DiversityChecker *diverse_data, float temp) { 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 @@ void magLogPrint(struct DiversityChecker *diverse_data, float temp) { } 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 @@ void magLogPrint(struct DiversityChecker *diverse_data, float temp) { } } } -#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 index 381a925b..bbc079cb 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 MagCalParameters { }; struct MagCal { -#ifdef DIVERSITY_CHECK_ENABLED struct DiversityChecker diversity_checker; -#endif struct KasaFit kasa; uint64_t start_time; // [micro-seconds] @@ -89,21 +85,16 @@ struct MagCal { #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 magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, 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 @@ void magLogPrint(struct DiversityChecker *moc, float temp); } #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 index 3e23dcfd..93c2ac61 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 index 7c2fc864..3ae4687b 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 @@ void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz); } #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 88557ad9..12b84701 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 68b477b7..a5de5d7e 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 @@ using ::online_calibration::SensorType; #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 @@ void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal, 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 @@ void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal, 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 @@ void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal, 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 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { 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 @@ bool NanoSensorCal::NotifyAshCalibration( 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 @@ bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, 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 && + // 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 (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME && ash_cal_parameters->offsetTempCelsiusSource == - ASH_CAL_PARAMS_SOURCE_FACTORY; + ASH_CAL_PARAMS_SOURCE_RUNTIME) { + runtime_cal_detected = true; + *flags = CalibrationTypeFlags::BIAS; + } - bool runtime_cal_detected = false; - if (factory_cal_detected) { - // Prints the retrieved factory calibration data. - NANO_CAL_LOGI(sensor_tag, "Factory calibration detected."); - PrintAshCalParams(*ash_cal_parameters, sensor_tag); + if (ash_cal_parameters->tempSensitivitySource == + ASH_CAL_PARAMS_SOURCE_RUNTIME && + ash_cal_parameters->tempInterceptSource == + ASH_CAL_PARAMS_SOURCE_RUNTIME) { + *flags |= CalibrationTypeFlags::OVER_TEMP; + } - // 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; + 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); } 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 @@ bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, // 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); - - 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]); - - 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.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); + } + + 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]); + } + + 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]); + } + + 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]); + } + + 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 59850545..6b19f7ef 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 @@ class NanoSensorCal { 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 @@ class NanoSensorCal { // 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 3958ae85..e9def4e6 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 @@ CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) { 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 @@ bool AccelOffsetCal::SetInitialCalibration( // 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 d2ec494c..0256495a 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 @@ namespace online_calibration { /* * This class is a wrapper for accelerometer offset calibration. + * + * NOTE: Calibration quality reporting (quantitative metric is not used): + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::LOW_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality */ class AccelOffsetCal final : public OnlineCalibration<CalibrationDataThreeAxis> { 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 96c061f8..62bc443d 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 23ebe4fa..8f1470a8 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 8b95ef7f..0404936b 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 @@ namespace online_calibration { * 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 @@ enum class CalibrationTypeFlags : uint8_t { SCALE_FACTOR = 0x02, CROSS_AXIS = 0x04, OVER_TEMP = 0x08, + QUALITY_DEGRADED = 0x10, ALL = 0xFF }; @@ -102,11 +122,16 @@ struct CalibrationDataThreeAxis { // 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 @@ struct CalibrationDataThreeAxis { // 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 @@ struct CalibrationDataSingleAxis { // 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 @@ struct CalibrationDataSingleAxis { // 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 00000000..d6475c78 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides quality definitions that represent the accuracy + * associated with calibration data. This information may be used to affect how + * a system uses available sensor calibration data. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ + +#include <float.h> +#include <stdint.h> + +namespace online_calibration { + +/* + * In general, calibration quality values may be thought of in terms of + * something akin to an error standard deviation. That is, it's a non-negative + * value where lower values imply higher calibration quality, and larger values + * indicate poorer quality. However, the units and numerical interpretation is + * ultimately dictated by the sensor type and calibration algorithm. Consult the + * calibration code implementation for the actual calibration quality metric + * details. + */ + +/* + * Bitmask used to provide a qualitative ranking of the calibration data's + * accuracy. Many systems use this sort of interpretation for calibration + * quality (e.g., Android). + * + * [Bit Flag] | [Qualitative Calibration Quality] + * UNDETERMINED - Calibration quality has not yet been determined (e.g., a + * calibration hasn't occurred, or a metric hasn't been + * established). + * LOW_QUALITY - Sensor calibration is needed. System properties have + * changed that may have affected the applicability of the + * current calibration (e.g., calibration data is old, anomaly + * detected, etc.). + * MEDIUM_QUALITY - The reported sensor calibration has an average level of + * accuracy, updated calibration may improve the readings. + * HIGH_QUALITY - The reported calibration has maximal accuracy. + */ +enum class CalibrationQualityLevel : uint8_t { + UNDETERMINED = 0x00, + LOW_QUALITY = 0x01, + MEDIUM_QUALITY = 0x02, + HIGH_QUALITY = 0x04, +}; + +// Sets the calibration quality value when this metric is either not +// implemented, or has not yet been determined (e.g., a calibration hasn't +// occurred). +constexpr float kUndeterminedCalibrationQuality = -1.0f; + +/* + * Calibration quality structure that contains a quantitative (float) and + * qualitative (enum) measure of a sensor's calibration accuracy. Both entries + * should be co-defined by the algorithm providing the calibration update. The + * enum sets the qualitative interpretation of the float value, this is often + * used in systems (Android, etc.) to label quality and affect the use of the + * calibration data. + */ +struct CalibrationQuality { + // Provides a qualitative measure for sensor calibration accuracy. + CalibrationQualityLevel level = CalibrationQualityLevel::UNDETERMINED; + + // Quantitative metric for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + float value = kUndeterminedCalibrationQuality; + + // Helper function that resets the calibration quality to an initial state. + void reset() { + level = CalibrationQualityLevel::UNDETERMINED; + value = kUndeterminedCalibrationQuality; + } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h index bf348ac2..c8930652 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 9849ad2c..915554f3 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 67ea4974..26eef573 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 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( 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 @@ bool GyroOffsetOtcCal::SetInitialCalibration( 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 74ba9828..e21007b3 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 @@ namespace online_calibration { /* * This class is a wrapper for the gyroscope offset calibration with * over-temperature compensation (OTC). + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::MEDIUM_QUALITY + * CalibrationQuality.value = kMediumQualityRps + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = kHighQualityRps */ class GyroOffsetOtcCal final : public OnlineCalibration<CalibrationDataThreeAxis> { public: + // Estimated upper bounds on gyro offset error (i.e., 3-sigma values). + static constexpr float kMediumQualityRps = 5.23599e-3f; // 300 mDPS + static constexpr float kHighQualityRps = 1.04720e-3f; // 60 mDPS + // Default constructor. GyroOffsetOtcCal() = default; 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 a963dcd5..a4b4dfd7 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 @@ CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) { 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 @@ bool MagDiverseCal::SetInitialCalibration( // 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 23ac3c49..212ffeb9 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 @@ namespace online_calibration { /* * This class is a wrapper for the magnetometer offset calibration with * diversity checking. + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::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 908d383a..6b64cb8f 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 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, // 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 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // 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 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, 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 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, 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 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, 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 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) { 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 @@ float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, // 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, // 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 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 18e2befd..1f849442 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 a501331e..d1b66195 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 @@ void sampleRateEstimatorUpdate( 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 00000000..2de4a8c0 --- /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 00000000..405d0acc --- /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 00000000..853a73d5 --- /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 00000000..d3bbf7f1 --- /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 593c0876..46297dbb 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 } |