[IMU_Cal] Runtime sensor calibration code update.
This code transfers all of the refactored changes developed and
tested in Google3.
Tip of G3 CL: 191014272
Bug: 75333210
Test: ./load_app.sh and verified calibrations on device.
Change-Id: Ieb414468296ce620ecf7fe275595c561c33779c1
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index da402ea..3c3160f 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -15,13 +15,15 @@
*/
#include "calibration/accelerometer/accel_cal.h"
+
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
-#include "calibration/magnetometer/mag_cal.h"
+
#include "calibration/util/cal_log.h"
+// clang-format off
#define KSCALE \
0.101936799f // Scaling from m/s^2 to g (0.101 = 1/(9.81 m/s^2)).
#define KSCALE2 9.81f // Scaling from g to m/s^2.
@@ -38,6 +40,7 @@
#define TEMP_CUT 30 // Separation point for temperature buckets 30 degree C.
#define EIGEN_RATIO 0.35f // EIGEN_RATIO (must be greater than 0.35).
#define EIGEN_MAG 0.97f // Eigen value magnitude (must be greater than 0.97).
+#define ACCEL_NEW_BIAS_THRESHOLD (0.0f) // Bias update detection threshold.
#ifdef ACCEL_CAL_DBG_ENABLED
#define TEMP_HIST_LOW \
16 // Putting all Temp counts in first bucket for temp < 16 degree C.
@@ -47,8 +50,9 @@
#endif
#ifdef IMU_TEMP_DBG_ENABLED
#define IMU_TEMP_DELTA_TIME_NANOS \
- 5000000000 // Printing every 5 seconds IMU temp.
+ 5000000000 // Printing every 5 seconds IMU temp.
#endif
+// clang-format on
/////////// Start Debug //////////////////////
@@ -159,19 +163,29 @@
uint32_t fxb, uint32_t fy, uint32_t fyb,
uint32_t fz, uint32_t fzb, uint32_t fle) {
accelGoodDataInit(&acc->agd, fx, fxb, fy, fyb, fz, fzb, fle);
- initKasa(&acc->akf);
+ kasaInit(&acc->akf);
+}
+
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc) {
+ return fabsf(acc->x_bias - acc->x_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+ fabsf(acc->y_bias - acc->y_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+ fabsf(acc->z_bias - acc->z_bias_new) > ACCEL_NEW_BIAS_THRESHOLD;
}
// Accel cal init.
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
- uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
- uint32_t fz, uint32_t fzb, uint32_t fle) {
+void accelCalInit(struct AccelCal *acc,
+ const struct AccelCalParameters *parameters) {
// Init core accel data.
- accelCalAlgoInit(&acc->ac1[0], fx, fxb, fy, fyb, fz, fzb, fle);
- accelCalAlgoInit(&acc->ac1[1], fx, fxb, fy, fyb, fz, fzb, fle);
+ accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb,
+ parameters->fy, parameters->fyb, parameters->fz,
+ parameters->fzb, parameters->fle);
+ accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb,
+ parameters->fy, parameters->fyb, parameters->fz,
+ parameters->fzb, parameters->fle);
// Stillness Reset.
- accelStillInit(&acc->asd, t0, n_s, th);
+ accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
// Debug data init.
#ifdef ACCEL_CAL_DBG_ENABLED
@@ -267,31 +281,6 @@
return complete;
}
-// Accumulate data for KASA fit.
-static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) {
- // Run accumulators.
- float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
- asd->mean_z * asd->mean_z;
-
- akf->acc_x += asd->mean_x;
- akf->acc_y += asd->mean_y;
- akf->acc_z += asd->mean_z;
- akf->acc_w += w;
-
- akf->acc_xx += asd->mean_x * asd->mean_x;
- akf->acc_xy += asd->mean_x * asd->mean_y;
- akf->acc_xz += asd->mean_x * asd->mean_z;
- akf->acc_xw += asd->mean_x * w;
-
- akf->acc_yy += asd->mean_y * asd->mean_y;
- akf->acc_yz += asd->mean_y * asd->mean_z;
- akf->acc_yw += asd->mean_y * w;
-
- akf->acc_zz += asd->mean_z * asd->mean_z;
- akf->acc_zw += asd->mean_z * w;
- akf->nsamples += 1;
-}
-
// Good data detection, sorting and accumulate the data for Kasa.
static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
float temp) {
@@ -304,42 +293,42 @@
ac1->agd.nx += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative x bucket nxb.
if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) {
ac1->agd.nxb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Y bucket ny.
if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) {
ac1->agd.ny += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative y bucket nyb.
if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) {
ac1->agd.nyb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Z bucket nz.
if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) {
ac1->agd.nz += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative z bucket nzb.
if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) {
ac1->agd.nzb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// The leftover bucket nle.
if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
@@ -348,7 +337,7 @@
ac1->agd.nle += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Checking if all buckets are full.
if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb &&
@@ -357,32 +346,16 @@
// Check if akf->nsamples is zero.
if (ac1->akf.nsamples == 0) {
agdReset(&ac1->agd);
- magKasaReset(&ac1->akf);
+ kasaReset(&ac1->akf);
complete = 0;
return complete;
- } else {
- // Normalize the data to the sample numbers.
- inv = 1.0f / ac1->akf.nsamples;
}
- ac1->akf.acc_x *= inv;
- ac1->akf.acc_y *= inv;
- ac1->akf.acc_z *= inv;
- ac1->akf.acc_w *= inv;
+ // Normalize the data to the sample numbers.
+ kasaNormalize(&ac1->akf);
- ac1->akf.acc_xx *= inv;
- ac1->akf.acc_xy *= inv;
- ac1->akf.acc_xz *= inv;
- ac1->akf.acc_xw *= inv;
-
- ac1->akf.acc_yy *= inv;
- ac1->akf.acc_yz *= inv;
- ac1->akf.acc_yw *= inv;
-
- ac1->akf.acc_zz *= inv;
- ac1->akf.acc_zw *= inv;
-
- // Calculate the temp VAR and MEA.N
+ // Calculate the temp VAR and MEAN.
+ inv = 1.0f / ac1->akf.nsamples;
ac1->agd.var_t =
(ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv;
ac1->agd.mean_t = ac1->agd.acc_t * inv;
@@ -395,7 +368,7 @@
ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
agdReset(&ac1->agd);
- magKasaReset(&ac1->akf);
+ kasaReset(&ac1->akf);
complete = 0;
return complete;
}
@@ -488,11 +461,11 @@
", %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_ENCODE_FLOAT(acc->x_bias_new, 5),
+ CAL_ENCODE_FLOAT(acc->y_bias_new, 5),
+ CAL_ENCODE_FLOAT(acc->z_bias_new, 5));
acc->temp_time_nanos = sample_time_nanos;
- }
+ }
#endif
int temp_gate = 0;
@@ -523,7 +496,8 @@
float radius;
// Grabbing the fit from the MAG cal.
- magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius);
+ kasaFit(&acc->ac1[temp_gate].akf, &bias, &radius, G_NORM_MAX,
+ G_NORM_MIN);
// If offset is too large don't take.
if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF &&
@@ -567,7 +541,7 @@
// Resetting the structs for a new accel cal run.
agdReset(&acc->ac1[temp_gate].agd);
- magKasaReset(&acc->ac1[temp_gate].akf);
+ kasaReset(&acc->ac1[temp_gate].akf);
}
}
}
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 1cfef61..3324875 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -27,7 +27,7 @@
#include <stdint.h>
#include <sys/types.h>
-#include "calibration/magnetometer/mag_cal.h"
+#include "common/math/kasa.h"
#include "common/math/mat.h"
#ifdef __cplusplus
@@ -127,6 +127,25 @@
struct KasaFit akf;
};
+// AccelCal algorithm parameters (see the AccelCal for details).
+struct AccelCalParameters {
+ // t0 -> Sets the time how long the accel has to be still in ns.
+ // n_s -> Defines the minimum number of samples for the stillness.
+ // th -> Sets the threshold for the stillness VAR in (g rms)^2.
+ // fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+ // sphere cap (Bucket) is needed to reach full.
+ uint32_t t0;
+ uint32_t n_s;
+ uint32_t fx;
+ uint32_t fxb;
+ uint32_t fy;
+ uint32_t fyb;
+ uint32_t fz;
+ uint32_t fzb;
+ uint32_t fle;
+ float th;
+};
+
// Complete accel calibration struct.
struct AccelCal {
struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS];
@@ -163,15 +182,15 @@
float y, float z, float temp);
/* This function initializes the accCalRun data struct.
- * t0 -> Sets the time how long the accel has to be still in ns.
- * n_s -> Defines the minimum number of samples for the stillness.
- * th -> Sets the threshold for the stillness VAR in (g rms)^2.
- * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
- * sphere cap (Bucket) is needed to reach full.
+ * [parameters]:
+ * t0 -> Sets the time how long the accel has to be still in ns.
+ * n_s -> Defines the minimum number of samples for the stillness.
+ * th -> Sets the threshold for the stillness VAR in (g rms)^2.
+ * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+ * sphere cap (Bucket) is needed to reach full.
*/
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
- uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
- uint32_t fz, uint32_t fzb, uint32_t fle);
+void accelCalInit(struct AccelCal *acc,
+ const struct AccelCalParameters *parameters);
void accelCalDestroy(struct AccelCal *acc);
@@ -182,6 +201,9 @@
void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z);
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc);
+
#ifdef ACCEL_CAL_DBG_ENABLED
void accelCalDebPrint(struct AccelCal *acc, float temp);
#endif
diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/common/calibration_data.c
deleted file mode 100644
index 9ae72d2..0000000
--- a/firmware/os/algos/calibration/common/calibration_data.c
+++ /dev/null
@@ -1,36 +0,0 @@
-#include "calibration/common/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/common/calibration_data.h b/firmware/os/algos/calibration/common/calibration_data.h
deleted file mode 100644
index b0e6809..0000000
--- a/firmware/os/algos/calibration/common/calibration_data.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * 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_COMMON_CALIBRATION_DATA_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_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_COMMON_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/common/sphere_fit_calibration.c
deleted file mode 100644
index b417506..0000000
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.c
+++ /dev/null
@@ -1,247 +0,0 @@
-#include "calibration/common/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 {%s%d.%06d [M(1,1)], %s%d.%06d [M(2,1)], "
- "%s%d.%06d [M(2,2)], \n"
- "%s%d.%06d [M(3,1)], %s%d.%06d [M(3,2)], %s%d.%06d [M(3,3)], \n"
- "%s%d.%06d [b(1)], %s%d.%06d [b(2)], %s%d.%06d [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
- 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
- }
-
- return false;
-}
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.h b/firmware/os/algos/calibration/common/sphere_fit_calibration.h
deleted file mode 100644
index e49f225..0000000
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * 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_COMMON_SPHERE_FIT_CALIBRATION_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
-
-#include <stdbool.h>
-#include <stdint.h>
-
-#include "calibration/common/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_COMMON_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
similarity index 68%
rename from firmware/os/algos/calibration/common/diversity_checker.c
rename to firmware/os/algos/calibration/diversity_checker/diversity_checker.c
index 3655147..3fab81f 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
@@ -14,52 +14,46 @@
* limitations under the License.
*/
-#include "calibration/common/diversity_checker.h"
+#include "calibration/diversity_checker/diversity_checker.h"
#include <errno.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>
+
#include "common/math/vec.h"
// Struct initialization.
-void diversityCheckerInit(
- struct DiversityChecker* diverse_data,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold,
- float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param) {
+void diversityCheckerInit(struct DiversityChecker* diverse_data,
+ const struct DiversityCheckerParameters* parameters) {
ASSERT_NOT_NULL(diverse_data);
// Initialize parameters.
diverse_data->threshold_tuning_param_sq =
- (threshold_tuning_param * threshold_tuning_param);
+ (parameters->threshold_tuning_param * parameters->threshold_tuning_param);
diverse_data->max_distance_tuning_param_sq =
- (max_distance_tuning_param * max_distance_tuning_param);
+ (parameters->max_distance_tuning_param *
+ parameters->max_distance_tuning_param);
// Updating the threshold and max_distance using assumed local field.
// Testing for zero and negative local_field.
- if (local_field <= 0) {
- local_field = 1;
- }
+ const float local_field =
+ (parameters->local_field <= 0.0f) ? 1.0f : parameters->local_field;
diversityCheckerLocalFieldUpdate(diverse_data, local_field);
- diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
+ diverse_data->min_num_diverse_vectors = parameters->min_num_diverse_vectors;
// Checking for min_num_diverse_vectors = 0.
- if (min_num_diverse_vectors < 1) {
+ if (parameters->min_num_diverse_vectors < 1) {
diverse_data->min_num_diverse_vectors = 1;
}
- diverse_data->max_num_max_distance = max_num_max_distance;
- diverse_data->var_threshold = var_threshold;
- diverse_data->max_min_threshold = max_min_threshold;
+ diverse_data->max_num_max_distance = parameters->max_num_max_distance;
+ diverse_data->var_threshold = parameters->var_threshold;
+ diverse_data->max_min_threshold = parameters->max_min_threshold;
// Setting the rest to zero.
diversityCheckerReset(diverse_data);
- // Debug Messages
+ // Debug Messages
#ifdef DIVERSE_DEBUG_ENABLE
memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg));
#endif
@@ -69,8 +63,7 @@
void diversityCheckerReset(struct DiversityChecker* diverse_data) {
ASSERT_NOT_NULL(diverse_data);
// Clear data memory.
- memset(&diverse_data->diverse_data, 0,
- sizeof(diverse_data->diverse_data));
+ memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data));
// Resetting counters and data full bit.
diverse_data->num_points = 0;
@@ -78,13 +71,13 @@
diverse_data->data_full = false;
}
-int32_t diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
- float x, float y, float z) {
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+ float x, float y, float z) {
// Converting three single inputs to a vector.
- const float vec[3] = {x, y, z};
+ const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
// Result vector for vector difference.
- float vec_diff[3];
+ float vec_diff[THREE_AXIS_DATA_DIM];
// normSquared result (k)
float norm_squared_result;
@@ -94,9 +87,7 @@
// Running over all existing data points
for (i = 0; i < diverse_data->num_points; ++i) {
// v = v1 - v2;
- vecSub(vec_diff,
- &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
- vec,
+ vecSub(vec_diff, &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec,
THREE_AXIS_DATA_DIM);
// k = |v|^2
@@ -104,41 +95,40 @@
// if k < Threshold then leave the function.
if (norm_squared_result < diverse_data->threshold) {
- return (int32_t)i;
+ return false;
}
// if k > max_distance, count and leave the function.
if (norm_squared_result > diverse_data->max_distance) {
diverse_data->num_max_dist_violations++;
- return -2;
+ return false;
}
}
- return -1;
+ return true;
}
-void diversityCheckerUpdate(
- struct DiversityChecker* diverse_data, float x, float y, float z) {
+void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
+ float y, float z) {
ASSERT_NOT_NULL(diverse_data);
// If memory is full, no need to run through the data.
if (!diverse_data->data_full) {
-
- // diversityCheckerDataSet() returns -1, if input data is diverse against
+ // diversityCheckerDataSet() returns true, if input data is diverse against
// the already stored.
- if (diversityCheckerFindNearestPoint(diverse_data, x, y, z) == -1) {
+ if (diversityCheckerFindNearestPoint(diverse_data, x, y, z)) {
// Converting three single inputs to a vector.
- const float vec[3] = {x, y, z};
+ const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
// Notice that the first data vector will be stored no matter what.
- memcpy(&diverse_data->
- diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
- vec,
- sizeof(float) * THREE_AXIS_DATA_DIM);
+ memcpy(
+ &diverse_data
+ ->diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
+ vec, sizeof(float) * THREE_AXIS_DATA_DIM);
// Count new data point.
diverse_data->num_points++;
- // Setting data_full to 1, if memory is full.
+ // Setting data_full to true, if memory is full.
if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
diverse_data->data_full = true;
}
@@ -147,18 +137,16 @@
}
bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
- float x_bias,
- float y_bias,
- float z_bias) {
+ float x_bias, float y_bias, float z_bias) {
ASSERT_NOT_NULL(diverse_data);
// If not enough diverse data points or max distance violations return false.
if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
diverse_data->num_max_dist_violations >=
- diverse_data->max_num_max_distance) {
+ diverse_data->max_num_max_distance) {
return false;
}
- float vec_bias[3] = {x_bias, y_bias, z_bias};
- float vec_bias_removed[3];
+ float vec_bias[THREE_AXIS_DATA_DIM] = {x_bias, y_bias, z_bias};
+ float vec_bias_removed[THREE_AXIS_DATA_DIM];
float norm_results;
float acc_norm = 0.0f;
float acc_norm_square = 0.0f;
@@ -168,8 +156,7 @@
for (i = 0; i < diverse_data->num_points; ++i) {
// v = v1 - v_bias;
vecSub(vec_bias_removed,
- &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
- vec_bias,
+ &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec_bias,
THREE_AXIS_DATA_DIM);
// norm = ||v||
@@ -177,7 +164,7 @@
// Accumulate for mean and VAR.
acc_norm += norm_results;
- acc_norm_square += norm_results * norm_results ;
+ acc_norm_square += norm_results * norm_results;
if (i == 0) {
min = norm_results;
@@ -219,11 +206,11 @@
float local_field) {
if (local_field > 0) {
// Updating threshold based on the local field information.
- diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
- (local_field * local_field);
+ diverse_data->threshold =
+ diverse_data->threshold_tuning_param_sq * (local_field * local_field);
// Updating max distance based on the local field information.
diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
- (local_field * local_field);
+ (local_field * local_field);
}
}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
similarity index 84%
rename from firmware/os/algos/calibration/common/diversity_checker.h
rename to firmware/os/algos/calibration/diversity_checker/diversity_checker.h
index 62a1b3e..fe95196 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.h
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
@@ -41,8 +41,8 @@
* full. This has been done in order to save processing power.
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
#include <stdbool.h>
#include <stddef.h>
@@ -68,6 +68,17 @@
};
#endif
+// DiversityChecker parameters container.
+struct DiversityCheckerParameters {
+ float var_threshold;
+ float max_min_threshold;
+ float local_field;
+ float threshold_tuning_param;
+ float max_distance_tuning_param;
+ size_t min_num_diverse_vectors;
+ size_t max_num_max_distance;
+};
+
// Main data struct.
struct DiversityChecker {
// Data memory.
@@ -97,7 +108,6 @@
bool data_full;
// Setup variables for NormQuality check.
-
size_t min_num_diverse_vectors;
size_t max_num_max_distance;
float var_threshold;
@@ -109,7 +119,7 @@
#endif
};
-// Initialization of the function/struct, input:
+// Initialization of the function/struct, input parameters struct consists of:
// min_num_diverse_vectors -> sets the gate for a minimum number of data points
// in the memory
// max_num_max_distance -> sets the value for a max distance violation number
@@ -123,22 +133,17 @@
// max_distance_tuning_param -> Max distance tuning parameter used to calculate
// max_distance.
void diversityCheckerInit(struct DiversityChecker* diverse_data,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance, float var_threshold,
- float max_min_threshold, float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param);
+ const struct DiversityCheckerParameters* parameters);
// Resetting the memory and the counters, leaves threshold and max_distance
// as well as the setup variables for NormQuality check untouched.
void diversityCheckerReset(struct DiversityChecker* diverse_data);
-// Checks if data point (x, y, z) is diverse against the diverse_data set. It
-// returns -1: when the input point is diverse or the index to which vector the
-// input is diverse
- // returns -2: when a maximum distance check is violated
-int32_t diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
- float x, float y, float z);
+// Checks if data point (x, y, z) is diverse against the diverse_data set.
+// Returns true when the input point is diverse.
+// Returns false when a maximum distance check is violated.
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+ float x, float y, float z);
// Main function. Tests the data (x,y,z) against the memory if diverse and
// stores it, if so.
@@ -156,9 +161,7 @@
// -> norm must be within a MAX/MIN window.
// Returned value will only be true if all 4 gates are passed.
bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
- float x_bias,
- float y_bias,
- float z_bias);
+ float x_bias, float y_bias, float z_bias);
// This function updates the threshold value and max distance value based on the
// local field. This ensures a local field independent operation of the
@@ -172,4 +175,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 3179b0e..9da7aa4 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -40,6 +40,10 @@
// A debug version label to help with tracking results.
#define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
+// Parameters used for sample rate estimation.
+#define GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS (100)
+#define GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC (1.0f)
+
// Debug log tag string used to identify debug report output data.
#define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
#endif // GYRO_CAL_DBG_ENABLED
@@ -103,26 +107,6 @@
MAG_STATS_TUNING
};
-/*
- * Updates the running calculation of the gyro's mean sampling rate.
- *
- * Behavior:
- * 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
- * calculation of the sampling rate is copied, and the function returns.
- * 2) Else, if 'reset_stats' is 'true' then the local estimate is reset and
- * the function returns.
- * 3) Otherwise, the local estimate of the mean sampling rates is updated.
- *
- * INPUTS:
- * sample_rate_estimator: Pointer to the estimator data structure.
- * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update.
- * timestamp_nanos: Time stamp (nanoseconds).
- * reset_stats: Flag that signals a reset of the sampling rate estimate.
- */
-static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
- float* debug_mean_sampling_rate_hz,
- uint64_t timestamp_nanos, bool reset_stats);
-
// Updates the information used for debug printouts.
static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
@@ -130,35 +114,13 @@
static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
char* debug_tag,
enum DebugPrintData print_data);
-
-// This conversion function is necessary for Nanohub firmware compilation (i.e.,
-// can't cast a uint64_t to a float directly). This conversion function was
-// copied from: /third_party/contexthub/firmware/src/floatRt.c
-static float floatFromUint64(uint64_t v)
-{
- uint32_t hi = v >> 32, lo = v;
-
- if (!hi) //this is very fast for cases where we fit into a uint32_t
- return (float)lo;
- else {
- return ((float)hi) * 4294967296.0f + (float)lo;
- }
-}
#endif // GYRO_CAL_DBG_ENABLED
/////// FUNCTION DEFINITIONS /////////////////////////////////////////
// Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
- uint64_t max_still_duration_nanos, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos,
- uint64_t window_time_duration_nanos, float gyro_var_threshold,
- float gyro_confidence_delta, float accel_var_threshold,
- float accel_confidence_delta, float mag_var_threshold,
- float mag_confidence_delta, float stillness_threshold,
- float stillness_mean_delta_limit,
- float temperature_delta_limit_celsius,
- bool gyro_calibration_enable) {
+void gyroCalInit(struct GyroCal* gyro_cal,
+ const struct GyroCalParameters* parameters) {
// Clear gyro_cal structure memory.
memset(gyro_cal, 0, sizeof(struct GyroCal));
@@ -166,35 +128,38 @@
// Gyro parameter input units are [rad/sec].
// Accel parameter input units are [m/sec^2].
// Magnetometer parameter input units are [uT].
- gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold,
- gyro_confidence_delta);
- gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold,
- accel_confidence_delta);
- gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold,
- mag_confidence_delta);
+ gyroStillDetInit(&gyro_cal->gyro_stillness_detect,
+ parameters->gyro_var_threshold,
+ parameters->gyro_confidence_delta);
+ gyroStillDetInit(&gyro_cal->accel_stillness_detect,
+ parameters->accel_var_threshold,
+ parameters->accel_confidence_delta);
+ gyroStillDetInit(&gyro_cal->mag_stillness_detect,
+ parameters->mag_var_threshold,
+ parameters->mag_confidence_delta);
// Reset stillness flag and start timestamp.
gyro_cal->prev_still = false;
gyro_cal->start_still_time_nanos = 0;
// Set the min and max window stillness duration.
- gyro_cal->min_still_duration_nanos = min_still_duration_nanos;
- gyro_cal->max_still_duration_nanos = max_still_duration_nanos;
+ gyro_cal->min_still_duration_nanos = parameters->min_still_duration_nanos;
+ gyro_cal->max_still_duration_nanos = parameters->max_still_duration_nanos;
// Sets the duration of the stillness processing windows.
- gyro_cal->window_time_duration_nanos = window_time_duration_nanos;
+ gyro_cal->window_time_duration_nanos = parameters->window_time_duration_nanos;
// Set the watchdog timeout duration.
gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS;
// Load the last valid cal from system memory.
- gyro_cal->bias_x = bias_x; // [rad/sec]
- gyro_cal->bias_y = bias_y; // [rad/sec]
- gyro_cal->bias_z = bias_z; // [rad/sec]
- gyro_cal->calibration_time_nanos = calibration_time_nanos;
+ gyro_cal->bias_x = parameters->bias_x; // [rad/sec]
+ gyro_cal->bias_y = parameters->bias_y; // [rad/sec]
+ gyro_cal->bias_z = parameters->bias_z; // [rad/sec]
+ gyro_cal->calibration_time_nanos = parameters->calibration_time_nanos;
// Set the stillness threshold required for gyro bias calibration.
- gyro_cal->stillness_threshold = stillness_threshold;
+ gyro_cal->stillness_threshold = parameters->stillness_threshold;
// Current window end-time used to assist in keeping sensor data collection in
// sync. Setting this to zero signals that sensor data will be dropped until a
@@ -202,13 +167,14 @@
gyro_cal->stillness_win_endtime_nanos = 0;
// Gyro calibrations will be applied (see, gyroCalRemoveBias()).
- gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0);
+ gyro_cal->gyro_calibration_enable = (parameters->gyro_calibration_enable > 0);
// Sets the stability limit for the stillness window mean acceptable delta.
- gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;
+ gyro_cal->stillness_mean_delta_limit = parameters->stillness_mean_delta_limit;
// Sets the min/max temperature delta limit for the stillness period.
- gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;
+ gyro_cal->temperature_delta_limit_celsius =
+ parameters->temperature_delta_limit_celsius;
// Ensures that the data tracking functionality is reset.
gyroStillMeanTracker(gyro_cal, DO_RESET);
@@ -221,41 +187,47 @@
CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
}
- // Ensures that the gyro sampling rate estimate is reset.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
- /*reset_stats=*/true);
+ // Initializes the gyro sampling rate estimator.
+ sampleRateEstimatorInit(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+ GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS,
+ GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC);
#endif // GYRO_CAL_DBG_ENABLED
}
// Void pointer in the gyro calibration data structure (doesn't do anything
// except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) {
- (void)gyro_cal;
-}
+void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
- float* bias_z, float* temperature_celsius) {
+ float* bias_z, float* temperature_celsius,
+ uint64_t* calibration_time_nanos) {
*bias_x = gyro_cal->bias_x;
*bias_y = gyro_cal->bias_y;
*bias_z = gyro_cal->bias_z;
+ *calibration_time_nanos = gyro_cal->calibration_time_nanos;
*temperature_celsius = gyro_cal->bias_temperature_celsius;
}
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos) {
+ float bias_z, float temperature_celsius,
+ uint64_t calibration_time_nanos) {
gyro_cal->bias_x = bias_x;
gyro_cal->bias_y = bias_y;
gyro_cal->bias_z = bias_z;
gyro_cal->calibration_time_nanos = calibration_time_nanos;
+ gyro_cal->bias_temperature_celsius = temperature_celsius;
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
- "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
+ "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
+ 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);
#endif // GYRO_CAL_DBG_ENABLED
}
@@ -298,8 +270,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Update the gyro sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/false);
+ sampleRateEstimatorUpdate(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+ sample_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Pass gyro data to stillness detector
@@ -396,7 +368,7 @@
// Determines if the device is currently still.
device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
- !mean_not_stable && !min_max_temp_exceeded;
+ !mean_not_stable && !min_max_temp_exceeded;
if (device_is_still) {
// Device is "still" logic:
@@ -440,12 +412,6 @@
computeGyroCal(gyro_cal,
gyro_cal->gyro_stillness_detect.last_sample_time);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Update stillness flag. Force the start of a new stillness period.
gyro_cal->prev_still = false;
} else {
@@ -484,12 +450,6 @@
gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
gyroStillMeanTracker(gyro_cal, DO_RESET);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Update stillness flag.
gyro_cal->prev_still = false;
}
@@ -501,11 +461,11 @@
// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
// Check to see if new calibration values is within acceptable range.
- if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
+ if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS &&
- gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
+ gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS &&
- gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
+ gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG(
@@ -607,12 +567,6 @@
gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
gyroStillMeanTracker(gyro_cal, DO_RESET);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Resets the stillness window end-time.
gyro_cal->stillness_win_endtime_nanos = 0;
@@ -631,7 +585,6 @@
}
// Assert watchdog timeout flags.
- gyro_cal->gyro_watchdog_timeout |= watchdog_timeout;
gyro_cal->gyro_watchdog_start_nanos = 0;
}
}
@@ -833,53 +786,6 @@
}
#ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
- float* debug_mean_sampling_rate_hz,
- uint64_t timestamp_nanos, bool reset_stats) {
- // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
- // out the estimate of the sampling rate.
- if (debug_mean_sampling_rate_hz) {
- if (sample_rate_estimator->num_samples > 1 &&
- sample_rate_estimator->time_delta_accumulator > 0) {
- // Computes the final mean calculation.
- *debug_mean_sampling_rate_hz =
- sample_rate_estimator->num_samples /
- (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
- NANOS_TO_SEC);
- } else {
- // Not enough samples to compute a valid sample rate estimate. Indicate
- // this with a -1 value.
- *debug_mean_sampling_rate_hz = -1.0f;
- }
- reset_stats = true;
- }
-
- // Resets the sampling rate mean estimator data.
- if (reset_stats) {
- sample_rate_estimator->last_timestamp_nanos = 0;
- sample_rate_estimator->time_delta_accumulator = 0;
- sample_rate_estimator->num_samples = 0;
- return;
- }
-
- // Skip adding this data to the accumulator if:
- // 1. A bad timestamp was received (i.e., time not monotonic).
- // 2. 'last_timestamp_nanos' is zero.
- if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
- sample_rate_estimator->last_timestamp_nanos == 0) {
- sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
- return;
- }
-
- // Increments the number of samples.
- sample_rate_estimator->num_samples++;
-
- // Accumulate the time steps.
- sample_rate_estimator->time_delta_accumulator +=
- timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
- sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
-}
-
void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
// Only update this data if debug printing is not currently in progress
// (i.e., don't want to risk overwriting debug information that is actively
@@ -912,11 +818,6 @@
gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y;
gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
- // Records the mean gyroscope sampling rate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
- &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
- /*reset_stats=*/true);
-
// Records the min/max gyroscope window stillness mean values.
memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
sizeof(gyro_cal->gyro_winmean_min));
@@ -1029,7 +930,9 @@
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
gyro_cal->debug_gyro_cal.temperature_min_celsius,
3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.sample_rate_estimator
+ .mean_sampling_rate_estimate_hz,
+ 3));
break;
case GYRO_MINMAX_STILLNESS_MEAN:
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 5e7d5ee..837a4c0 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -43,6 +43,10 @@
#include "calibration/gyroscope/gyro_stillness_detect.h"
+#ifdef GYRO_CAL_DBG_ENABLED
+#include "calibration/sample_rate_estimator/sample_rate_estimator.h"
+#endif // GYRO_CAL_DBG_ENABLED
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -63,10 +67,10 @@
// Gyro Cal debug information/data tracking structure.
struct DebugGyroCal {
+ struct SampleRateEstimator sample_rate_estimator;
uint64_t start_still_time_nanos;
uint64_t end_still_time_nanos;
uint64_t stillness_duration_nanos;
- float mean_sampling_rate_hz;
float accel_stillness_conf;
float gyro_stillness_conf;
float mag_stillness_conf;
@@ -84,15 +88,29 @@
float temperature_mean_celsius;
bool using_mag_sensor;
};
-
-// Data structure for sample rate estimation.
-struct SampleRateData {
- uint64_t last_timestamp_nanos;
- uint64_t time_delta_accumulator;
- size_t num_samples;
-};
#endif // GYRO_CAL_DBG_ENABLED
+// GyroCal algorithm parameters (see GyroCal and GyroStillDet for details).
+struct GyroCalParameters {
+ uint64_t min_still_duration_nanos;
+ uint64_t max_still_duration_nanos;
+ uint64_t calibration_time_nanos;
+ uint64_t window_time_duration_nanos;
+ float bias_x; // units: radians per second
+ float bias_y;
+ float bias_z;
+ float stillness_threshold; // units: (radians per second)^2
+ float stillness_mean_delta_limit; // units: radians per second
+ float gyro_var_threshold; // units: (radians per second)^2
+ float gyro_confidence_delta; // units: (radians per second)^2
+ float accel_var_threshold; // units: (meters per second)^2
+ float accel_confidence_delta; // units: (meters per second)^2
+ float mag_var_threshold; // units: micro-tesla^2
+ float mag_confidence_delta; // units: micro-tesla^2
+ float temperature_delta_limit_celsius;
+ bool gyro_calibration_enable;
+};
+
// Data structure for tracking min/max window mean during device stillness.
struct MinMaxWindowMeanData {
float gyro_winmean_min[3];
@@ -149,7 +167,6 @@
// Watchdog timer to reset to a known good state when data capture stalls.
uint64_t gyro_watchdog_start_nanos;
uint64_t gyro_watchdog_timeout_duration_nanos;
- bool gyro_watchdog_timeout;
// Flag is "true" when the magnetometer is used.
bool using_mag_sensor;
@@ -177,7 +194,7 @@
float temperature_mean_celsius;
float temperature_delta_limit_celsius;
-//----------------------------------------------------------------
+ //----------------------------------------------------------------
#ifdef GYRO_CAL_DBG_ENABLED
// Debug info.
@@ -185,9 +202,6 @@
enum GyroCalDebugState debug_state; // Debug printout state machine.
enum GyroCalDebugState next_state; // Debug state machine next state.
uint64_t wait_timer_nanos; // Debug message throttle timer.
-
- struct SampleRateData sample_rate_estimator; // Debug sample rate estimator.
-
size_t debug_calibration_count; // Total number of cals performed.
size_t debug_watchdog_count; // Total number of watchdog timeouts.
bool debug_print_trigger; // Flag used to trigger data printout.
@@ -197,27 +211,21 @@
/////// FUNCTION PROTOTYPES //////////////////////////////////////////
// Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
- uint64_t max_still_duration_nanos, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos,
- uint64_t window_time_duration_nanos, float gyro_var_threshold,
- float gyro_confidence_delta, float accel_var_threshold,
- float accel_confidence_delta, float mag_var_threshold,
- float mag_confidence_delta, float stillness_threshold,
- float stillness_mean_delta_limit,
- float temperature_delta_limit_celsius,
- bool gyro_calibration_enable);
+void gyroCalInit(struct GyroCal* gyro_cal,
+ const struct GyroCalParameters* parameters);
// Void all pointers in the gyro calibration data structure.
void gyroCalDestroy(struct GyroCal* gyro_cal);
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
- float* bias_z, float* temperature_celsius);
+ float* bias_z, float* temperature_celsius,
+ uint64_t* calibration_time_nanos);
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos);
+ float bias_z, float temperature_celsius,
+ uint64_t calibration_time_nanos);
// Remove gyro bias from the calibration [rad/sec].
void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index 80f2fa2..deb26b3 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -25,8 +25,8 @@
/////// FUNCTION DEFINITIONS /////////////////////////////////////////
// Initialize the GyroStillDet structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
- float var_threshold, float confidence_delta) {
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+ float confidence_delta) {
// Clear all data structure variables to 0.
memset(gyro_still_det, 0, sizeof(struct GyroStillDet));
@@ -192,12 +192,12 @@
// Each axis score is limited [0,1].
tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
gyro_still_det->stillness_confidence =
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
+ tmp_denom) *
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
+ tmp_denom) *
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
+ tmp_denom);
}
}
@@ -207,8 +207,7 @@
// Resets the stillness detector and initiates a new detection window.
// 'reset_stats' determines whether the stillness statistics are reset.
-void gyroStillDetReset(struct GyroStillDet* gyro_still_det,
- bool reset_stats) {
+void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) {
float tmp_denom = 1.f;
// Reset the stillness data ready flag.
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
index 9a1d876..51c4bee 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
@@ -60,7 +60,7 @@
// is used to keep track of the window start time.
bool start_new_window;
- // Starting time stamp for the the current window.
+ // Starting time stamp for the current window.
uint64_t window_start_time;
// Accumulator variables for tracking the sample mean during
@@ -93,8 +93,8 @@
/////// FUNCTION PROTOTYPES //////////////////////////////////////////
// Initialize the gyro_still_det_t structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
- float var_threshold, float confidence_delta);
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+ float confidence_delta);
// Update the stillness detector with a new sample.
void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
index 7f8e563..40749e1 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c
@@ -21,6 +21,7 @@
#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
@@ -42,13 +43,14 @@
#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
+#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
static int moc_eigen_test(struct KasaFit *kasa) {
@@ -82,71 +84,8 @@
return eigen_pass;
}
-// Kasa sphere fitting with normal equation
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) {
- // A * out = b
- // (4 x 4) (4 x 1) (4 x 1)
- struct Mat44 A;
- A.elem[0][0] = kasa->acc_xx;
- A.elem[0][1] = kasa->acc_xy;
- A.elem[0][2] = kasa->acc_xz;
- A.elem[0][3] = kasa->acc_x;
- A.elem[1][0] = kasa->acc_xy;
- A.elem[1][1] = kasa->acc_yy;
- A.elem[1][2] = kasa->acc_yz;
- A.elem[1][3] = kasa->acc_y;
- A.elem[2][0] = kasa->acc_xz;
- A.elem[2][1] = kasa->acc_yz;
- A.elem[2][2] = kasa->acc_zz;
- A.elem[2][3] = kasa->acc_z;
- A.elem[3][0] = kasa->acc_x;
- A.elem[3][1] = kasa->acc_y;
- A.elem[3][2] = kasa->acc_z;
- A.elem[3][3] = 1.0f;
-
- struct Vec4 b;
- initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
-
- struct Size4 pivot;
- mat44DecomposeLup(&A, &pivot);
-
- struct Vec4 out;
- mat44Solve(&A, &out, &b, &pivot);
-
- // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
- //
- // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
- // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
-
- struct Vec3 v;
- initVec3(&v, out.x, out.y, out.z);
- vec3ScalarMul(&v, -0.5f);
-
- float r_square = vec3Dot(&v, &v) - out.w;
- float r = (r_square > 0) ? sqrtf(r_square) : 0;
-
- initVec3(bias, v.x, v.y, v.z);
- *radius = r;
-
- int success = 0;
- if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) {
- success = 1;
- }
-
- return success;
-}
-
-void magKasaReset(struct KasaFit *kasa) {
- kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
- kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
- kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
- kasa->acc_zz = kasa->acc_zw = 0.0f;
-
- kasa->nsamples = 0;
-}
-
void magCalReset(struct MagCal *moc) {
- magKasaReset(&moc->kasa);
+ kasaReset(&moc->kasa);
#ifdef DIVERSITY_CHECK_ENABLED
diversityCheckerReset(&moc->diversity_checker);
#endif
@@ -154,12 +93,12 @@
moc->kasa_batching = false;
}
-static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
- int complete = 0;
+static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
+ bool complete = false;
if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) &&
(moc->kasa.nsamples > MIN_BATCH_SIZE)) {
- complete = 1;
+ complete = true;
} else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
// not enough samples collected in MAX_BATCH_WINDOW or too many
@@ -170,42 +109,32 @@
return complete;
}
-void initKasa(struct KasaFit *kasa) {
- magKasaReset(kasa);
-}
-
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros
+void initMagCal(struct MagCal *moc,
+ const struct MagCalParameters *mag_cal_parameters
#ifdef DIVERSITY_CHECK_ENABLED
- ,size_t min_num_diverse_vectors
- ,size_t max_num_max_distance
- ,float var_threshold
- ,float max_min_threshold
- ,float local_field
- ,float threshold_tuning_param
- ,float max_distance_tuning_param
+ ,
+ const struct DiversityCheckerParameters *diverse_parameters
#endif
- ) {
+) {
magCalReset(moc);
moc->update_time = 0;
- moc->min_batch_window_in_micros = min_batch_window_in_micros;
+ moc->min_batch_window_in_micros =
+ mag_cal_parameters->min_batch_window_in_micros;
moc->radius = 0.0f;
- moc->x_bias = x_bias;
- moc->y_bias = y_bias;
- moc->z_bias = z_bias;
+ moc->x_bias = mag_cal_parameters->x_bias;
+ moc->y_bias = mag_cal_parameters->y_bias;
+ moc->z_bias = mag_cal_parameters->z_bias;
- moc->c00 = c00;
- moc->c01 = c01;
- moc->c02 = c02;
- moc->c10 = c10;
- moc->c11 = c11;
- moc->c12 = c12;
- moc->c20 = c20;
- moc->c21 = c21;
- moc->c22 = c22;
+ moc->c00 = mag_cal_parameters->c00;
+ moc->c01 = mag_cal_parameters->c01;
+ moc->c02 = mag_cal_parameters->c02;
+ moc->c10 = mag_cal_parameters->c10;
+ moc->c11 = mag_cal_parameters->c11;
+ moc->c12 = mag_cal_parameters->c12;
+ moc->c20 = mag_cal_parameters->c20;
+ moc->c21 = mag_cal_parameters->c21;
+ moc->c22 = mag_cal_parameters->c22;
#ifdef MAG_CAL_DEBUG_ENABLE
moc->mag_dbg.mag_trigger_count = 0;
@@ -214,14 +143,7 @@
#ifdef DIVERSITY_CHECK_ENABLED
// Diversity Checker
- diversityCheckerInit(&moc->diversity_checker,
- min_num_diverse_vectors,
- max_num_max_distance,
- var_threshold,
- max_min_threshold,
- local_field,
- threshold_tuning_param,
- max_distance_tuning_param);
+ diversityCheckerInit(&moc->diversity_checker, diverse_parameters);
#endif
}
@@ -237,117 +159,70 @@
#endif
// 1. run accumulators
- float w = x * x + y * y + z * z;
+ kasaAccumulate(&moc->kasa, x, y, z);
- moc->kasa.acc_x += x;
- moc->kasa.acc_y += y;
- moc->kasa.acc_z += z;
- moc->kasa.acc_w += w;
-
- moc->kasa.acc_xx += x * x;
- moc->kasa.acc_xy += x * y;
- moc->kasa.acc_xz += x * z;
- moc->kasa.acc_xw += x * w;
-
- moc->kasa.acc_yy += y * y;
- moc->kasa.acc_yz += y * z;
- moc->kasa.acc_yw += y * w;
-
- moc->kasa.acc_zz += z * z;
- moc->kasa.acc_zw += z * w;
-
- if (++moc->kasa.nsamples == 1) {
+ if (moc->kasa.nsamples == 1) {
moc->start_time = sample_time_us;
moc->kasa_batching = true;
}
// 2. batch has enough samples?
if (moc_batch_complete(moc, sample_time_us)) {
- float inv = 1.0f / moc->kasa.nsamples;
-
- moc->kasa.acc_x *= inv;
- moc->kasa.acc_y *= inv;
- moc->kasa.acc_z *= inv;
- moc->kasa.acc_w *= inv;
-
- moc->kasa.acc_xx *= inv;
- moc->kasa.acc_xy *= inv;
- moc->kasa.acc_xz *= inv;
- moc->kasa.acc_xw *= inv;
-
- moc->kasa.acc_yy *= inv;
- moc->kasa.acc_yz *= inv;
- moc->kasa.acc_yw *= inv;
-
- moc->kasa.acc_zz *= inv;
- moc->kasa.acc_zw *= inv;
+ kasaNormalize(&moc->kasa);
// 3. eigen test
if (moc_eigen_test(&moc->kasa)) {
struct Vec3 bias;
float radius;
// 4. Kasa sphere fitting
- if (magKasaFit(&moc->kasa, &bias, &radius)) {
-
+ 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_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(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
#ifdef DIVERSITY_CHECK_ENABLED
// Update the local field.
- diversityCheckerLocalFieldUpdate(&moc->diversity_checker,
- radius);
+ diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius);
// checking if data is diverse.
- if (diversityCheckerNormQuality(&moc->diversity_checker,
- bias.x,
- bias.y,
+ if (diversityCheckerNormQuality(&moc->diversity_checker, bias.x, bias.y,
bias.z) &&
- moc->diversity_checker.num_max_dist_violations
- <= MAX_DISTANCE_VIOLATIONS) {
-
+ moc->diversity_checker.num_max_dist_violations <=
+ MAX_DISTANCE_VIOLATIONS) {
// 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",
- 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),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.mean_log, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.max_log, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.min_log, 3),
- 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,
- 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);
+ 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",
+ 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),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.mean_log,
+ 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.max_log, 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.min_log, 3),
+ 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,
+ 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
@@ -416,102 +291,100 @@
#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSE_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) {
+void magLogPrint(struct DiversityChecker *diverse_data, float temp) {
// Sample counter.
static size_t sample_counter = 0;
- const float* data_log_ptr =
- &diverse_data->diversity_dbg.diverse_data_log[0];
+ const float *data_log_ptr = &diverse_data->diversity_dbg.diverse_data_log[0];
if (diverse_data->diversity_dbg.new_trigger == 1) {
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,
- 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),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3], 3),
- CAL_ENCODE_FLOAT(temp, 3));
+ 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,
+ 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),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3], 3), CAL_ENCODE_FLOAT(temp, 3));
}
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,
- 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),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3+1], 3));
+ 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,
+ 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),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 1], 3));
}
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,
- 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),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3+2], 3));
+ 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,
+ 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),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 2], 3));
sample_counter = 0;
diverse_data->diversity_dbg.new_trigger = 0;
}
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
index 8c9da6f..381a925 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h
@@ -13,6 +13,7 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
+
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
@@ -26,8 +27,9 @@
#include <stdint.h>
#include <sys/types.h>
#ifdef DIVERSITY_CHECK_ENABLED
-#include "calibration/common/diversity_checker.h"
+#include "calibration/diversity_checker/diversity_checker.h"
#endif
+#include "common/math/kasa.h"
#include "common/math/mat.h"
#include "common/math/vec.h"
@@ -35,17 +37,13 @@
extern "C" {
#endif
-struct KasaFit {
- float acc_x, acc_y, acc_z, acc_w;
- float acc_xx, acc_xy, acc_xz, acc_xw;
- float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
- size_t nsamples;
-};
-
enum MagUpdate {
NO_UPDATE = 0x00,
UPDATE_BIAS = 0x01,
UPDATE_SPHERE_FIT = 0x02,
+ UPDATE_BIAS_MAGGYRO_MEDIUM = 0x04,
+ UPDATE_BIAS_MAGGYRO_HIGH = 0x08,
+ MAGGYRO_TIMEOUT = 0x10,
};
#ifdef MAG_CAL_DEBUG_ENABLE
@@ -55,17 +53,34 @@
};
#endif
+// MagCal algorithm parameters (see MagCal for details).
+struct MagCalParameters {
+ uint32_t min_batch_window_in_micros;
+ float x_bias; // [micro-Tesla]
+ float y_bias; // [micro-Tesla]
+ float z_bias; // [micro-Tesla]
+ float c00;
+ float c01;
+ float c02;
+ float c10;
+ float c11;
+ float c12;
+ float c20;
+ float c21;
+ float c22;
+};
+
struct MagCal {
#ifdef DIVERSITY_CHECK_ENABLED
struct DiversityChecker diversity_checker;
#endif
struct KasaFit kasa;
- uint64_t start_time;
- uint64_t update_time;
+ uint64_t start_time; // [micro-seconds]
+ uint64_t update_time; // [micro-seconds]
uint32_t min_batch_window_in_micros;
float x_bias, y_bias, z_bias;
- float radius;
+ float radius; // [micro-Tesla]
bool kasa_batching;
float c00, c01, c02, c10, c11, c12, c20, c21, c22;
@@ -74,21 +89,13 @@
#endif
};
-void initKasa(struct KasaFit *kasa);
-
#ifdef DIVERSITY_CHECK_ENABLED
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors, size_t max_num_max_distance,
- float var_threshold, float max_min_threshold, float local_field,
- float threshold_tuning_param, float max_distance_tuning_param);
+void initMagCal(struct MagCal *moc,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters);
#else
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros);
+void initMagCal(struct MagCal *moc,
+ const struct MagCalParameters *mag_cal_parameters);
#endif
void magCalDestroy(struct MagCal *moc);
@@ -110,12 +117,8 @@
void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
float *xo, float *yo, float *zo);
-void magKasaReset(struct KasaFit *kasa);
-
void magCalReset(struct MagCal *moc);
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius);
-
#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSITY_CHECK_ENABLED
void magLogPrint(struct DiversityChecker *moc, float temp);
#endif
diff --git a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c
new file mode 100644
index 0000000..29a7ba0
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.c
@@ -0,0 +1,271 @@
+#include "calibration/magnetometer/mag_gyro_cal.h"
+
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+#include "common/math/macros.h"
+
+#define SINGULAR_TOL (1e-4f)
+
+static bool allDiagonalsLessThan(struct Mat33 *matrix, float threshold) {
+ return matrix->elem[0][0] < threshold && matrix->elem[1][1] < threshold &&
+ matrix->elem[2][2] < threshold;
+}
+
+static void calculatingAndSettingBias(struct MagGyroCal *mgc,
+ struct Mat33 *first_sum_inverse,
+ struct Vec3 *bias) {
+ // Calculate bias, since we have high trust into this estimates.
+ mat33Apply(bias, first_sum_inverse, &mgc->mgac.second_sum_average);
+
+ // Setting bias update.
+ mgc->x_bias = bias->x;
+ mgc->y_bias = bias->y;
+ mgc->z_bias = bias->z;
+}
+
+void magGyroCalBatchReset(struct MagGyroBatchCal *mgbc) {
+ mgbc->first_sample = true;
+ mgbc->start_time_ns = 0;
+ mgbc->timestamp_buffer_ns = 0;
+ initZeroMatrix(&mgbc->first_sum_update);
+ initVec3(&mgbc->last_mag_sample, 0.0f, 0.0f, 0.0f);
+ initVec3(&mgbc->second_sum_update, 0.0f, 0.0f, 0.0f);
+}
+
+static void magGyroCalAverageReset(struct MagGyroAverageCal *mgac) {
+ initZeroMatrix(&mgac->first_sum_average);
+ initVec3(&mgac->second_sum_average, 0.0f, 0.0f, 0.0f);
+ initVec3(&mgac->confidence_metric, 0.0f, 0.0f, 0.0f);
+ mgac->medium_accuracy_bias_update = false;
+ initVec3(&mgac->bias_running, 0.0f, 0.0f, 0.0f);
+}
+
+void magGyroCalReset(struct MagGyroCal *mgc) {
+ // Reset Batch Cal.
+ magGyroCalBatchReset(&mgc->mgbc);
+
+ // Reset Avarge Cal.
+ magGyroCalAverageReset(&mgc->mgac);
+
+ initVec3(&mgc->mgac.confidence_metric,
+ mgc->algo_parameters.confidence_threshold_start,
+ mgc->algo_parameters.confidence_threshold_start,
+ mgc->algo_parameters.confidence_threshold_start);
+}
+
+void magGyroCalGetBias(struct MagGyroCal *mgc, float *x, float *y, float *z) {
+ *x = mgc->x_bias;
+ *y = mgc->y_bias;
+ *z = mgc->z_bias;
+}
+
+void magGyroCalInit(struct MagGyroCal *mgc,
+ const struct MagGyroCalParameters *params) {
+ // Tuning parameters.
+ memcpy(&mgc->algo_parameters, params, sizeof(mgc->algo_parameters));
+
+ // Bias Updates set to zero.
+ mgc->x_bias = 0.0f;
+ mgc->y_bias = 0.0f;
+ mgc->z_bias = 0.0f;
+
+ // Reseting Algo.
+ magGyroCalReset(mgc);
+}
+
+void magGyroCalBatchUpdate(struct MagGyroBatchCal *mgbc,
+ uint64_t sample_time_ns, float mag_x_ut,
+ float mag_y_ut, float mag_z_ut, float gyro_x_rps,
+ float gyro_y_rps, float gyro_z_rps) {
+ // local variables, only needed for math on single samples.
+ struct Vec3 mag_data;
+ struct Vec3 gyro_data;
+ struct Vec3 mag_data_dot;
+ struct Vec3 cross_mag_gyro;
+ struct Vec3 second_sum;
+ struct Vec3 yi;
+ struct Mat33 first_sum;
+ struct Mat33 w;
+ struct Mat33 w_trans;
+ float odr_hz_int;
+ // Creating gyro and mag vector.
+ initVec3(&mag_data, mag_x_ut, mag_y_ut, mag_z_ut);
+ initVec3(&gyro_data, gyro_x_rps, gyro_y_rps, gyro_z_rps);
+
+ // Saving first sample in a batch and skipping the rest, this is done in order
+ // to do the discrete time derivative. x_dot = x(n) - x(n-1).
+ if (mgbc->first_sample) {
+ initVec3(&mgbc->last_mag_sample, mag_x_ut, mag_y_ut, mag_z_ut);
+ mgbc->first_sample = false;
+ mgbc->start_time_ns = sample_time_ns;
+ mgbc->timestamp_buffer_ns = sample_time_ns;
+ return;
+ }
+ // checking if delta time can become zero or negative.
+ if (sample_time_ns <= mgbc->timestamp_buffer_ns) {
+ // Timestamp is not monotonic, resetting batch.
+ magGyroCalBatchReset(mgbc);
+ return;
+ }
+
+ // Calculating time delta between this sample and the sample before.
+ float delta_time_ns = (float)(sample_time_ns - mgbc->timestamp_buffer_ns);
+
+ // Calculating 1/deltaT = ODR [Hz], for time derivative.
+ odr_hz_int = 1 / (delta_time_ns * NANOS_TO_SEC);
+ mgbc->timestamp_buffer_ns = sample_time_ns;
+
+ // creating mag_dot.
+ // mag_data_dot = (mag_data - last_mag_sample) * odr.
+ vec3SubVecs(&mag_data_dot, &mag_data, &mgbc->last_mag_sample);
+ vec3ScalarMul(&mag_data_dot, odr_hz_int);
+
+ // Cross product (gyro x mag).
+ vec3Cross(&cross_mag_gyro, &gyro_data, &mag_data);
+
+ // Saving data into memory, for next derivative.
+ initVec3(&mgbc->last_mag_sample, mag_x_ut, mag_y_ut, mag_z_ut);
+
+ // calculating yi = mag_dot + (gyro x mag).
+ vec3AddVecs(&yi, &mag_data_dot, &cross_mag_gyro);
+
+ // creating skew-symmetric matrix.
+ // / 0, -w3, w2 \.
+ // w = | w3, 0, -w1 |.
+ // \ -w2, w1, 0 /.
+ w.elem[0][0] = 0;
+ w.elem[0][1] = -gyro_z_rps;
+ w.elem[0][2] = gyro_y_rps;
+ w.elem[1][0] = gyro_z_rps;
+ w.elem[1][1] = 0;
+ w.elem[1][2] = -gyro_x_rps;
+ w.elem[2][0] = -gyro_y_rps;
+ w.elem[2][1] = gyro_x_rps;
+ w.elem[2][2] = 0;
+
+ // calculating second_sum:
+ // second_sum = w^T * yi.
+ mat33Transpose(&w_trans, &w);
+ mat33Apply(&second_sum, &w_trans, &yi);
+
+ // calculating first sum:
+ // first_sum = w^T * w.
+ mat33MultiplyTransposed(&first_sum, &w, &w);
+
+ // Updating sums.
+ mat33Add(&mgbc->first_sum_update, &first_sum);
+ vec3Add(&mgbc->second_sum_update, &second_sum);
+}
+
+enum MagUpdate magGyroCalBiasUpdate(struct MagGyroCal *mgc,
+ struct Mat33 *first_sum_inverse) {
+ struct Vec3 bias;
+ // Testing for medium accuracy.
+ if (allDiagonalsLessThan(first_sum_inverse,
+ mgc->algo_parameters.confidence_threshold_medium)) {
+ // Testing for high accuracy, will leave the function here if true.
+ if (allDiagonalsLessThan(first_sum_inverse,
+ mgc->algo_parameters.confidence_threshold_high)) {
+ // Calculate bias, since we have high trust into this estimates.
+ calculatingAndSettingBias(mgc, first_sum_inverse, &bias);
+
+ // Resetting everything since we have found the bias.
+ magGyroCalReset(mgc);
+
+ // Return high accuracy indicator.
+ return UPDATE_BIAS_MAGGYRO_HIGH;
+ }
+ // Only will reach this point if accuracy is between high and medium, now
+ // we test for if we already have a medium update.
+ if (mgc->mgac.medium_accuracy_bias_update == false) {
+ // Calculate bias, since we have high trust into the estimates.
+ calculatingAndSettingBias(mgc, first_sum_inverse, &bias);
+
+ // Resetting Batch, since we are not done yet.
+ magGyroCalBatchReset(&mgc->mgbc);
+
+ // Setting medium_accuracy_bias_update to "true".
+ mgc->mgac.medium_accuracy_bias_update = true;
+
+ // Return medium accuracy indicator.
+ return UPDATE_BIAS_MAGGYRO_MEDIUM;
+
+ // already have an medium update, still calculate the actual offset, can
+ // be used for log files and manual bias extractions.
+ } else {
+ // Calculate bias, since we have high trust into the estimates.
+ mat33Apply(&bias, first_sum_inverse, &mgc->mgac.second_sum_average);
+
+ // Setting bias update.
+ mgc->mgac.bias_running = bias;
+
+ // Resetting Batch, since we are not done yet.
+ magGyroCalBatchReset(&mgc->mgbc);
+
+ // Return no update indicator.
+ return NO_UPDATE;
+ }
+ }
+ // Have not gotten to medium accuracy, reset the batch.
+ magGyroCalBatchReset(&mgc->mgbc);
+
+ // Return no update indicator.
+ return NO_UPDATE;
+}
+
+enum MagUpdate magGyroCalRun(struct MagGyroCal *mgc, uint64_t sample_time_ns,
+ float mag_x_ut, float mag_y_ut, float mag_z_ut,
+ float gyro_x_rps, float gyro_y_rps,
+ float gyro_z_rps) {
+ struct Mat33 first_sum_inverse;
+ // Running samples into the batch function.
+ magGyroCalBatchUpdate(&mgc->mgbc, sample_time_ns, mag_x_ut, mag_y_ut,
+ mag_z_ut, gyro_x_rps, gyro_y_rps, gyro_z_rps);
+
+ // Timeout, reset and start over, return MAGGYRO_TIMEOUT.
+ if ((sample_time_ns - mgc->mgbc.start_time_ns) >=
+ mgc->algo_parameters.max_run_time_in_ns) {
+ magGyroCalReset(mgc);
+ return MAGGYRO_TIMEOUT;
+ }
+
+ // Early return if batch not complete.
+ if ((sample_time_ns - mgc->mgbc.start_time_ns) <=
+ mgc->algo_parameters.batch_window_in_ns) {
+ return NO_UPDATE;
+ }
+
+ // Updating the average matrix.
+ mat33Add(&mgc->mgac.first_sum_average, &mgc->mgbc.first_sum_update);
+
+ // Testing for singular matrix.
+ if (fabsf(mat33Determinant(&mgc->mgac.first_sum_average)) > SINGULAR_TOL) {
+ // Inverse of the average system matrix.
+ mat33Invert(&first_sum_inverse, &mgc->mgac.first_sum_average);
+
+ // Testing if the confidence metric has been improved.
+ if (first_sum_inverse.elem[0][0] < mgc->mgac.confidence_metric.x &&
+ first_sum_inverse.elem[1][1] < mgc->mgac.confidence_metric.y &&
+ first_sum_inverse.elem[2][2] < mgc->mgac.confidence_metric.z) {
+ // Updating the second_sum vector.
+ vec3Add(&mgc->mgac.second_sum_average, &mgc->mgbc.second_sum_update);
+
+ // Updating the confidence metric.
+ initVec3(&mgc->mgac.confidence_metric, first_sum_inverse.elem[0][0],
+ first_sum_inverse.elem[1][1], first_sum_inverse.elem[2][2]);
+
+ // Testing for Bias update and leaving the function.
+ return magGyroCalBiasUpdate(mgc, &first_sum_inverse);
+ }
+ // No improvement, removing the last batch from the average.
+ mat33Sub(&mgc->mgac.first_sum_average, &mgc->mgbc.first_sum_update);
+
+ // Resetting Batch, since we are not done yet.
+ magGyroCalBatchReset(&mgc->mgbc);
+
+ // Returning no bias update.
+ return NO_UPDATE;
+ }
+ return NO_UPDATE;
+}
diff --git a/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h
new file mode 100644
index 0000000..edf92e8
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_gyro_cal.h
@@ -0,0 +1,188 @@
+/*
+ * This module contains the algorithms for producing a magnetometer offset
+ * calibration. The algorithm is based on the paper “Adaptive Estimation of
+ * Measurement Bias in Three-Dimensional Field Sensors with Angular-Rate
+ * Sensors: Theory and Comparative Experimental Evaluation” by G. Troni and L.
+ * Whitcomb and uses gyroscope data as well as magnetometer data. We use a batch
+ * method, resulting in a Linear Least-Squares Fit (Section III B of the paper).
+ * We define the following mathematical setup:
+ *
+ * wi (wx,wy,wx) -> are the gyroscope readings [RAD/s]
+ * xi -> are the magnetometer readings [uT]
+ * xi_dot -> is the derivative of xi
+ *
+ * Form here we can define:
+ *
+ * yi = xi_dot + (wi x xi) (x denotes the cross product)
+ *
+ * and
+ *
+ * / 0 -wz wy \
+ * Wi =| wz 0 -wx |
+ * \ -wy wx 0 /
+ *
+ * which we can use to define, what we call first sum and second sum
+ *
+ * firstSum = sum( Wi^2)
+ *
+ * and
+ *
+ * secondSum = sum(Wi yi)
+ *
+ * The bias is calculated the following way
+ *
+ * b = inverse(firstSum) * secondSum
+ *
+ * Notice that we use the diagonal of the matrix “inverse(firstSum)” as a
+ * confidence metric, to make a accuracy assessment.
+ *
+ * The algorithm expects time synchronized magnetometer and gyro data, any delay
+ * shift between those two greater than 5-10ms will result in poor accuracy.
+ * Furthermore, the minimum sample rate of the sensors should be higher than 50
+ * Hz.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
+
+#ifdef SPHERE_FIT_ENABLED
+#ifndef DIVERSITY_CHECK_ENABLED
+#define DIVERSITY_CHECK_ENABLED
+#endif
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "calibration/magnetometer/mag_cal.h"
+#include "common/math/mat.h"
+#include "common/math/vec.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Algo Batch Data Struct.
+struct MagGyroBatchCal {
+ // Batch Start time.
+ uint64_t start_time_ns;
+
+ // Time Stamp Buffer. Variable used to calculate 1/deltaT = odr_in_hz, where
+ // 1/deltaT is used to calculate the derivative of the mag signal.
+ uint64_t timestamp_buffer_ns;
+
+ // First sample in batch indicator.
+ bool first_sample;
+
+ // Matrix and vector definition.
+ struct Mat33 first_sum_update;
+ struct Vec3 last_mag_sample;
+ struct Vec3 second_sum_update;
+};
+
+// Algorithm tuning Parameter struct.
+struct MagGyroCalParameters {
+ // Batch window size.
+ uint64_t batch_window_in_ns;
+
+ // Algo resets after max_run_time.
+ uint64_t max_run_time_in_ns;
+
+ // Confidence Threshold.
+ // The confidence metric is explained in go/magcalp design document.
+ // Furthermore more details are given as well on how to tune it. Confidence
+ // Threshold high accuracy.
+ float confidence_threshold_high;
+
+ // Confidence Threshold medium accuracy.
+ float confidence_threshold_medium;
+
+ // Confidence Threshold start. Don't want to start with a bad matrix.
+ float confidence_threshold_start;
+};
+
+// Data Struct for the Batch average function.
+struct MagGyroAverageCal {
+ // Matrix and vector definition.
+ struct Mat33 first_sum_average;
+ struct Vec3 second_sum_average;
+
+ // Medium accuracy bias update.
+ bool medium_accuracy_bias_update;
+
+ // Confidence Metric.
+ struct Vec3 confidence_metric;
+
+ // Running biases update.
+ struct Vec3 bias_running;
+};
+
+// Main algo data Struct.
+struct MagGyroCal {
+ // Algorithm tuning parameters.
+ struct MagGyroCalParameters algo_parameters;
+
+ // Single batch struct. We use batches, which in turn get again averaged over
+ // time. The single batch is the amount of data that we do the Linear
+ // Least-Squares fit on and decide if we will add this to the averaging based
+ // on the confidence metric. Notice that we do on every batch a matrix
+ // inversion, hence in order to save power it is beneficial to make the batch
+ // size reasonably large. This is controlled by the “batch_window_in_ns”.
+ struct MagGyroBatchCal mgbc;
+
+ // Algo average struct.
+ struct MagGyroAverageCal mgac;
+
+ // New bias update.
+ float x_bias;
+ float y_bias;
+ float z_bias;
+};
+
+// Algorithm initialization.
+void magGyroCalInit(struct MagGyroCal *mgc,
+ const struct MagGyroCalParameters *params);
+
+// Resets the batch struct.
+void magGyroCalReset(struct MagGyroCal *mgc);
+
+// Gets the current MagGyro bias estimate.
+void magGyroCalGetBias(struct MagGyroCal *mgc, float *x, float *y, float *z);
+
+// Main algorithm runner function.
+// sample_time_ns in [ns].
+// mag_x_ut, mag_y_ut, mag_z_ut are in [uT].
+// gyro_x_rps, gyro_y_rps, gyro_z_rps are in [RAD/s].
+// odr_hz is in [Hz].
+enum MagUpdate magGyroCalRun(struct MagGyroCal *mgc, uint64_t sample_time_ns,
+ float mag_x_ut, float mag_y_ut, float mag_z_ut,
+ float gyro_x_rps, float gyro_y_rps,
+ float gyro_z_rps);
+
+//////// Function exposed for testing ///////////////////////////////////
+// Updating sensor data into the batch.
+// sample_time_ns in [ns].
+// mag_x_ut, mag_y_ut, mag_z_ut are in [uT].
+// gyro_x_rps, gyro_y_rps, gyro_z_rps are in [RAD/s].
+// odr_hz is in [Hz].
+void magGyroCalBatchUpdate(struct MagGyroBatchCal *mgbc,
+ uint64_t sample_time_ns, float mag_x_ut,
+ float mag_y_ut, float mag_z_ut, float gyro_x_rps,
+ float gyro_y_rps, float gyro_z_rps);
+
+// Checks metrics and classifies bias update into either:
+// NO_UPDATE and no bias is updated
+// UPDATE_BIAS_MEDIUM bias is update with medium accuracy.
+// UPDATE_BIAS_HIGH bias is update with high accuracy.
+enum MagUpdate magGyroCalBiasUpdate(struct MagGyroCal *mgc,
+ struct Mat33 *first_sum_inverse);
+
+// Resetting the batch struct.
+void magGyroCalBatchReset(struct MagGyroBatchCal *mgbc);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_GYRO_CAL_H_
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
index eb23b87..3e23dcf 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
@@ -33,22 +33,16 @@
memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data));
}
-void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias,
- float z_bias, float c00, float c01, float c02, float c10,
- float c11, float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance, float var_threshold,
- float max_min_threshold, float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param) {
- initMagCal(&mocs->moc, x_bias, y_bias, z_bias, c00, c01, c02, c10, c11, c12,
- c20, c21, c22, min_batch_window_in_micros, min_num_diverse_vectors,
- max_num_max_distance, var_threshold, max_min_threshold,
- local_field, threshold_tuning_param, max_distance_tuning_param);
+void initMagCalSphere(
+ struct MagCalSphere *mocs,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters,
+ float default_odr_in_hz) {
+ initMagCal(&mocs->moc, mag_cal_parameters, diverse_parameters);
mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA;
mocs->batch_time_in_sec =
- (float)(min_batch_window_in_micros) * FROM_MICRO_SEC_TO_SEC;
+ (float)(mag_cal_parameters->min_batch_window_in_micros) *
+ FROM_MICRO_SEC_TO_SEC;
// Initialize to take every sample, default setting.
mocs->sample_drop = 0;
magCalSphereReset(mocs);
@@ -63,6 +57,9 @@
sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal,
&mocs->sphere_fit.lm_data);
calDataReset(&mocs->sphere_fit.sphere_param);
+
+ // Initializes the starting output data rate estimate.
+ magCalSphereOdrUpdate(mocs, default_odr_in_hz);
}
void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; }
@@ -91,7 +88,7 @@
if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) {
memcpy(&mocs->sphere_data[mocs->number_of_data_samples *
THREE_AXIS_DATA_DIM],
- vec, sizeof(float) * 3);
+ vec, sizeof(float) * THREE_AXIS_DATA_DIM);
// counting the numbers of samples in the data set.
mocs->number_of_data_samples++;
}
@@ -114,8 +111,7 @@
// Running the sphere fit and checking if successful.
if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) {
- // Updating Sphere parameters. Can use "calDataCorrectData" function to
- // correct data.
+ // Updating sphere parameters.
sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal,
&mocs->sphere_fit.sphere_param);
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
index 85df48f..7c2fc86 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
@@ -13,11 +13,12 @@
* See the License for the specific language governing permissions and
* 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_
-#include "calibration/common/sphere_fit_calibration.h"
#include "calibration/magnetometer/mag_cal.h"
+#include "calibration/sphere_fit/sphere_fit_calibration.h"
#define NUM_SPHERE_FIT_DATA 50
@@ -50,24 +51,17 @@
float sphere_data[THREE_AXIS_DATA_DIM * NUM_SPHERE_FIT_DATA];
};
-void initMagCalSphere(struct MagCalSphere *mocs,
- float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold,
- float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param);
+void initMagCalSphere(
+ struct MagCalSphere *mocs,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters,
+ float default_odr_in_hz);
void magCalSphereDestroy(struct MagCalSphere *mocs);
enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
- uint64_t sample_time_us,
- float x, float y, float z);
+ uint64_t sample_time_us, float x, float y,
+ float z);
void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz);
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
new file mode 100644
index 0000000..88557ad
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
@@ -0,0 +1,125 @@
+/*
+ * 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_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+
+#ifdef ACCEL_CAL_ENABLED
+#include "calibration/accelerometer/accel_cal.h"
+#endif // ACCEL_CAL_ENABLED
+
+#ifdef GYRO_CAL_ENABLED
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/over_temp/over_temp_cal.h"
+#endif // GYRO_CAL_ENABLED
+
+#ifdef MAG_CAL_ENABLED
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "calibration/magnetometer/mag_cal.h"
+#endif // MAG_CAL_ENABLED
+
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+//////// ACCELEROMETER CALIBRATION ////////
+#ifdef ACCEL_CAL_ENABLED
+constexpr AccelCalParameters kAccelCalParameters{
+ MSEC_TO_NANOS(800), // t0
+ 5, // n_s
+ 15, // fx
+ 15, // fxb
+ 15, // fy
+ 15, // fyb
+ 15, // fz
+ 15, // fzb
+ 15, // fle
+ 0.00025f // th
+};
+#endif // ACCEL_CAL_ENABLED
+
+//////// GYROSCOPE CALIBRATION ////////
+#ifdef GYRO_CAL_ENABLED
+constexpr GyroCalParameters kGyroCalParameters{
+ SEC_TO_NANOS(1.4), // min_still_duration_nanos
+ SEC_TO_NANOS(1.4), // max_still_duration_nanos [see, NOTE 1]
+ 0, // calibration_time_nanos
+ MSEC_TO_NANOS(500), // window_time_duration_nanos
+ 0, // bias_x
+ 0, // bias_y
+ 0, // bias_z
+ 0.95f, // stillness_threshold
+ MDEG_TO_RAD * 60.0f, // stillness_mean_delta_limit [rad/sec]
+ 3.0e-5f, // gyro_var_threshold [rad/sec]^2
+ 3.0e-6f, // gyro_confidence_delta [rad/sec]^2
+ 4.5e-3f, // accel_var_threshold [m/sec^2]^2
+ 9.0e-4f, // accel_confidence_delta [m/sec^2]^2
+ 5.0f, // mag_var_threshold [uTesla]^2
+ 1.0f, // mag_confidence_delta [uTesla]^2
+ 1.5f, // temperature_delta_limit_celsius
+ true // gyro_calibration_enable
+};
+// [NOTE 1]: 'max_still_duration_nanos' is set to 1.4 seconds to achieve a max
+// stillness period of 1.5 seconds and avoid buffer boundary conditions that
+// could push the max stillness to the next multiple of the analysis window
+// length (i.e., 2.0 seconds).
+
+constexpr OverTempCalParameters kGyroOtcParameters{
+ MSEC_TO_NANOS(100), // min_temp_update_period_nanos
+ DAYS_TO_NANOS(2), // age_limit_nanos
+ 0.75f, // delta_temp_per_bin
+ 40.0f * MDEG_TO_RAD, // jump_tolerance
+ 100.0f * MDEG_TO_RAD, // outlier_limit
+ 250.0f * MDEG_TO_RAD, // temp_sensitivity_limit
+ 8.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
+ 0.1f * MDEG_TO_RAD, // significant_offset_change
+ 5, // min_num_model_pts
+ true // over_temp_enable
+};
+#endif // GYRO_CAL_ENABLED
+
+//////// MAGNETOMETER CALIBRATION ////////
+#ifdef MAG_CAL_ENABLED
+constexpr MagCalParameters kMagCalParameters{
+ 3000000, // min_batch_window_in_micros
+ 0.0f, // x_bias
+ 0.0f, // y_bias
+ 0.0f, // z_bias
+ 1.0f, // c00
+ 0.0f, // c01
+ 0.0f, // c02
+ 0.0f, // c10
+ 1.0f, // c11
+ 0.0f, // c12
+ 0.0f, // c20
+ 0.0f, // c21
+ 1.0f // c22
+};
+
+constexpr DiversityCheckerParameters kMagDiversityParameters{
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.0f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f, // max_distance_tuning_param
+ 8, // min_num_diverse_vectors
+ 1 // max_num_max_distance
+};
+#endif // MAG_CAL_ENABLED
+
+} // namespace nano_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
new file mode 100644
index 0000000..68b477b
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -0,0 +1,410 @@
+/*
+ * Copyright (C) 2017 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/nano_calibration/nano_calibration.h"
+
+#include <cstring>
+
+#include "chre/util/nanoapp/log.h"
+
+namespace nano_calibration {
+namespace {
+
+using ::online_calibration::CalibrationDataThreeAxis;
+using ::online_calibration::CalibrationTypeFlags;
+using ::online_calibration::SensorData;
+using ::online_calibration::SensorIndex;
+using ::online_calibration::SensorType;
+
+// NanoSensorCal logging macros.
+#ifdef NANO_SENSOR_CAL_DBG_ENABLED
+#ifndef LOG_TAG
+#define LOG_TAG "[ImuCal]"
+#endif
+#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__)
+#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__)
+#endif // NANO_SENSOR_CAL_DBG_ENABLED
+
+} // namespace
+
+void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
+ OnlineCalibrationThreeAxis *gyro_cal,
+ OnlineCalibrationThreeAxis *mag_cal) {
+ // Loads stored calibration data and initializes the calibration algorithms.
+ 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);
+ }
+ NANO_CAL_LOGI(kAccelTag,
+ "Accelerometer runtime calibration initialized.");
+ } else {
+ accel_cal_ = nullptr;
+ NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+
+ 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);
+ }
+ NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
+ } else {
+ gyro_cal_ = nullptr;
+ NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+
+ 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);
+ }
+ NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
+ } else {
+ mag_cal_ = nullptr;
+ NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+}
+
+void NanoSensorCal::HandleSensorSamples(
+ uint16_t event_type, const chreSensorThreeAxisData *event_data) {
+ // Converts CHRE Event -> SensorData::SensorType.
+ SensorData sample;
+ switch (event_type) {
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
+ sample.type = SensorType::kAccelerometerMps2;
+ break;
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
+ sample.type = SensorType::kGyroscopeRps;
+ break;
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
+ sample.type = SensorType::kMagnetometerUt;
+ break;
+ default:
+ // This sensor type is not used.
+ NANO_CAL_LOGW("[NanoSensorCal]",
+ "Unexpected 3-axis sensor type received.");
+ return;
+ }
+
+ // Sends the sensor payload to the calibration algorithms and checks for
+ // calibration updates.
+ const auto &header = event_data->header;
+ const auto *data = event_data->readings;
+ sample.timestamp_nanos = header.baseTimestamp;
+ for (size_t i = 0; i < header.readingCount; i++) {
+ sample.timestamp_nanos += data[i].timestampDelta;
+ memcpy(sample.data, data[i].v, sizeof(sample.data));
+ ProcessSample(sample);
+ }
+}
+
+void NanoSensorCal::HandleTemperatureSamples(
+ uint16_t event_type, const chreSensorFloatData *event_data) {
+ // Computes the mean of the batched temperature samples and delivers it to the
+ // calibration algorithms. Note, the temperature sensor batch size determines
+ // its minimum update interval.
+ if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
+ event_data->header.readingCount > 0) {
+ const auto header = event_data->header;
+ const auto *data = event_data->readings;
+
+ SensorData sample;
+ sample.type = SensorType::kTemperatureCelsius;
+ sample.timestamp_nanos = header.baseTimestamp;
+
+ float accum_temperature_celsius = 0.0f;
+ for (size_t i = 0; i < header.readingCount; i++) {
+ sample.timestamp_nanos += data[i].timestampDelta;
+ accum_temperature_celsius += data[i].value;
+ }
+ sample.data[SensorIndex::kSingleAxis] =
+ accum_temperature_celsius / header.readingCount;
+ ProcessSample(sample);
+ } else {
+ NANO_CAL_LOGW("[NanoSensorCal]",
+ "Unexpected single-axis sensor type received.");
+ }
+}
+
+void NanoSensorCal::ProcessSample(const SensorData &sample) {
+ // Sends a new sensor sample to each active calibration algorithm and sends
+ // out notifications for new calibration updates.
+ if (accel_cal_ != nullptr) {
+ const CalibrationTypeFlags accel_cal_flags =
+ accel_cal_->SetMeasurement(sample);
+ if (accel_cal_flags != CalibrationTypeFlags::NONE) {
+ NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
+ accel_cal_->GetSensorCalibration(), accel_cal_flags,
+ kAccelTag);
+ PrintCalibration(accel_cal_->GetSensorCalibration(), accel_cal_flags,
+ kAccelTag);
+ }
+ }
+
+ if (gyro_cal_ != nullptr) {
+ const CalibrationTypeFlags gyro_cal_flags =
+ gyro_cal_->SetMeasurement(sample);
+ if (gyro_cal_flags != CalibrationTypeFlags::NONE) {
+ if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
+ gyro_cal_->GetSensorCalibration(),
+ gyro_cal_flags, kGyroTag)) {
+ // Limits the log messaging update rate for the gyro calibrations since
+ // these can occur frequently with rapid temperature changes.
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ sample.timestamp_nanos, gyro_notification_time_nanos_,
+ kNanoSensorCalMessageIntervalNanos)) {
+ gyro_notification_time_nanos_ = sample.timestamp_nanos;
+ PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_flags,
+ kGyroTag);
+ }
+ }
+ }
+ }
+
+ if (mag_cal_ != nullptr) {
+ const CalibrationTypeFlags mag_cal_flags = mag_cal_->SetMeasurement(sample);
+ if (mag_cal_flags != CalibrationTypeFlags::NONE) {
+ NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
+ mag_cal_->GetSensorCalibration(), mag_cal_flags,
+ kMagTag);
+ PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_flags,
+ kMagTag);
+ }
+ }
+}
+
+bool NanoSensorCal::NotifyAshCalibration(
+ uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
+ CalibrationTypeFlags flags, const char *sensor_tag) {
+ // Updates the sensor offset calibration using the ASH API.
+ ashCalInfo ash_cal_info;
+ memset(&ash_cal_info, 0, sizeof(ashCalInfo));
+ ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor).
+ 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;
+
+ if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
+ return false;
+ }
+
+ // Uses the ASH API to store ONLY the algorithm calibration parameters that
+ // have been modified by the calibration algorithm.
+ ashCalParams ash_cal_parameters;
+ if (flags & CalibrationTypeFlags::BIAS) {
+ ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius;
+ memcpy(ash_cal_parameters.offset, cal_data.offset,
+ sizeof(ash_cal_parameters.offset));
+ ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ }
+
+ if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity,
+ sizeof(ash_cal_parameters.tempSensitivity));
+ memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept,
+ sizeof(ash_cal_parameters.tempIntercept));
+ ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ }
+
+ if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
+ return false;
+ }
+
+ return true;
+}
+
+bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
+ OnlineCalibrationThreeAxis *online_cal,
+ const char *sensor_tag) {
+ ashCalParams recalled_ash_cal_parameters;
+ if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
+ &recalled_ash_cal_parameters)) {
+ // Checks whether a valid set of runtime calibration parameters was received
+ // and can be used for initialization.
+ CalibrationTypeFlags flags = CalibrationTypeFlags::NONE;
+ if (DetectRuntimeCalibration(chreSensorType, sensor_tag, &flags,
+ &recalled_ash_cal_parameters)) {
+ CalibrationDataThreeAxis cal_data;
+ cal_data.type = online_cal->get_sensor_type();
+ cal_data.cal_update_time_nanos = chreGetTime();
+
+ // Analyzes the calibration flags and sets only the runtime calibration
+ // values that were received.
+ if (flags & CalibrationTypeFlags::BIAS) {
+ cal_data.offset_temp_celsius =
+ recalled_ash_cal_parameters.offsetTempCelsius;
+ memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
+ sizeof(cal_data.offset));
+ }
+
+ if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ memcpy(cal_data.temp_sensitivity,
+ recalled_ash_cal_parameters.tempSensitivity,
+ sizeof(cal_data.temp_sensitivity));
+ memcpy(cal_data.temp_intercept,
+ recalled_ash_cal_parameters.tempIntercept,
+ sizeof(cal_data.temp_intercept));
+ }
+
+ // Sets the algorithm's initial calibration data and notifies ASH to apply
+ // the recalled calibration data.
+ if (online_cal->SetInitialCalibration(cal_data)) {
+ return NotifyAshCalibration(chreSensorType,
+ online_cal->GetSensorCalibration(), flags,
+ sensor_tag);
+ } else {
+ NANO_CAL_LOGE(sensor_tag,
+ "Calibration data failed to initialize algorithm.");
+ }
+ }
+ } else {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data.");
+ }
+
+ return false;
+}
+
+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 &&
+ ash_cal_parameters->offsetTempCelsiusSource ==
+ ASH_CAL_PARAMS_SOURCE_FACTORY;
+
+ 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);
+
+ // Since the factory calibration is applied lower in the sensor framework,
+ // initialization using these parameters must be skipped to avoid the
+ // possibility of double calibration. Returns 'false' to avoid further
+ // initialization. Note, the runtime calibration algorithms dynamically
+ // correct the residuals that the factory calibration doesn't or can't
+ // account for (e.g., aging induced error charateristics).
+ return false;
+ } else {
+ // Analyzes calibration source flags to determine whether runtime
+ // calibration values have been loaded and may be used for initialization. A
+ // valid runtime calibration source will include at least an offset.
+
+ // Converts the ASH calibration source flags to CalibrationTypeFlags. These
+ // will be used to determine which values to copy from 'ash_cal_parameters'
+ // and provide to the calibration algorithms for initialization.
+ if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->offsetTempCelsiusSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ runtime_cal_detected = true;
+ *flags = CalibrationTypeFlags::BIAS;
+ }
+
+ if (ash_cal_parameters->tempSensitivitySource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->tempInterceptSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ runtime_cal_detected = true;
+ *flags |= CalibrationTypeFlags::OVER_TEMP;
+ }
+
+ if (!runtime_cal_detected) {
+ // This is a warning (not an error) since the runtime algorithms will
+ // function correctly with no recalled calibration values. They will
+ // eventually trigger and update the system with valid calibration data.
+ NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
+ }
+ }
+
+ return runtime_cal_detected;
+}
+
+// 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]);
+}
+
+void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
+ CalibrationTypeFlags flags,
+ const char *sensor_tag) {
+ if (flags & CalibrationTypeFlags::BIAS) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+ cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
+ cal_data.offset_temp_celsius);
+ }
+
+ if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f",
+ cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1],
+ cal_data.temp_sensitivity[2]);
+ NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f",
+ cal_data.temp_intercept[0], cal_data.temp_intercept[1],
+ cal_data.temp_intercept[2]);
+ }
+}
+
+} // namespace nano_calibration
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
new file mode 100644
index 0000000..5985054
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) 2017 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 a containing class (NanoSensorCal) for dynamic runtime
+ * calibration algorithms that affect the following sensors:
+ * - Accelerometer (offset)
+ * - Gyroscope (offset, with over-temperature compensation)
+ * - Magnetometer (offset)
+ *
+ * Sensor Units:
+ * - Accelerometer [meters/sec^2]
+ * - Gyroscope [radian/sec]
+ * - Magnetometer [micro Tesla, uT]
+ * - Temperature [Celsius].
+ *
+ * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging.
+ *
+ * NOTE2: This module uses pointers to runtime calibration algorithm objects.
+ * These must be constructed and initialized outside of this class. The owner
+ * bares the burden of managing the lifetime of these objects with respect to
+ * the NanoSensorCal class which depends on these objects and handles their
+ * interaction with the Android ASH/CHRE system. This arrangement makes it
+ * convenient to modify the specific algorithm implementations (i.e., choice of
+ * calibration algorithm, parameter tuning, etc.) at the nanoapp level without
+ * the need to specialize the standard functionality implemented here.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <ash.h>
+#include <chre.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+// Common log message sensor-specific identifiers.
+constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
+constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
+constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+
+// Limits NanoSensorCal notifications to once every minute.
+constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
+
+/*
+ * NanoSensorCal is a container class for dynamic runtime calibration sensor
+ * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
+ * is to transfer sensor data to the sensor calibration algorithms and provide
+ * calibration updates to CHRE using the ASH API.
+ */
+class NanoSensorCal {
+ public:
+ // Alias used to reference the three-axis OnlineCalibration baseclass used by
+ // the runtime calibration sensor wrappers. This is for convenience and to
+ // help with code readability.
+ using OnlineCalibrationThreeAxis = online_calibration::OnlineCalibration<
+ online_calibration::CalibrationDataThreeAxis>;
+
+ 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.
+ void Initialize(OnlineCalibrationThreeAxis *accel_cal,
+ OnlineCalibrationThreeAxis *gyro_cal,
+ OnlineCalibrationThreeAxis *mag_cal);
+
+ // Sends new sensor samples to the calibration algorithms.
+ void HandleSensorSamples(uint16_t event_type,
+ const chreSensorThreeAxisData *event_data);
+
+ // Provides temperature updates to the calibration algorithms.
+ void HandleTemperatureSamples(uint16_t event_type,
+ const chreSensorFloatData *event_data);
+
+ private:
+ // 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.
+ bool LoadAshCalibration(uint8_t chreSensorType,
+ OnlineCalibrationThreeAxis *online_cal,
+ const char *sensor_tag);
+
+ // Provides sensor calibration updates using the ASH API. Returns 'true' with
+ // a successful ASH update.
+ bool NotifyAshCalibration(
+ uint8_t chreSensorType,
+ const online_calibration::CalibrationDataThreeAxis &cal_data,
+ online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+ // Checks whether 'ash_cal_parameters' is a valid set of runtime calibration
+ // data and can be used for algorithm initialization.
+ bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag,
+ online_calibration::CalibrationTypeFlags *flags,
+ ashCalParams *ash_cal_parameters);
+
+ // Helper functions for logging calibration information.
+ void PrintAshCalParams(const ashCalParams &cal_params,
+ const char *sensor_tag);
+
+ void PrintCalibration(
+ const online_calibration::CalibrationDataThreeAxis &cal_data,
+ online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+ // Pointer to the accelerometer runtime calibration object.
+ OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
+
+ // Pointer to the gyroscope runtime calibration object.
+ OnlineCalibrationThreeAxis *gyro_cal_ = nullptr;
+
+ // Limits the log messaging update rate for the gyro calibrations since these
+ // can occur frequently with rapid temperature changes.
+ uint64_t gyro_notification_time_nanos_ = 0;
+
+ // Pointer to the magnetometer runtime calibration object.
+ OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
+};
+
+} // namespace nano_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
new file mode 100644
index 0000000..3958ae8
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
@@ -0,0 +1,71 @@
+#include "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+void AccelOffsetCal::Initialize(
+ const AccelCalParameters& accel_cal_parameters) {
+ accelCalInit(&accel_cal_, &accel_cal_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ switch (sample.type) {
+ case SensorType::kAccelerometerMps2:
+ accelCalRun(&accel_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis], // [m/sec^2]
+ temperature_celsius_);
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+ // Prints debug data report.
+ accelCalDebPrint(&accel_cal_, temperature_celsius_);
+#endif
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new offset estimate, and updates the calibration data.
+ if (accelCalNewBiasAvailable(&accel_cal_)) {
+ accelCalUpdateBias(&accel_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+ &cal_data_.offset[2]);
+
+ cal_data_.offset_temp_celsius = temperature_celsius_;
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+ OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+ }
+
+ return cal_update_polling_flags_;
+}
+
+bool AccelOffsetCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[AccelOffsetCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sets the accelerometer algorithm's calibration data.
+ accelCalBiasSet(&accel_cal_, input_cal_data.offset[0],
+ input_cal_data.offset[1], input_cal_data.offset[2]);
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
new file mode 100644
index 0000000..d2ec494
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
@@ -0,0 +1,51 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+
+#include "calibration/accelerometer/accel_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for accelerometer offset calibration.
+ */
+class AccelOffsetCal final
+ : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ // Default constructor.
+ AccelOffsetCal() = default;
+
+ // Creates an AccelOffsetCal with specified algorithm parameters.
+ explicit AccelOffsetCal(const AccelCalParameters& accel_cal_parameters) {
+ Initialize(accel_cal_parameters);
+ }
+
+ // Initializes with specified algorithm parameters, and resets the calibration
+ // data.
+ void Initialize(const AccelCalParameters& accel_cal_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kAccelerometerMps2;
+ };
+
+ private:
+ // Accelerometer offset calibration algorithm data structure.
+ AccelCal accel_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
new file mode 100644
index 0000000..96c061f
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
@@ -0,0 +1,68 @@
+/*
+ * This module provides the callback functionality employed by the online sensor
+ * calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+
+#include "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+/*
+ * This codebase avoids Standard Template Library (STL) containers to maximize
+ * its usefullness on embedded hardware with basic C++ compiler support. The
+ * following uses type erasure to implement callback functionality to a user's
+ * desired class method. The idea here is to store an object pointer by
+ * instantiating a class template (Callback) which implements a virtual
+ * interface function (CallbackInterface::Call).
+ *
+ * USAGE:
+ * See unit testing for a simple example of how to use this for callback
+ * functionality.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class CallbackInterface {
+ public:
+ // Interface function that is defined to implement the desired callback.
+ virtual void Call(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags) = 0;
+ virtual ~CallbackInterface() {}
+};
+
+// ClassCallerType: Sets the object's class type for the callback.
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class ClassCallerType, class CalibrationType>
+class Callback : public CallbackInterface<CalibrationType> {
+ public:
+ // Constructors.
+ Callback() = delete;
+ Callback(ClassCallerType* obj,
+ void (ClassCallerType::*func)(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags))
+ : object_ptr_(obj), function_ptr_(func) {}
+
+ // Implements the callback to the desired class-method.
+ void Call(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags) final {
+ (object_ptr_->*function_ptr_)(cal_data, cal_update_flags);
+ }
+
+ private:
+ // Pointer to the class that owns the callback method.
+ ClassCallerType* object_ptr_;
+
+ // Templated function pointer with the required function signature.
+ // Calibration callbacks must accept:
+ // 1. Constant reference to the calibration.
+ // 2. Bitmask indicating which calibration components have been updated.
+ void (ClassCallerType::*function_ptr_)(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags);
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
new file mode 100644
index 0000000..23ebe4f
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
@@ -0,0 +1,22 @@
+#include "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+ CalibrationTypeFlags rhs) {
+ return static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+ static_cast<char>(rhs));
+}
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs) {
+ return static_cast<char>(lhs) & static_cast<char>(rhs);
+}
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+ CalibrationTypeFlags rhs) {
+ lhs = static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+ static_cast<char>(rhs));
+ return lhs;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
new file mode 100644
index 0000000..8b95ef7
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
@@ -0,0 +1,196 @@
+/*
+ * This module provides the component definitions used to represent sensor
+ * calibration data, labeled flags/enumerators, and the callback functionality
+ * employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_model.h"
+
+namespace online_calibration {
+
+/*
+ * Bitmask used to indicate which calibration values have changed for a given
+ * calibration update.
+ *
+ * [Bit Flag] | [Affected Sensor Calibration Value]
+ * BIAS - Quasi-static non-zero sensor output (offset), given conditions
+ * where the sensor should ideally output zero. Includes
+ * corrections for over-temperature compensation.
+ * SCALE_FACTOR - Sensor axis scaling (ideally unity).
+ * CROSS_AXIS - Output sensitivity due to variations of perpendicular sensing
+ * axes (ideally zero).
+ * OVER_TEMP - Model parameters that capture variations in sensor behavior
+ * with temperature (e.g., linear bias sensitivity model).
+ */
+enum class CalibrationTypeFlags : uint8_t {
+ NONE = 0x00,
+ BIAS = 0x01,
+ SCALE_FACTOR = 0x02,
+ CROSS_AXIS = 0x04,
+ OVER_TEMP = 0x08,
+ ALL = 0xFF
+};
+
+// Logic operators to assist with common bitmask setting/checking.
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+ CalibrationTypeFlags rhs);
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs);
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+ CalibrationTypeFlags rhs);
+
+/*
+ * Defines the calibration data specific to a prototypical three-axis sensing
+ * device (e.g., accelerometer, gyroscope, magnetometer).
+ *
+ * Calibration correction may be applied as:
+ * corrected_data = scale_skew_matrix * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied
+ * when supported by the underlying calibration algorithm).
+ *
+ * The 'scale_skew_matrix' is assumed to be in lower diagonal form where the
+ * sensor frame reference definition is such that cross-axis sensitivities
+ * cross_axis_xy, cross_axis_xz, and cross_axis_yz are set to zero.
+ *
+ * scale_skew_matrix = [scale_factor_x 0 0
+ * cross_axis_yx scale_factor_y 0
+ * cross_axis_zx cross_axis_zy scale_factor_z]
+ *
+ * NOTE1: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ * temp_intercept - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ * compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ *
+ * NOTE2: Unless otherwise stated, 3-dimensional array indices: 0=x, 1=y, 2=z.
+ */
+struct CalibrationDataThreeAxis {
+ // Timestamp for the most recent calibration update.
+ uint64_t cal_update_time_nanos = 0;
+
+ // The sensor's offset (i.e., bias) in the x-, y-, z-axes at
+ // 'offset_temp_celsius'.
+ float offset[3];
+
+ // The temperature associated with the sensor offset.
+ float offset_temp_celsius;
+
+ // The temperature sensitivity of the offset.
+ float temp_sensitivity[3]; // [sensor_units/Celsius]
+
+ // The sensor's offset at zero degrees Celsius.
+ float temp_intercept[3]; // [sensor_units]
+
+ // The sensor's scale factor for each axis.
+ float scale_factor[3];
+
+ // The cross-axis factors in order: [0=yx, 1=zx, 2=zy].
+ float cross_axis[3];
+
+ // 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
+ // over the linear model parameters provided in the calibration data.
+ OverTempModelThreeAxis* otc_model_data = nullptr;
+ int16_t num_model_pts = 0;
+
+ // Helper function that resets the calibration data to a set of neutral
+ // reference values where no calibration correction would be applied if used.
+ void reset() {
+ otc_model_data = nullptr;
+ offset_temp_celsius = 0.0f;
+ scale_factor[0] = 1.0f;
+ scale_factor[1] = 1.0f;
+ scale_factor[2] = 1.0f;
+ memset(offset, 0, sizeof(offset));
+ memset(temp_sensitivity, 0, sizeof(temp_sensitivity));
+ memset(temp_intercept, 0, sizeof(temp_intercept));
+ memset(cross_axis, 0, sizeof(cross_axis));
+ }
+
+ CalibrationDataThreeAxis() { reset(); }
+};
+
+/*
+ * Defines the calibration data for single dimensional sensing device (e.g.,
+ * thermometer, barometer).
+ *
+ * Calibration correction may be applied as:
+ * corrected_data = scale_factor * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied,
+ * if supported by the underlying calibration algorithm).
+ *
+ * NOTE: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ * temp_intercept - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ * compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ */
+struct CalibrationDataSingleAxis {
+ // Timestamp for the most recent calibration update.
+ uint64_t cal_update_time_nanos = 0;
+
+ // The sensor's offset (i.e., bias) at temperature, 'offset_temp_celsius'.
+ float offset;
+
+ // The temperature associated with the sensor offset.
+ float offset_temp_celsius;
+
+ // The temperature sensitivity of the offset.
+ float temp_sensitivity; // [sensor_units/Celsius]
+
+ // The sensor's offset at zero degrees Celsius.
+ float temp_intercept; // [sensor_units]
+
+ // The sensor's scale factor.
+ float scale_factor;
+
+ // 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
+ // over the linear model parameters provided in the calibration data.
+ OverTempModelSingleAxis* otc_model_data = nullptr;
+ int16_t num_model_pts = 0;
+
+ // Helper function that resets the calibration data to a set of neutral
+ // reference values where no calibration correction would be applied if used.
+ void reset() {
+ otc_model_data = nullptr;
+ scale_factor = 1.0f;
+ offset_temp_celsius = 0.0f;
+ offset = 0.0f;
+ temp_sensitivity = 0.0f;
+ temp_intercept = 0.0f;
+ }
+
+ CalibrationDataSingleAxis() { reset(); }
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
new file mode 100644
index 0000000..bf348ac
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
@@ -0,0 +1,128 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+
+#include <string.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This abstract base class provides a set of general interface functions for
+ * calibration algorithms. The data structures used are intended to be lean and
+ * portable to a wide variety of software and hardware systems. Algorithm
+ * wrappers may use this as a basis for providing the following functionality:
+ *
+ * SetMeasurement - Delivers new sensor data.
+ * SetInitialCalibration - Initializes the algorithm's calibration data.
+ * GetSensorCalibration - Retrieves the latest calibration data set.
+ * new_calibration_ready - Used to poll for new calibration updates.
+ * SetCalibrationCallback - User provides a pointer its own Callback object.
+ * UpdateDynamicSystemSettings - Provides feedback to adjust system behavior.
+ * get_sensor_type - Returns the sensor type which is being calibrated.
+ *
+ * NOTE 1: This class accomodates two methods of providing calibration updates.
+ * Either, or both, may be used depending on system requirements. 1) Polling can
+ * be achieved with new_calibration_ready/GetSensorCalibration functions. 2)
+ * Callback notification of new calibration updates can managed using the
+ * SetCalibrationCallback function.
+ *
+ * NOTE 2: This code implementation specifically avoids using standard template
+ * libraries (STL) and other external API’s since they may not be fully
+ * supported on embedded hardware targets. Only basic C/C++ support will be
+ * assumed.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class OnlineCalibration {
+ public:
+ // Virtual destructor.
+ virtual ~OnlineCalibration() {}
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // "true" if set successfully.
+ virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0;
+
+ // Polling Updates: New calibration updates are generated during
+ // SetMeasurement and the 'cal_update_polling_flags_' are set according to
+ // which calibration values have changed. To prevent missing updates in
+ // systems that use polling, this bitmask remains latched until the
+ // calibration data is retrieved with this function.
+ const CalibrationType& GetSensorCalibration() const {
+ cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+ return cal_data_;
+ }
+
+ // Polling Updates: This function returns 'cal_update_polling_flags_' to
+ // indicate which calibration components have a pending update. The updated
+ // calibration data may be retrieved with GetSensorCalibration, and the
+ // 'cal_update_polling_flags_' will reset.
+ CalibrationTypeFlags new_calibration_ready() const {
+ return cal_update_polling_flags_;
+ }
+
+ // Sets the pointer to the CallbackInterface object used for notification of
+ // new calibration updates.
+ void SetCalibrationCallback(
+ CallbackInterface<CalibrationType>* calibration_callback) {
+ calibration_callback_ = calibration_callback;
+ }
+
+ // This provides operational feedback to the system based on status such as
+ // active sensors and sampling rates. Feedback may include adjusting
+ // system-level settings including sensor sampling rate, latency,
+ // active/passive subscription, etc. Returns "true" if a system setting is
+ // requested to change.
+ // TODO(davejacobs) -- Determine implementation details.
+ virtual bool UpdateDynamicSystemSettings(
+ /*System Parameters In, System Feedback Out*/) const {
+ return false;
+ }
+
+ // Returns the sensor-type this calibration algorithm provides updates for.
+ virtual SensorType get_sensor_type() const = 0;
+
+ protected:
+ // Helper function that activates the registered callback.
+ void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const {
+ if (calibration_callback_ != nullptr) {
+ calibration_callback_->Call(cal_data_, cal_update_flags);
+ }
+ }
+
+ // Helper function used to initialize the calibration data.
+ void InitializeCalData() {
+ cal_data_.reset();
+ cal_data_.type = get_sensor_type();
+ cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+ }
+
+ // Stores the sensor calibration data.
+ CalibrationType cal_data_;
+
+ // Tracks the most recent sensor temperature value.
+ float temperature_celsius_ = kInvalidTemperatureCelsius;
+
+ // This bitmask indicates which subset of calibration parameters have changed
+ // and is used specifically for polling; the callback notification passes its
+ // own set of update flags which do not need this latching behavior. Marked
+ // mutable so that these flags may be reset when GetSensorCalibration is
+ // called.
+ mutable CalibrationTypeFlags cal_update_polling_flags_ =
+ CalibrationTypeFlags::NONE;
+
+ private:
+ // Pointer to a callback object.
+ CallbackInterface<CalibrationType>* calibration_callback_ = nullptr;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
new file mode 100644
index 0000000..9849ad2
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -0,0 +1,83 @@
+/*
+ * This module provides the component definitions used to represent sensor
+ * data employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+namespace online_calibration {
+
+// Defines an invalid or uninitialized temperature value.
+constexpr float kInvalidTemperatureCelsius = -274.0f;
+
+// Unit conversion from nanoseconds to microseconds.
+constexpr uint64_t NanoToMicroseconds(uint64_t x) { return x / 1000; }
+
+// Identifies the various sensing devices used by the calibration algorithms.
+enum class SensorType : int8_t {
+ kUndefined = 0,
+ kAccelerometerMps2 = 1, // 3-axis sensor (units = meter/sec^2).
+ kGyroscopeRps = 2, // 3-axis sensor (units = radian/sec).
+ kMagnetometerUt = 3, // 3-axis sensor (units = micro-Tesla).
+ kTemperatureCelsius = 4, // 1-axis sensor (units = degrees Celsius).
+ kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal).
+ kWifiM = 6 // 3-axis sensor (units = meter).
+};
+
+/*
+ * SensorData is a generalized data structure used to represent sensor samples
+ * produced by either a single- or three-axis device. Usage is implied through
+ * the sensor type (i.e., Gyroscope is a three-axis sensor and would therefore
+ * use all elements of 'data'; a pressure sensor is single-dimensional and would
+ * use 'data[SensorIndex::kSingleAxis]'). This arbitration is determined
+ * at the algorithm wrapper level where knowledge of a sensor's dimensionality
+ * is clearly understood.
+ *
+ * NOTE: The unified dimensional representation makes it convenient to pass
+ * either type of data into the interface functions defined in the
+ * OnlineCalibration.
+ */
+
+// Axis index definitions for SensorData::data.
+enum SensorIndex : int8_t {
+ kSingleAxis = 0,
+ kXAxis = kSingleAxis,
+ kYAxis = 1,
+ kZAxis = 2,
+};
+
+struct SensorData {
+ // Indicates the type of sensor this data originated from.
+ SensorType type;
+
+ // Sensor sample timestamp.
+ uint64_t timestamp_nanos;
+
+ // Generalized sensor sample (represents either single- or three-axis data).
+ float data[3];
+
+ SensorData() : type(SensorType::kUndefined), timestamp_nanos(0) {
+ memset(data, 0, sizeof(data));
+ }
+
+ SensorData(SensorType type, uint64_t ts, float x, float y, float z)
+ : type(type), timestamp_nanos(ts) {
+ data[SensorIndex::kXAxis] = x;
+ data[SensorIndex::kYAxis] = y;
+ data[SensorIndex::kZAxis] = z;
+ }
+
+ SensorData(SensorType type, uint64_t ts, float single_axis_sample)
+ : type(type), timestamp_nanos(ts) {
+ data[SensorIndex::kSingleAxis] = single_axis_sample;
+ }
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
new file mode 100644
index 0000000..67ea497
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
@@ -0,0 +1,150 @@
+#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 {
+
+void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters) {
+ gyroCalInit(&gyro_cal_, &gyro_cal_parameters);
+ overTempCalInit(&over_temp_cal_, &otc_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
+ const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ switch (sample.type) {
+ case SensorType::kAccelerometerMps2:
+ gyroCalUpdateAccel(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]); // [m/sec^2]
+ break;
+
+ case SensorType::kGyroscopeRps:
+ gyroCalUpdateGyro(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis], // [rad/sec]
+ temperature_celsius_);
+ break;
+
+ case SensorType::kMagnetometerUt:
+ gyroCalUpdateMag(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]); // [micro-Tesla]
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ overTempCalSetTemperature(&over_temp_cal_, sample.timestamp_nanos,
+ temperature_celsius_);
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new calibration, and updates the OTC.
+ if (gyroCalNewBiasAvailable(&gyro_cal_)) {
+ float offset[3];
+ float temperature_celsius = kInvalidTemperatureCelsius;
+ uint64_t calibration_time_nanos = 0;
+ gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
+ &temperature_celsius, &calibration_time_nanos);
+ overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
+ offset, temperature_celsius);
+ }
+
+ // Checks the OTC for a new calibration model update.
+ const bool new_otc_model_update =
+ overTempCalNewModelUpdateAvailable(&over_temp_cal_);
+
+ // Checks for a change in the temperature compensated offset estimate.
+ const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_);
+
+ // Sets the new calibration data.
+ CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE;
+ if (new_otc_offset) {
+ overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius,
+ cal_data_.offset);
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_callback_flags |= CalibrationTypeFlags::BIAS;
+ }
+
+ if (new_otc_model_update) {
+ // Sets the pointer to the OTC model dataset and the number of model points.
+ cal_data_.otc_model_data = over_temp_cal_.model_data;
+ cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+ cal_update_callback_flags |= CalibrationTypeFlags::OVER_TEMP;
+ overTempCalGetModel(&over_temp_cal_, cal_data_.offset,
+ &cal_data_.offset_temp_celsius,
+ &cal_data_.cal_update_time_nanos,
+ cal_data_.temp_sensitivity, cal_data_.temp_intercept);
+ }
+
+ // Sets the new-calibration polling flag, and notifies a calibration callback
+ // listener of the new update.
+ if (new_otc_model_update || new_otc_offset) {
+ cal_update_polling_flags_ |= cal_update_callback_flags;
+ OnNotifyCalibrationUpdate(cal_update_callback_flags);
+ }
+
+ // Print debug data reports.
+#ifdef GYRO_CAL_DBG_ENABLED
+ gyroCalDebugPrint(&gyro_cal_, sample.timestamp_nanos);
+#endif // GYRO_CAL_DBG_ENABLED
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ overTempCalDebugPrint(&over_temp_cal_, sample.timestamp_nanos);
+#endif // OVERTEMPCAL_DBG_ENABLED
+
+ return cal_update_polling_flags_;
+}
+
+bool GyroOffsetOtcCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[GyroOffsetOtcCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ // Sets the initial calibration data (offset and OTC model parameters).
+ gyroCalSetBias(&gyro_cal_, cal_data_.offset[0], cal_data_.offset[1],
+ cal_data_.offset[2], cal_data_.offset_temp_celsius,
+ cal_data_.cal_update_time_nanos);
+
+ overTempCalSetModel(&over_temp_cal_, cal_data_.offset,
+ cal_data_.offset_temp_celsius,
+ cal_data_.cal_update_time_nanos,
+ cal_data_.temp_sensitivity, cal_data_.temp_intercept,
+ /*jump_start_model=*/false);
+
+ const bool load_new_model_dataset =
+ (input_cal_data.otc_model_data != nullptr &&
+ input_cal_data.num_model_pts > 0);
+
+ if (load_new_model_dataset) {
+ // Loads the new model dataset and uses it to update the linear model
+ // parameters.
+ overTempCalSetModelData(&over_temp_cal_, input_cal_data.num_model_pts,
+ cal_data_.cal_update_time_nanos,
+ input_cal_data.otc_model_data);
+ }
+
+ // Sets the pointer to the OTC model dataset and the number of model points.
+ cal_data_.otc_model_data = over_temp_cal_.model_data;
+ cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
new file mode 100644
index 0000000..74ba982
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
@@ -0,0 +1,58 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_cal.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the gyroscope offset calibration with
+ * over-temperature compensation (OTC).
+ */
+class GyroOffsetOtcCal final
+ : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ // Default constructor.
+ GyroOffsetOtcCal() = default;
+
+ // Creates an GyroOffsetOtcCal with specified algorithm parameters.
+ GyroOffsetOtcCal(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters) {
+ Initialize(gyro_cal_parameters, otc_parameters);
+ }
+
+ // Initializes with specified algorithm parameters, and resets the calibration
+ // data.
+ void Initialize(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kGyroscopeRps;
+ };
+
+ private:
+ // GyroCal algorithm data structure.
+ GyroCal gyro_cal_;
+
+ // Over-temperature offset compensation algorithm data structure.
+ OverTempCal over_temp_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
new file mode 100644
index 0000000..a963dcd
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
@@ -0,0 +1,68 @@
+#include "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+void MagDiverseCal::Initialize(
+ const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters) {
+ initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ MagUpdate new_calibration_update = MagUpdate::NO_UPDATE;
+ switch (sample.type) {
+ case SensorType::kMagnetometerUt:
+ new_calibration_update = magCalUpdate(
+ &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos),
+ sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]);
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new offset estimate, and updates the calibration data.
+ if (MagUpdate::UPDATE_BIAS & new_calibration_update) {
+ magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+ &cal_data_.offset[2]);
+
+ cal_data_.offset_temp_celsius = temperature_celsius_;
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+ OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+ }
+
+ return cal_update_polling_flags_;
+}
+
+bool MagDiverseCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[MagDiverseCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sets the magnetometer algorithm's calibration data.
+ magCalReset(&mag_cal_); // Resets the magnetometer's offset vector.
+ magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1],
+ input_cal_data.offset[2]);
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
new file mode 100644
index 0000000..23ac3c4
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
@@ -0,0 +1,52 @@
+#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/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the magnetometer offset calibration with
+ * diversity checking.
+ */
+class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ MagDiverseCal() = default;
+
+ // Creates an MagDiverseCal with specified algorithm parameters.
+ MagDiverseCal(const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters) {
+ Initialize(mag_cal_parameters, diversity_parameters);
+ }
+
+ // Initializes with specified algorithm parameters.
+ void Initialize(const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kMagnetometerUt;
+ };
+
+ private:
+ // MagCal algorithm data structure.
+ MagCal mag_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
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 d92f1da..908d383 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -18,11 +18,9 @@
#include <float.h>
#include <math.h>
-#include <stdio.h>
#include <string.h>
#include "calibration/util/cal_log.h"
-#include "common/math/macros.h"
#include "util/nano_assert.h"
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,40 +30,39 @@
// Defines the default weighting function for the linear model fit routine.
// Weighting = 10.0; for offsets newer than 5 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+static const struct OverTempCalWeight kOtcDefaultWeight0 = {
.offset_age_nanos = MIN_TO_NANOS(5),
.weight = 10.0f,
};
// Weighting = 0.1; for offsets newer than 15 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+static const struct OverTempCalWeight kOtcDefaultWeight1 = {
.offset_age_nanos = MIN_TO_NANOS(15),
.weight = 0.1f,
};
// The default weighting used for all older offsets.
-#define OTC_MIN_WEIGHT_VALUE (0.04f)
+#define OTC_MIN_WEIGHT_VALUE (0.04f)
#ifdef OVERTEMPCAL_DBG_ENABLED
// A debug version label to help with tracking results.
-#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
+#define OTC_DEBUG_VERSION_STRING "[Jan 10, 2018]"
// The time interval used to throttle debug messaging (100msec).
#define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
-// The time interval used to throttle temperture print messaging (1 second).
+// The time interval used to throttle temperature print messaging (1 second).
#define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
-static const char kDebugAxisLabel[3] = "XYZ";
+static const char kDebugAxisLabel[3] = "XYZ";
#endif // OVERTEMPCAL_DBG_ENABLED
/////// FORWARD DECLARATIONS //////////////////////////////////////////////////
// Updates the latest received model estimate data.
static void setLatestEstimate(struct OverTempCal *over_temp_cal,
- const float *offset, float offset_temp_celsius,
- uint64_t timestamp_nanos);
+ const float *offset, float offset_temp_celsius);
/*
* Determines if a new over-temperature model fit should be performed, and then
@@ -103,11 +100,10 @@
* Since it may take a while for an empty model to build up enough data to start
* producing new model parameter updates, the model collection can be
* jump-started by using the new model parameters to insert "fake" data in place
- * of actual sensor offset data. 'timestamp_nanos' sets the timestamp for the
- * new model data.
+ * of actual sensor offset data. The new model data 'offset_age_nanos' is set to
+ * zero.
*/
-static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos);
+static bool jumpStartModelData(struct OverTempCal *over_temp_cal);
/*
* Computes a new model fit and provides updated model parameters for the
@@ -116,7 +112,6 @@
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
- * timestamp_nanos: Current timestamp for the model update.
* OUTPUTS:
* temp_sensitivity: Updated modeled temperature sensitivity (array).
* sensor_intercept: Updated model intercept (array).
@@ -127,8 +122,7 @@
* Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992.
*/
static void updateModel(const struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos, float *temp_sensitivity,
- float *sensor_intercept);
+ float *temp_sensitivity, float *sensor_intercept);
/*
* Computes a new over-temperature compensated offset estimate based on the
@@ -183,7 +177,8 @@
// Returns "true" if the candidate linear model parameters are within the valid
// range, and not all zeros.
static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
- float temp_sensitivity, float sensor_intercept);
+ float temp_sensitivity,
+ float sensor_intercept);
// Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
@@ -191,8 +186,12 @@
// Returns the least-squares weight based on the age of a particular offset
// estimate.
static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
- uint64_t offset_timestamp_nanos,
- uint64_t current_timestamp_nanos);
+ uint64_t offset_age_nanos);
+
+// Computes the age increment, adds it to the age of each OTC model data point,
+// and resets the age update counter.
+static void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos);
// Updates 'compensated_offset' using the linear OTC model.
static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -208,9 +207,10 @@
float delta_temp_celsius);
// Provides an over-temperature compensated offset based on the 'estimate'.
-static void compensateWithEstimate(
- struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
- struct OverTempCalDataPt *estimate, float temperature_celsius);
+static void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ struct OverTempModelThreeAxis *estimate,
+ float temperature_celsius);
// Evaluates the nearest-temperature compensation (with linear extrapolation
// term due to temperature), and compares it with the compensation due to
@@ -233,7 +233,7 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
// This helper function stores all of the debug tracking information necessary
// for printing log messages.
-static void updateDebugData(struct OverTempCal* over_temp_cal);
+static void updateDebugData(struct OverTempCal *over_temp_cal);
// Helper function that creates tag strings useful for identifying specific
// debug output data (embedded system friendly; not all systems have 'sprintf').
@@ -251,12 +251,7 @@
/////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
void overTempCalInit(struct OverTempCal *over_temp_cal,
- size_t min_num_model_pts,
- uint64_t min_temp_update_period_nanos,
- float delta_temp_per_bin, float jump_tolerance,
- float outlier_limit, uint64_t age_limit_nanos,
- float temp_sensitivity_limit, float sensor_intercept_limit,
- float significant_offset_change, bool over_temp_enable) {
+ const struct OverTempCalParameters *parameters) {
ASSERT_NOT_NULL(over_temp_cal);
// Clears OverTempCal memory.
@@ -264,7 +259,7 @@
// Initializes the pointers to important sensor offset estimates.
over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
- over_temp_cal->latest_offset = NULL;
+ over_temp_cal->latest_offset = NULL;
// Initializes the OTC linear model parameters.
resetOtcLinearModel(over_temp_cal);
@@ -272,25 +267,23 @@
// Initializes the model identification parameters.
over_temp_cal->new_overtemp_model_available = false;
over_temp_cal->new_overtemp_offset_available = false;
- over_temp_cal->min_num_model_pts = min_num_model_pts;
- over_temp_cal->min_temp_update_period_nanos = min_temp_update_period_nanos;
- over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
- over_temp_cal->jump_tolerance = jump_tolerance;
- over_temp_cal->outlier_limit = outlier_limit;
- over_temp_cal->age_limit_nanos = age_limit_nanos;
- over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
- over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
- over_temp_cal->significant_offset_change = significant_offset_change;
- over_temp_cal->over_temp_enable = over_temp_enable;
+ over_temp_cal->min_num_model_pts = parameters->min_num_model_pts;
+ over_temp_cal->min_temp_update_period_nanos =
+ parameters->min_temp_update_period_nanos;
+ over_temp_cal->delta_temp_per_bin = parameters->delta_temp_per_bin;
+ over_temp_cal->jump_tolerance = parameters->jump_tolerance;
+ over_temp_cal->outlier_limit = parameters->outlier_limit;
+ over_temp_cal->age_limit_nanos = parameters->age_limit_nanos;
+ over_temp_cal->temp_sensitivity_limit = parameters->temp_sensitivity_limit;
+ over_temp_cal->sensor_intercept_limit = parameters->sensor_intercept_limit;
+ over_temp_cal->significant_offset_change =
+ parameters->significant_offset_change;
+ over_temp_cal->over_temp_enable = parameters->over_temp_enable;
// Initializes the over-temperature compensated offset temperature.
over_temp_cal->compensated_offset.offset_temp_celsius =
OTC_TEMP_INVALID_CELSIUS;
- // Defines the default weighting function for the linear model fit routine.
- overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
- overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
-
#ifdef OVERTEMPCAL_DBG_ENABLED
// Sets the default sensor descriptors for debugging.
overTempCalDebugDescriptors(over_temp_cal, "OVER_TEMP_CAL", "mDPS",
@@ -305,6 +298,10 @@
"Over-temperature compensation DISABLED.");
}
#endif // OVERTEMPCAL_DBG_ENABLED
+
+ // Defines the default weighting function for the linear model fit routine.
+ overTempValidateAndSetWeight(over_temp_cal, 0, &kOtcDefaultWeight0);
+ overTempValidateAndSetWeight(over_temp_cal, 1, &kOtcDefaultWeight1);
}
void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
@@ -332,8 +329,7 @@
// Model "Jump-Start".
const bool model_jump_started =
- (jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos)
- : false;
+ jump_start_model ? jumpStartModelData(over_temp_cal) : false;
if (!model_jump_started) {
// Checks that the new offset data is valid.
@@ -342,7 +338,7 @@
memcpy(over_temp_cal->model_data[0].offset, offset,
sizeof(over_temp_cal->model_data[0].offset));
over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
+ over_temp_cal->model_data[0].offset_age_nanos = 0;
over_temp_cal->num_model_pts = 1;
} else {
// No valid offset data to load.
@@ -361,7 +357,7 @@
memcpy(over_temp_cal->compensated_offset.offset, offset,
sizeof(over_temp_cal->compensated_offset.offset));
over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
+ over_temp_cal->compensated_offset.offset_age_nanos = 0;
}
// Resets the latest offset pointer. There are no new offset estimates to
@@ -435,7 +431,7 @@
void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
size_t data_length, uint64_t timestamp_nanos,
- const struct OverTempCalDataPt *model_data) {
+ const struct OverTempModelThreeAxis *model_data) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(model_data);
@@ -446,11 +442,7 @@
if (isValidOtcOffset(model_data[i].offset,
model_data[i].offset_temp_celsius)) {
memcpy(&over_temp_cal->model_data[i], &model_data[i],
- sizeof(struct OverTempCalDataPt));
-
- // Updates the model time stamps to the current load time.
- over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
-
+ sizeof(struct OverTempModelThreeAxis));
valid_data_count++;
}
}
@@ -491,11 +483,11 @@
void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
size_t *data_length,
- struct OverTempCalDataPt *model_data) {
+ struct OverTempModelThreeAxis *model_data) {
ASSERT_NOT_NULL(over_temp_cal);
*data_length = over_temp_cal->num_model_pts;
memcpy(model_data, over_temp_cal->model_data,
- over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
+ over_temp_cal->num_model_pts * sizeof(struct OverTempModelThreeAxis));
}
void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
@@ -559,6 +551,9 @@
ASSERT_NOT_NULL(offset);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+ // Updates the age of each OTC model data point.
+ modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+
// Checks that the new offset data is valid, returns if bad.
if (!isValidOtcOffset(offset, temperature_celsius)) {
return;
@@ -586,8 +581,8 @@
createDebugTag(over_temp_cal, ":OUTLIER]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temperature|Time [%s|C|nsec]: "
- CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+ "Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %llu",
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),
@@ -648,8 +643,8 @@
// oldest data with the incoming one.
over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
- if (over_temp_cal->latest_offset->timestamp_nanos <
- over_temp_cal->model_data[i].timestamp_nanos) {
+ if (over_temp_cal->latest_offset->offset_age_nanos <
+ over_temp_cal->model_data[i].offset_age_nanos) {
over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
}
}
@@ -657,8 +652,7 @@
}
// Updates the latest model estimate data.
- setLatestEstimate(over_temp_cal, offset, temperature_celsius,
- timestamp_nanos);
+ setLatestEstimate(over_temp_cal, offset, temperature_celsius);
// The latest offset estimate is the nearest temperature offset.
over_temp_cal->nearest_offset = over_temp_cal->latest_offset;
@@ -712,6 +706,13 @@
#endif // OVERTEMPCAL_DBG_LOG_TEMP
#endif // OVERTEMPCAL_DBG_ENABLED
+ // Updates the age of each OTC model data point.
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->last_age_update_nanos,
+ OTC_MODEL_AGE_UPDATE_NANOS)) {
+ modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+ }
+
// This check throttles new OTC offset compensation updates so that high data
// rate temperature samples do not cause excessive computational burden. Note,
// temperature sensor updates are expected to potentially increase the data
@@ -720,7 +721,7 @@
if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
timestamp_nanos, over_temp_cal->last_offset_update_nanos,
over_temp_cal->min_temp_update_period_nanos)) {
- return; // Time interval too short, skip further data processing.
+ return; // Time interval too short, skip further data processing.
}
// Checks that the offset temperature is within a valid range, saturates if
@@ -746,8 +747,8 @@
}
void overTempGetModelError(const struct OverTempCal *over_temp_cal,
- const float *temp_sensitivity, const float *sensor_intercept,
- float *max_error) {
+ const float *temp_sensitivity,
+ const float *sensor_intercept, float *max_error) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
@@ -770,14 +771,34 @@
}
}
-// TODO(davejacobs): Refactor to implement a compliance check on the storage of
-// 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
struct OverTempCal *over_temp_cal, size_t index,
- const struct OverTempCalWeightPt *new_otc_weight) {
- if (index < OTC_NUM_WEIGHT_LEVELS) {
- over_temp_cal->weighting_function[index] = *new_otc_weight;
+ const struct OverTempCalWeight *new_otc_weight) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(new_otc_weight);
+
+ // The input weighting coefficient must be positive.
+ if (new_otc_weight->weight <= 0.0f) {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Invalid weight: Must > 0.");
+#endif // OVERTEMPCAL_DBG_ENABLED
+ return false;
}
+
+ // Ensures that the 'index-1' weight's age is younger.
+ if (index == 0 ||
+ over_temp_cal->weighting_function[index - 1].offset_age_nanos <
+ new_otc_weight->offset_age_nanos) {
+ over_temp_cal->weighting_function[index] = *new_otc_weight;
+ return true;
+ }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Non monotonic weight age.");
+#endif // OVERTEMPCAL_DBG_ENABLED
+ return false;
}
/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
@@ -827,9 +848,10 @@
}
}
-void compensateWithEstimate(
- struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
- struct OverTempCalDataPt *estimate, float temperature_celsius) {
+void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ struct OverTempModelThreeAxis *estimate,
+ float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(estimate);
@@ -927,9 +949,9 @@
// than one estimate in the model (i.e., don't want to remove all data, even
// if it is very old [something is likely better than nothing]).
if ((timestamp_nanos >=
- OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
+ OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer_nanos) &&
over_temp_cal->num_model_pts > 1) {
- over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer.
+ over_temp_cal->stale_data_timer_nanos = timestamp_nanos; // Resets timer.
removeStaleModelData(over_temp_cal, timestamp_nanos);
}
@@ -944,12 +966,25 @@
// not empty.
const bool model_points_available = (over_temp_cal->num_model_pts > 0);
+ // 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;
+
// True when the latest offset estimate will be used to compute a sensor
// offset calibration estimate.
const bool use_latest_offset_compensation =
- over_temp_cal->latest_offset && model_points_available &&
- timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos +
- OTC_USE_RECENT_OFFSET_TIME_NANOS;
+ over_temp_cal->latest_offset != NULL && model_points_available &&
+ latest_offset_age_nanos <= OTC_USE_RECENT_OFFSET_TIME_NANOS;
// True when the conditions are met to use the nearest-temperature offset to
// compute a sensor offset calibration estimate.
@@ -958,7 +993,7 @@
// ii. Offset temperature must be within a small neighborhood of the
// current measured temperature (+/- 'delta_temp_per_bin').
const bool can_compensate_with_nearest =
- model_points_available && over_temp_cal->nearest_offset &&
+ model_points_available && over_temp_cal->nearest_offset != NULL &&
NANO_ABS(temperature_celsius -
over_temp_cal->nearest_offset->offset_temp_celsius) <
over_temp_cal->delta_temp_per_bin;
@@ -966,18 +1001,14 @@
// True if the last received sensor offset estimate is old or non-existent.
const bool latest_model_point_not_relevant =
(over_temp_cal->latest_offset == NULL) ||
- (over_temp_cal->latest_offset &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, over_temp_cal->latest_offset->timestamp_nanos,
- OTC_OFFSET_IS_STALE_NANOS));
+ (over_temp_cal->latest_offset != NULL &&
+ latest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
// True if the nearest-temperature offset estimate is old or non-existent.
const bool nearest_model_point_not_relevant =
(over_temp_cal->nearest_offset == NULL) ||
- (over_temp_cal->nearest_offset &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, over_temp_cal->nearest_offset->timestamp_nanos,
- OTC_OFFSET_IS_STALE_NANOS));
+ (over_temp_cal->nearest_offset != NULL &&
+ nearest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
// ---------------------------------------------------------------------------
// The following conditional expressions govern new OTC offset updates.
@@ -1051,26 +1082,25 @@
over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available;
// If the offset has changed significantly, then the offset compensation
- // vector and timestamp are updated.
+ // vector is updated.
if (new_overtemp_offset_available) {
memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
sizeof(over_temp_cal->compensated_offset.offset));
- over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
}
}
void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
- float offset_temp_celsius, uint64_t timestamp_nanos) {
+ float offset_temp_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(offset);
- if (over_temp_cal->latest_offset) {
+ if (over_temp_cal->latest_offset != NULL) {
// Sets the latest over-temp calibration estimate.
memcpy(over_temp_cal->latest_offset->offset, offset,
sizeof(over_temp_cal->latest_offset->offset));
over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
+ over_temp_cal->latest_offset->offset_age_nanos = 0;
}
}
@@ -1098,19 +1128,17 @@
// Ensures that the minimum number of points required for a model fit has been
// satisfied.
- if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts)
- return;
+ if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) return;
// Updates the linear model fit.
float temp_sensitivity[3];
float sensor_intercept[3];
- updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity,
- sensor_intercept);
+ updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);
// 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
- // a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
- // b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
+ // a. |temp_sensitivity| < temp_sensitivity_limit
+ // b. |sensor_intercept| < sensor_intercept_limit
// NOTE: Model parameter updates are not qualified against model fit error
// here to protect against the case where there is large change in the
// temperature characteristic either during runtime (e.g., temperature
@@ -1186,9 +1214,8 @@
bool removed_one = false;
for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
- if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
- timestamp_nanos > over_temp_cal->age_limit_nanos +
- over_temp_cal->model_data[i].timestamp_nanos) {
+ if (over_temp_cal->model_data[i].offset_age_nanos >=
+ over_temp_cal->age_limit_nanos) {
// If the latest offset was removed, then indicate this by setting it to
// NULL.
if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) {
@@ -1223,7 +1250,7 @@
createDebugTag(over_temp_cal, ":REMOVE]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ "Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_3DIGITS ", %llu",
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
@@ -1238,21 +1265,20 @@
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]
- .timestamp_nanos);
+ .offset_age_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
// Remove the model data at 'model_index'.
for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
- sizeof(struct OverTempCalDataPt));
+ sizeof(struct OverTempModelThreeAxis));
}
over_temp_cal->num_model_pts--;
return true;
}
-bool jumpStartModelData(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos) {
+bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
@@ -1291,7 +1317,7 @@
over_temp_cal->sensor_intercept[j];
}
over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
+ over_temp_cal->model_data[i].offset_age_nanos = 0;
offset_temp_celsius += over_temp_cal->delta_temp_per_bin;
over_temp_cal->num_model_pts++;
@@ -1310,8 +1336,7 @@
}
void updateModel(const struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos, float *temp_sensitivity,
- float *sensor_intercept) {
+ float *temp_sensitivity, float *sensor_intercept) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
@@ -1328,8 +1353,7 @@
const size_t n = over_temp_cal->num_model_pts;
for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
- over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
- timestamp_nanos);
+ over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
sw += weight;
st += over_temp_cal->model_data[i].offset_temp_celsius * weight;
@@ -1343,13 +1367,11 @@
const float inv_sw = 1.0f / sw;
for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
- over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
- timestamp_nanos);
+ over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
const float t =
- over_temp_cal->model_data[i].offset_temp_celsius -
- st * inv_sw;
- stt += weight * t * t;
+ over_temp_cal->model_data[i].offset_temp_celsius - st * inv_sw;
+ stt += weight * t * t;
stsx += t * over_temp_cal->model_data[i].offset[0] * weight;
stsy += t * over_temp_cal->model_data[i].offset[1] * weight;
stsz += t * over_temp_cal->model_data[i].offset[2] * weight;
@@ -1440,13 +1462,11 @@
}
float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
- uint64_t offset_timestamp_nanos,
- uint64_t current_timestamp_nanos) {
+ uint64_t offset_age_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
- if (current_timestamp_nanos <=
- offset_timestamp_nanos +
- over_temp_cal->weighting_function[i].offset_age_nanos) {
+ if (offset_age_nanos <=
+ over_temp_cal->weighting_function[i].offset_age_nanos) {
return over_temp_cal->weighting_function[i].weight;
}
}
@@ -1455,6 +1475,21 @@
return OTC_MIN_WEIGHT_VALUE;
}
+void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ uint64_t age_increment_nanos =
+ timestamp_nanos - over_temp_cal->last_age_update_nanos;
+
+ // Resets the age update counter.
+ over_temp_cal->last_age_update_nanos = timestamp_nanos;
+
+ // Updates the model dataset ages.
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+ over_temp_cal->model_data[i].offset_age_nanos += age_increment_nanos;
+ }
+}
+
/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
#ifdef OVERTEMPCAL_DBG_ENABLED
@@ -1468,7 +1503,7 @@
new_debug_tag, strlen(new_debug_tag) + 1);
}
-void updateDebugData(struct OverTempCal* over_temp_cal) {
+void updateDebugData(struct OverTempCal *over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
// Only update this data if debug printing is not currently in progress
@@ -1502,13 +1537,13 @@
// If 'latest_offset' is defined the copy the data for debug printing.
// Otherwise, the current compensated offset will be printed.
- if (over_temp_cal->latest_offset) {
+ if (over_temp_cal->latest_offset != NULL) {
memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
- over_temp_cal->latest_offset, sizeof(struct OverTempCalDataPt));
+ over_temp_cal->latest_offset, sizeof(struct OverTempModelThreeAxis));
} else {
memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
&over_temp_cal->compensated_offset,
- sizeof(struct OverTempCalDataPt));
+ sizeof(struct OverTempModelThreeAxis));
}
// Total number of OTC model data points.
@@ -1516,9 +1551,9 @@
// Computes the maximum error over all of the model data.
overTempGetModelError(over_temp_cal,
- over_temp_cal->debug_overtempcal.temp_sensitivity,
- over_temp_cal->debug_overtempcal.sensor_intercept,
- over_temp_cal->debug_overtempcal.max_error);
+ over_temp_cal->debug_overtempcal.temp_sensitivity,
+ over_temp_cal->debug_overtempcal.sensor_intercept,
+ over_temp_cal->debug_overtempcal.max_error);
}
void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
@@ -1554,7 +1589,7 @@
// Prints out the latest offset estimate (input data).
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "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,
@@ -1574,34 +1609,35 @@
.offset_temp_celsius,
3),
(unsigned long long int)
- over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);
+ over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos);
+ // clang-format off
over_temp_cal->wait_timer_nanos =
timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state =
OTC_PRINT_MODEL_PARAMETERS; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
+ // clang-format on
break;
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,
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
- over_temp_cal->otc_unit_conversion,
- 3));
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
+ "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ over_temp_cal->otc_unit_tag,
+ (unsigned long int)over_temp_cal->debug_num_estimates,
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+ over_temp_cal->otc_unit_conversion,
+ 3));
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
"Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
@@ -1621,8 +1657,9 @@
3));
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
- over_temp_cal->next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state.
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state =
+ OTC_PRINT_MODEL_ERROR; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1648,7 +1685,7 @@
over_temp_cal->model_counter = 0; // Resets the model data print counter.
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
+ timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state = OTC_PRINT_MODEL_DATA; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1683,22 +1720,22 @@
3),
(unsigned long long int)over_temp_cal
->model_data[over_temp_cal->model_counter]
- .timestamp_nanos);
+ .offset_age_nanos);
over_temp_cal->model_counter++;
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
+ timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state =
- OTC_PRINT_MODEL_DATA; // Sets the next state.
+ OTC_PRINT_MODEL_DATA; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait state.
+ OTC_WAIT_STATE; // First, go to wait state.
} else {
// Sends this state machine to its idle state.
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
- over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait state.
+ OTC_WAIT_STATE; // First, go to wait state.
}
break;
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 8a404d3..4cfbfc1 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -100,6 +100,9 @@
#include <stddef.h>
#include <stdint.h>
+#include "calibration/over_temp/over_temp_model.h"
+#include "common/math/macros.h"
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -129,6 +132,9 @@
// function.
#define OTC_NUM_WEIGHT_LEVELS (2)
+// The time interval used to update the model data age.
+#define OTC_MODEL_AGE_UPDATE_NANOS (MIN_TO_NANOS(1))
+
// Rate-limits the check of old data to every 2 hours.
#define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
@@ -143,7 +149,7 @@
#define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
// Defines a weighting function value for the linear model fit routine.
-struct OverTempCalWeightPt {
+struct OverTempCalWeight {
// The age limit below which an offset will use this weight value.
uint64_t offset_age_nanos;
@@ -151,14 +157,6 @@
float weight;
};
-// Over-temperature sensor offset estimate structure.
-struct OverTempCalDataPt {
- // Sensor offset estimate, temperature, and timestamp.
- uint64_t timestamp_nanos; // [nanoseconds]
- float offset_temp_celsius; // [Celsius]
- float offset[3];
-};
-
#ifdef OVERTEMPCAL_DBG_ENABLED
// Debug printout state enumeration.
enum OverTempCalDebugState {
@@ -173,7 +171,7 @@
// OverTempCal debug information/data tracking structure.
struct DebugOverTempCal {
// The latest received offset estimate data.
- struct OverTempCalDataPt latest_offset;
+ struct OverTempModelThreeAxis latest_offset;
// The maximum model error over all model_data points.
float max_error[3];
@@ -184,38 +182,55 @@
};
#endif // OVERTEMPCAL_DBG_ENABLED
+// OverTempCal algorithm parameters (see OverTempCal for details).
+struct OverTempCalParameters {
+ uint64_t min_temp_update_period_nanos;
+ uint64_t age_limit_nanos;
+ float delta_temp_per_bin; // [Celsius/bin]
+ float jump_tolerance; // [sensor units]
+ float outlier_limit; // [sensor units/Celsius]
+ float temp_sensitivity_limit; // [sensor units/Celsius]
+ float sensor_intercept_limit; // [sensor units]
+ float significant_offset_change; // [sensor units]
+ size_t min_num_model_pts;
+ bool over_temp_enable;
+};
+
// The following data structure contains all of the necessary components for
// modeling a sensor's temperature dependency and providing over-temperature
// offset corrections.
struct OverTempCal {
// Storage for over-temperature model data.
- struct OverTempCalDataPt model_data[OTC_MODEL_SIZE];
+ struct OverTempModelThreeAxis model_data[OTC_MODEL_SIZE];
// Implements a weighting function to emphasize fitting a linear model to
// younger offset estimates.
- struct OverTempCalWeightPt weighting_function[OTC_NUM_WEIGHT_LEVELS];
+ struct OverTempCalWeight weighting_function[OTC_NUM_WEIGHT_LEVELS];
// The active over-temperature compensated offset estimate data. Contains the
// current sensor temperature at which offset compensation is performed.
- struct OverTempCalDataPt compensated_offset;
+ struct OverTempModelThreeAxis compensated_offset;
// Timer used to limit the rate at which old estimates are removed from
// the 'model_data' collection.
- uint64_t stale_data_timer; // [nanoseconds]
+ uint64_t stale_data_timer_nanos;
// Duration beyond which data will be removed to avoid corrupting the model
// with drift-compromised data.
- uint64_t age_limit_nanos; // [nanoseconds]
+ uint64_t age_limit_nanos;
// Timestamp of the last OTC offset compensation update.
- uint64_t last_offset_update_nanos; // [nanoseconds]
+ uint64_t last_offset_update_nanos;
// Timestamp of the last OTC model update.
- uint64_t last_model_update_nanos; // [nanoseconds]
+ uint64_t last_model_update_nanos;
+
+ // Timestamp of the last OTC model dataset age update.
+ uint64_t last_age_update_nanos;
// Limit on the minimum interval for offset update calculations resulting from
// an arbitrarily high temperature sampling rate.
- uint64_t min_temp_update_period_nanos; // [nanoseconds]
+ uint64_t min_temp_update_period_nanos;
///// Online Model Identification Parameters ////////////////////////////////
//
@@ -229,17 +244,17 @@
// model_temp_span >= 'num_model_pts' * delta_temp_per_bin
// 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
- // a. ABS(temp_sensitivity) < temp_sensitivity_limit
- // b. ABS(sensor_intercept) < sensor_intercept_limit
- float temp_sensitivity_limit; // [sensor units/Celsius]
- float sensor_intercept_limit; // [sensor units]
+ // a. |temp_sensitivity| < temp_sensitivity_limit
+ // b. |sensor_intercept| < sensor_intercept_limit
+ float temp_sensitivity_limit; // [sensor units/Celsius]
+ float sensor_intercept_limit; // [sensor units]
size_t min_num_model_pts;
// Pointer to the offset estimate closest to the current sensor temperature.
- struct OverTempCalDataPt *nearest_offset;
+ struct OverTempModelThreeAxis *nearest_offset;
// Pointer to the most recent offset estimate.
- struct OverTempCalDataPt *latest_offset;
+ struct OverTempModelThreeAxis *latest_offset;
// Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius].
float temp_sensitivity[3];
@@ -251,15 +266,15 @@
// above which the model fit is preferred for providing offset compensation
// (also applies to checks between the nearest-temperature and the current
// compensated estimate).
- float jump_tolerance; // [sensor units]
+ float jump_tolerance; // [sensor units]
// A limit used to reject new offset estimates that deviate from the current
// model fit.
- float outlier_limit; // [sensor units]
+ float outlier_limit; // [sensor units]
// This parameter is used to detect offset changes that require updates to
// system calibration and persistent memory storage.
- float significant_offset_change; // [sensor units]
+ float significant_offset_change; // [sensor units]
// Used to track the previous significant change in temperature.
float last_temp_check_celsius;
@@ -282,7 +297,7 @@
// recent estimates in cases where they arrive frequently near a given
// temperature, and prevents model oversampling (i.e., dominance of estimates
// concentrated at a given set of temperatures).
- float delta_temp_per_bin; // [Celsius/bin]
+ float delta_temp_per_bin; // [Celsius/bin]
// Total number of model data points collected.
size_t num_model_pts;
@@ -299,8 +314,7 @@
// overTempCalNewModelUpdateAvailable() is called. This variable indicates
// that the following should be stored in persistent system memory:
// 1) 'temp_sensitivity' and 'sensor_intercept'.
- // 2) The 'compensated_offset' offset data
- // (saving timestamp information is not required).
+ // 2) The 'compensated_offset' offset data.
bool new_overtemp_model_available;
// True when a new offset estimate has been computed and registers as a
@@ -320,15 +334,15 @@
uint64_t temperature_print_timer;
#endif // OVERTEMPCAL_DBG_LOG_TEMP
- size_t model_counter; // Model output print counter.
- float otc_unit_conversion; // Unit conversion for debug display.
- char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS").
- char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO").
- char otc_debug_tag[32]; // Temporary string descriptor.
- size_t debug_num_model_updates; // Total number of model updates.
- size_t debug_num_estimates; // Total number of offset estimates.
- bool debug_print_trigger; // Flag used to trigger data printout.
-#endif // OVERTEMPCAL_DBG_ENABLED
+ size_t model_counter; // Model output print counter.
+ float otc_unit_conversion; // Unit conversion for debug display.
+ char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS").
+ char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO").
+ char otc_debug_tag[32]; // Temporary string descriptor.
+ size_t debug_num_model_updates; // Total number of model updates.
+ size_t debug_num_estimates; // Total number of offset estimates.
+ bool debug_print_trigger; // Flag used to trigger data printout.
+#endif // OVERTEMPCAL_DBG_ENABLED
};
/////// FUNCTION PROTOTYPES ///////////////////////////////////////////////////
@@ -338,6 +352,9 @@
*
* INPUTS:
* over_temp_cal: Over-temp main data structure.
+ * parameters: An algorithm parameters that contains the
+ * following initialization variables.
+ * [parameters]:
* min_num_model_pts: Minimum number of model points per model
* calculation update.
* min_temp_update_period_nanos: Limits the rate of offset updates due to an
@@ -351,19 +368,14 @@
* temp_sensitivity_limit: Values that define the upper limits for the
* sensor_intercept_limit: model parameters. The acceptance of new model
* parameters must satisfy:
- * i. ABS(temp_sensitivity) < temp_sensitivity_limit
- * ii. ABS(sensor_intercept) < sensor_intercept_limit
+ * i. |temp_sensitivity| < temp_sensitivity_limit
+ * ii. |sensor_intercept| < sensor_intercept_limit
* significant_offset_change Minimum limit that triggers offset updates.
* over_temp_enable: Flag that determines whether over-temp sensor
* offset compensation is applied.
*/
void overTempCalInit(struct OverTempCal *over_temp_cal,
- size_t min_num_model_pts,
- uint64_t min_temp_update_period_nanos,
- float delta_temp_per_bin, float jump_tolerance,
- float outlier_limit, uint64_t age_limit_nanos,
- float temp_sensitivity_limit, float sensor_intercept_limit,
- float significant_offset_change, bool over_temp_enable);
+ const struct OverTempCalParameters *parameters);
/*
* Sets the over-temp calibration model parameters.
@@ -417,7 +429,7 @@
*/
void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
size_t data_length, uint64_t timestamp_nanos,
- const struct OverTempCalDataPt *model_data);
+ const struct OverTempModelThreeAxis *model_data);
/*
* Gets the over-temp compensation model data set.
@@ -432,7 +444,7 @@
*/
void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
size_t *data_length,
- struct OverTempCalDataPt *model_data);
+ struct OverTempModelThreeAxis *model_data);
/*
* Gets the current over-temp compensated offset estimate data.
@@ -472,8 +484,8 @@
bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal);
/*
- * Updates the sensor's offset estimate and conditionally assimilates it into
- * the over-temp model data set, 'model_data'.
+ * Updates the sensor's offset estimate and conditionally incorporates it into
+ * the over-temp model data set, 'model_data'. Updates the model dataset age.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
@@ -491,7 +503,9 @@
// Updates the temperature at which the offset compensation is performed (i.e.,
// the current measured temperature value). This function is provided mainly for
// flexibility since temperature updates may come in from a source other than
-// the sensor itself, and at a different rate.
+// the sensor itself, and at a different rate. Since this function is
+// periodically called, it is also used to update the age of the model
+// estimates.
void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos,
float temperature_celsius);
@@ -527,7 +541,9 @@
* age is less than 'offset_age_nanos'. NOTE: The ordering of the
* 'offset_age_nanos' values in the weight function array should be
* monotonically increasing from lowest index to highest so that weighting
- * selection can be conveniently evaluated.
+ * selection can be conveniently evaluated. A simple compliance check is
+ * applied, and 'true' is returned when the input weight is positive and older
+ * than the 'index-1' weight's age.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
@@ -536,9 +552,9 @@
* value and corresponding age limit below which an offset
* will use the weight.
*/
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
struct OverTempCal *over_temp_cal, size_t index,
- const struct OverTempCalWeightPt *new_otc_weight);
+ const struct OverTempCalWeight *new_otc_weight);
#ifdef OVERTEMPCAL_DBG_ENABLED
// This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_model.h b/firmware/os/algos/calibration/over_temp/over_temp_model.h
new file mode 100644
index 0000000..18e2bef
--- /dev/null
+++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h
@@ -0,0 +1,38 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * Over-temperature data structures that contain a modeled sensor offset
+ * estimate, an associated temperature, and the age of the data point since it
+ * first entered the model.
+ */
+
+struct OverTempModelThreeAxis {
+ // Sensor offset estimate, temperature, and age timestamp.
+ uint64_t offset_age_nanos; // [nanoseconds]
+ float offset_temp_celsius; // [Celsius]
+
+ // Three-axis offset estimate (indices: 0=x, 1=y, 2=z).
+ float offset[3];
+};
+
+struct OverTempModelSingleAxis {
+ // Sensor offset estimate, temperature, and age timestamp.
+ uint64_t offset_age_nanos; // [nanoseconds]
+ float offset_temp_celsius; // [Celsius]
+
+ // Single-axis offset estimate.
+ float offset;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // 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
new file mode 100644
index 0000000..a501331
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
@@ -0,0 +1,97 @@
+/*
+ * 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/sample_rate_estimator/sample_rate_estimator.h"
+
+#include <string.h>
+
+#include "common/math/macros.h"
+#include "util/nano_assert.h"
+
+// Helper function used to reset the sampling rate estimator accumulator.
+static void sampleRateEstimatorResetAccumulator(
+ struct SampleRateEstimator* sample_rate_estimator) {
+ sample_rate_estimator->last_timestamp_nanos = 0.0f;
+ sample_rate_estimator->interval_accumulator_nanos = 0.0f;
+ sample_rate_estimator->num_intervals_collected = 0;
+}
+
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+ size_t num_intervals_to_collect,
+ float max_interval_sec) {
+ ASSERT_NOT_NULL(sample_rate_estimator);
+ memset(sample_rate_estimator, 0, sizeof(struct SampleRateEstimator));
+ sample_rate_estimator->mean_sampling_rate_estimate_hz =
+ SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ;
+ sample_rate_estimator->num_intervals_to_collect = num_intervals_to_collect;
+ sample_rate_estimator->max_interval_nanos =
+ max_interval_sec * SEC_TO_NANOS(1);
+}
+
+float sampleRateEstimatorGetHz(
+ struct SampleRateEstimator* sample_rate_estimator) {
+ sample_rate_estimator->new_sampling_rate_estimate_ready = false;
+ return sample_rate_estimator->mean_sampling_rate_estimate_hz;
+}
+
+void sampleRateEstimatorUpdate(
+ struct SampleRateEstimator* sample_rate_estimator,
+ uint64_t timestamp_nanos) {
+ // Resets the current interval capture and returns if:
+ // 1. A bad timestamp was received (i.e., time not monotonic).
+ // 2. 'last_timestamp_nanos' is zero. NOTE: 'last_timestamp_nanos' = 0
+ // indicates that the first complete time interval has not been captured
+ // yet (so, set it and return).
+ if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+ sample_rate_estimator->last_timestamp_nanos == 0) {
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+ return;
+ }
+
+ // Computes the current sampling interval. This conversion will be very fast
+ // for intervals less than ~4.3 seconds (i.e., 2^32 nano-seconds).
+ const float next_interval_nanos = floatFromUint64(
+ timestamp_nanos - sample_rate_estimator->last_timestamp_nanos);
+
+ // Helps prevent corruption of the estimator when there are gaps in the input
+ // sampling intervals greater than 'max_interval_nanos' (i.e., intermittant
+ // periods where there are no input timestamps).
+ if (next_interval_nanos >= sample_rate_estimator->max_interval_nanos) {
+ // Resets the estimator and returns.
+ sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+ return;
+ }
+
+ // Accumulates the next sampling interval.
+ sample_rate_estimator->interval_accumulator_nanos += next_interval_nanos;
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+ sample_rate_estimator->num_intervals_collected++;
+
+ // If the number of collected time intervals exceed the target number, then
+ // this computes a new sample rate estimate.
+ 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;
+
+ // Sets the polling flag to indicate that a new estimate is ready.
+ sample_rate_estimator->new_sampling_rate_estimate_ready = true;
+
+ // Resets the estimator variables.
+ sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+ }
+}
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
new file mode 100644
index 0000000..f506eea
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
@@ -0,0 +1,123 @@
+/*
+ * 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.
+ */
+
+/*
+ * [Sample Rate Estimator]
+ * This module periodically estimates the mean sampling rate of a uniformly
+ * sampled signal. Note, this estimator uses a floating point accumulator for
+ * the timing intervals. This trades-off some accumulation of finite precision
+ * error to enhance the range of estimated sampling rates and prevent overflow
+ * when an equivalent 32bit integer accumulator is used. In typical use cases
+ * (sample rates: 5Hz - 2kHz, num_intervals_to_collect 10 - 100), the sample
+ * rate accuracy is typically much better than 0.1Hz.
+ *
+ * FUNCTIONALITY:
+ * sampleRateEstimatorInit -- Initializes the estimator. Sets the number of time
+ * intervals require to compute the sample rate mean, and the upper limit for
+ * the acceptable time interval.
+ *
+ * new_sampling_rate_estimate_ready -- Check this boolean flag if polling for
+ * new estimates is desired.
+ *
+ * sampleRateEstimatorGetHz -- Returns the latest sample rate estimate and
+ * resets the polling flag.
+ *
+ * sampleRateEstimatorUpdate -- Processes new timestamp data and computes new
+ * sample rate estimates.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Designates the value used to indicate an invalid sample rate estimate.
+#define SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ (-1.0f)
+
+// Sample rate estimator data structure.
+struct SampleRateEstimator {
+ // Used to compute sample intervals to estimate the sampling rate.
+ uint64_t last_timestamp_nanos;
+
+ // Accumulator used for computing the mean sampling interval.
+ float interval_accumulator_nanos;
+
+ // The upper limit on the acceptance of valid time intervals (in nanoseconds).
+ // Larger time deltas result in a reset of the interval accumulator.
+ float max_interval_nanos;
+
+ // The most recent mean sampling rate estimate.
+ float mean_sampling_rate_estimate_hz;
+
+ /*
+ * The targeted number of time intervals required for a new sample rate mean
+ * calculation.
+ *
+ * NOTE: Assuming that the time interval noise is independent and identically
+ * distributed and drawn from a zero-mean normal distribution with variance
+ * 'Sigma^2'. The expected noise reduction is related to the choice of
+ * 'num_intervals_to_collect' as:
+ *
+ * Output RMS Noise = Sigma / sqrt(num_intervals_to_collect).
+ *
+ * Example, a value of 100 for a 100Hz signal would provide updates once every
+ * second and provide a 90% reduction (i.e., [1 - 1/sqrt(100)] * 100%) in time
+ * interval noise.
+ */
+ size_t num_intervals_to_collect;
+
+ // The number of time intervals currently collected.
+ size_t num_intervals_collected;
+
+ // Polling flag used to signal that a new estimate is ready. This flag is
+ // reset when 'sampleRateEstimatorGetHz' is called.
+ bool new_sampling_rate_estimate_ready;
+};
+
+// Initializes the sample rate estimator, sets the number of time intervals
+// for the mean sample rate calculation, and defines the tolerable gap in timing
+// data (in seconds).
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+ size_t num_intervals_to_collect,
+ float max_interval_sec);
+
+// Rather than using a function to poll for new sample rate estimates, which
+// would incur an additional function call, simply check
+// 'new_sampling_rate_estimate_ready' directly.
+
+// Returns the most recent sampling rate estimate, and resets the
+// 'new_sampling_rate_estimate_ready' polling flag. Note, if polling is not
+// used, one may access the sample rate estimate directly from the struct and
+// avoid this function call.
+float sampleRateEstimatorGetHz(
+ struct SampleRateEstimator* sample_rate_estimator);
+
+// Takes in a new timestamp and updates the sampling rate estimator.
+void sampleRateEstimatorUpdate(
+ struct SampleRateEstimator* sample_rate_estimator,
+ uint64_t timestamp_nanos);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
diff --git a/firmware/os/algos/calibration/synchronizer/synchronizer.cc b/firmware/os/algos/calibration/synchronizer/synchronizer.cc
new file mode 100644
index 0000000..a485389
--- /dev/null
+++ b/firmware/os/algos/calibration/synchronizer/synchronizer.cc
@@ -0,0 +1,370 @@
+#include "calibration/synchronizer/synchronizer.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace synchronizer {
+namespace {
+
+// Alias to simplify the access to SensorIndex.
+using SensorIndex = online_calibration::SensorIndex;
+
+// Seeks through data_to_interpolate_to and data_to_interpolate queues so that
+// first two entries of data_to_interpolate have timestamps that straddle the
+// timestamp of the first entry of data_to_interpolate_to. Leading queue items
+// are popped and discarded to perform seeking.
+//
+// Assumes both data_to_interpolate_to and data_to_interpolate sorted by
+// strictly increasing timestamp.
+//
+// Returns True if seeking was achieved. Returns false if seeking could not
+// be achieved, such is if either pointer was null or they contain data
+// streams which cannot be made to straddle in time.
+
+bool SeekStraddle(
+ chre::ArrayQueue<online_calibration::SensorData,
+ Synchronizer::kInputBufferSize>* data_to_interpolate_to,
+ chre::ArrayQueue<online_calibration::SensorData,
+ Synchronizer::kInputBufferSize>* data_to_interpolate) {
+ // If either is a null pointer, return false
+ if (data_to_interpolate_to == nullptr || data_to_interpolate == nullptr) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Unexpected null pointer during seek!");
+#endif
+ return false;
+ }
+
+ // If data_to_interpolate_to and data_to_interpolate are the same, no seeking.
+ if (reinterpret_cast<void*>(data_to_interpolate_to) ==
+ reinterpret_cast<void*>(data_to_interpolate)) {
+ return true;
+ }
+
+ // Check that queues are not empty and time min/max aren't impossible.
+ if (data_to_interpolate_to->empty() || data_to_interpolate->empty() ||
+ data_to_interpolate->front().timestamp_nanos >
+ data_to_interpolate_to->back().timestamp_nanos ||
+ data_to_interpolate->back().timestamp_nanos <
+ data_to_interpolate_to->front().timestamp_nanos) {
+ return false;
+ }
+
+ // Seek in data_to_interpolate_to until first entry of data_to_interpolate
+ // precedes first of data_to_interpolate_to.
+ while (!data_to_interpolate_to->empty() &&
+ data_to_interpolate->front().timestamp_nanos >
+ data_to_interpolate_to->front().timestamp_nanos) {
+ data_to_interpolate_to->pop();
+ }
+
+ // Check that data_to_interpolate_to still has at least one element.
+ if (data_to_interpolate_to->empty()) {
+ return false;
+ }
+
+ // Seek in data_to_interpolate until 1st and 2nd entries straddle first
+ // data_to_interpolate_to entry.
+ while (data_to_interpolate->size() > 1 &&
+ !((*data_to_interpolate)[0].timestamp_nanos <=
+ data_to_interpolate_to->front().timestamp_nanos &&
+ (*data_to_interpolate)[1].timestamp_nanos >=
+ data_to_interpolate_to->front().timestamp_nanos)) {
+ data_to_interpolate->pop();
+ }
+
+ // data_to_interpolate must either match exactly, or have >=2 elements to
+ // interpolate
+ if (data_to_interpolate->front().timestamp_nanos ==
+ data_to_interpolate_to->front().timestamp_nanos) {
+ return true;
+ } else if (data_to_interpolate->size() < 2) {
+ return false;
+ }
+
+ // Check that data_to_interpolate still has at least two elements and they
+ // straddle the first entry of data_to_interpolate_to.
+ if ((*data_to_interpolate)[0].timestamp_nanos >
+ data_to_interpolate_to->front().timestamp_nanos ||
+ (*data_to_interpolate)[1].timestamp_nanos <
+ data_to_interpolate_to->front().timestamp_nanos) {
+ return false;
+ }
+
+ return true;
+}
+// Performs linear interpolation for a single axis.
+float SingleAxisLinearInterpolation(float tb_minus_ta, float tc_minus_ta,
+ float va, float vc) {
+ return va + (vc - va) * (tb_minus_ta / tc_minus_ta);
+}
+
+// Interpolates data_to_interpolate using timestamps in
+// data_to_interpolate_to, writing to interpolated the resulting 3-axis
+// measurement vector and timestamp that matches a timestamp in
+// data_to_interpolate_to. For this to be possible, data_to_interpolate
+// must have at least two timestamps which straddle a timestamp in
+// data_to_interpolate_to.
+//
+// The input queues are assumed to have strictly increasing timestamps.
+//
+// First, this uses SeekStraddle() to pop off entries of each queue until the
+// first two entries of data_to_interpolate have timestamps that straddle the
+// timestamp of the first entry of data_to_interpolate_to. Then linear
+// interpolation proceeds and results are written to interpolated.
+//
+// Returns true if interpolation was possible. Returns false if any of the
+// inputs are nullptr or if interpolation was not possible given the
+// timestamps in the data queues.
+bool SeekAndInterpolateLinear(
+ chre::ArrayQueue<online_calibration::SensorData,
+ Synchronizer::kInputBufferSize>* data_to_interpolate_to,
+ chre::ArrayQueue<online_calibration::SensorData,
+ Synchronizer::kInputBufferSize>* data_to_interpolate,
+ online_calibration::SensorData* interpolated) {
+ // Seek until first two entries of data_to_interpolate straddle first entry of
+ // data_to_interpolate_to. Seek function also checks that
+ // data_to_interpolate_to and data_to_interpolate are not nullptr.
+ if (!SeekStraddle(data_to_interpolate_to, data_to_interpolate) ||
+ interpolated == nullptr) {
+ return false;
+ }
+
+ // Use some local variables to avoid repeated pointer dereferencing.
+ // data_to_interpolate_to and data_to_interpolate queues guaranteed by seek
+ // function to be non-empty.
+ online_calibration::SensorData data_to_interpolate_to_0 =
+ data_to_interpolate_to->front();
+ const online_calibration::SensorData data_to_interpolate_0 =
+ data_to_interpolate->front();
+ online_calibration::SensorData data_to_interpolate_1;
+
+ // Avoid floating point math if possible: check if timestamps match exactly.
+ if (data_to_interpolate_to_0.timestamp_nanos ==
+ data_to_interpolate_0.timestamp_nanos) {
+ *interpolated = data_to_interpolate_0;
+ return true;
+ } else if (data_to_interpolate->size() > 1) {
+ data_to_interpolate_1 = (*data_to_interpolate)[1];
+ if (data_to_interpolate_to_0.timestamp_nanos ==
+ data_to_interpolate_1.timestamp_nanos) {
+ *interpolated = data_to_interpolate_1;
+ return true;
+ }
+ } else {
+// Size was 1 and first entries didn't match, so interpolation impossible!
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Unexpected queue state while attempting interpolation!");
+#endif
+ return false;
+ }
+
+ // Linearly interpolate data_to_interpolate at data_to_interpolate_to's 0th
+ // entry timepoint. Interpolate vector vb between va, vc at time tb where
+ // ta < tb < tc.
+ const float tb_minus_ta =
+ static_cast<float>(data_to_interpolate_to_0.timestamp_nanos -
+ data_to_interpolate_0.timestamp_nanos);
+ const float tc_minus_ta =
+ static_cast<float>(data_to_interpolate_1.timestamp_nanos -
+ data_to_interpolate_0.timestamp_nanos);
+
+ if (data_to_interpolate_0.type ==
+ online_calibration::SensorType::kAccelerometerMps2 ||
+ data_to_interpolate_0.type ==
+ online_calibration::SensorType::kGyroscopeRps ||
+ data_to_interpolate_0.type ==
+ online_calibration::SensorType::kMagnetometerUt) {
+ // Perform linear interpolation for 3-axis sensor and populate the result
+ // struct.
+ interpolated->data[SensorIndex::kXAxis] = SingleAxisLinearInterpolation(
+ tb_minus_ta, tc_minus_ta,
+ data_to_interpolate_0.data[SensorIndex::kXAxis],
+ data_to_interpolate_1.data[SensorIndex::kXAxis]);
+ interpolated->data[SensorIndex::kYAxis] = SingleAxisLinearInterpolation(
+ tb_minus_ta, tc_minus_ta,
+ data_to_interpolate_0.data[SensorIndex::kYAxis],
+ data_to_interpolate_1.data[SensorIndex::kYAxis]);
+ interpolated->data[SensorIndex::kZAxis] = SingleAxisLinearInterpolation(
+ tb_minus_ta, tc_minus_ta,
+ data_to_interpolate_0.data[SensorIndex::kZAxis],
+ data_to_interpolate_1.data[SensorIndex::kZAxis]);
+ interpolated->timestamp_nanos = data_to_interpolate_to_0.timestamp_nanos;
+ interpolated->type = data_to_interpolate_0.type;
+ return true;
+ } else if (data_to_interpolate_0.type ==
+ online_calibration::SensorType::kTemperatureCelsius ||
+ data_to_interpolate_0.type ==
+ online_calibration::SensorType::kBarometerHpa) {
+ // Perform linear interpolation for 1-axis sensor and populate the result
+ // struct.
+ interpolated->data[SensorIndex::kSingleAxis] =
+ SingleAxisLinearInterpolation(
+ tb_minus_ta, tc_minus_ta,
+ data_to_interpolate_0.data[SensorIndex::kSingleAxis],
+ data_to_interpolate_1.data[SensorIndex::kSingleAxis]);
+ interpolated->timestamp_nanos = data_to_interpolate_to_0.timestamp_nanos;
+ interpolated->type = data_to_interpolate_0.type;
+ return true;
+ } else {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Invalid sensor_type in SeekAndInterpolateLinear");
+#endif
+ return false;
+ }
+}
+
+// Attempts to add a SensorData measurement to a deque of them, returning
+// whether the new measurement was strictly greater in time than the last
+// entry in the queue. If it was not, measurement is not added and false is
+// returned. This function is primarily for code compactness: several queues
+// of measurements are appended-to in this same manner.
+bool AddMeasurementToDeque(
+ const online_calibration::SensorData& measurement,
+ chre::ArrayQueue<online_calibration::SensorData,
+ Synchronizer::kInputBufferSize>* mutable_deque) {
+ // Push measurement onto the measurement queue if sequential.
+ if (mutable_deque->empty() ||
+ (!mutable_deque->empty() &&
+ measurement.timestamp_nanos > mutable_deque->back().timestamp_nanos)) {
+ mutable_deque->push(measurement);
+ return true;
+ } else {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG(
+ "[SYNCHRONIZER WARNING]",
+ "measurement timestamp: %lu was not strictly greater than existing"
+ "queued timestamps.",
+ measurement.timestamp_nanos);
+#endif
+ return false;
+ }
+}
+} // namespace
+
+Synchronizer::Synchronizer()
+ : Synchronizer(online_calibration::SensorType::kGyroscopeRps,
+ online_calibration::SensorType::kAccelerometerMps2) {}
+
+Synchronizer::Synchronizer(
+ const online_calibration::SensorType& sensor_to_interpolate_to,
+ const online_calibration::SensorType& sensor_to_interpolate) {
+ if (sensor_to_interpolate_to == sensor_to_interpolate) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Sensor_types are equal! Using default settings!");
+#endif
+ sensor_to_interpolate_to_ = online_calibration::SensorType::kGyroscopeRps;
+ sensor_to_interpolate_ = online_calibration::SensorType::kAccelerometerMps2;
+ } else {
+ // Setting sensors.
+ sensor_to_interpolate_to_ = sensor_to_interpolate_to;
+ sensor_to_interpolate_ = sensor_to_interpolate;
+ }
+}
+
+bool Synchronizer::SetDataToInterpolateToSensorType(
+ const online_calibration::SensorType& sensor_type) {
+ if (sensor_type == sensor_to_interpolate_) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Sensor_type in SetDataToInterpolateToSensorType is "
+ "already used!");
+#endif
+ return false;
+ }
+ if (sensor_type == online_calibration::SensorType::kUndefined) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Invalid sensor_type in SetDataToInterpolateToSensorType");
+#endif
+ return false;
+ }
+ sensor_to_interpolate_to_ = sensor_type;
+ return true;
+}
+
+bool Synchronizer::SetDataToInterpolateSensorType(
+ const online_calibration::SensorType& sensor_type) {
+ if (sensor_type == sensor_to_interpolate_to_) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG(
+ "[SYNCHRONIZER WARNING]",
+ "Sensor_type in SetDataToInterpolateSensorType is already used!");
+#endif
+ return false;
+ }
+ if (sensor_type == online_calibration::SensorType::kUndefined) {
+#ifdef SYNCHRONIZER_DBG_ENABLED
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "Invalid sensor_type in SetDataToInterpolateSensorType");
+#endif
+ return false;
+ }
+ sensor_to_interpolate_ = sensor_type;
+ return true;
+}
+
+void Synchronizer::AddMeasurement(const online_calibration::SensorData& meas) {
+ online_calibration::SensorType sensor_type = meas.type;
+ if (sensor_type == sensor_to_interpolate_to_) {
+ if (input_sensor_to_interpolate_to_.full()) {
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "input_sensor_to_interpolate_to queue is full! "
+ "Removing front element from queue");
+ input_sensor_to_interpolate_to_.pop();
+ }
+ if (AddMeasurementToDeque(meas, &input_sensor_to_interpolate_to_)) {
+ // Add succeeded. Process applicable measurement queue.
+ ProcessMeasurementQueues();
+ }
+ }
+ if (sensor_type == sensor_to_interpolate_) {
+ if (input_sensor_to_interpolate_.full()) {
+ CAL_DEBUG_LOG("[SYNCHRONIZER WARNING]",
+ "input_sensor_to_interpolate queue is full !"
+ "Removing front element from queue");
+ input_sensor_to_interpolate_.pop();
+ }
+ if (AddMeasurementToDeque(meas, &input_sensor_to_interpolate_)) {
+ // Add succeeded. Process applicable measurement queue.
+ ProcessMeasurementQueues();
+ }
+ }
+}
+
+bool Synchronizer::ConsumeSynchronizedMeasurementQueues(
+ /*first sensor*/ online_calibration::SensorData* sensor_to_interpolate_to,
+ /*second sensor*/ online_calibration::SensorData* sensor_to_interpolate) {
+ if (!synchronized_first_and_second_queues_.empty()) {
+ *sensor_to_interpolate_to =
+ synchronized_first_and_second_queues_.front().first;
+ *sensor_to_interpolate =
+ synchronized_first_and_second_queues_.front().second;
+ synchronized_first_and_second_queues_.pop();
+ return true;
+ }
+ return false;
+}
+
+void Synchronizer::ProcessMeasurementQueues() {
+ // Results of interpolation written here.
+ online_calibration::SensorData interpolated;
+
+ // Process the queues, interpolating and queuing up measurements.
+ while (SeekAndInterpolateLinear(&input_sensor_to_interpolate_to_,
+ &input_sensor_to_interpolate_,
+ &interpolated)) {
+ synchronized_first_and_second_queues_.push(
+ std::pair<online_calibration::SensorData,
+ online_calibration::SensorData>(
+ input_sensor_to_interpolate_to_.front(), interpolated));
+
+ // Pop off data_to_interpolate_to measurement used to create this sample.
+ // data_to_interpolate will be cleaned up next seek.
+ input_sensor_to_interpolate_to_.pop();
+ }
+}
+
+} // namespace synchronizer
diff --git a/firmware/os/algos/calibration/synchronizer/synchronizer.h b/firmware/os/algos/calibration/synchronizer/synchronizer.h
new file mode 100644
index 0000000..1a0ef4c
--- /dev/null
+++ b/firmware/os/algos/calibration/synchronizer/synchronizer.h
@@ -0,0 +1,100 @@
+/*
+ * This is a sensor data synchronizer module that interpolates buffers (queues)
+ * of streamed measurements with respect to a user-selected input master stream
+ * to set the timebase. To perform synchronization, a sensor (sensor_data.h)
+ * is selected to define the resulting synchronized time axis. The second
+ * selected sensor is interpolated to the first sensor's timescale. This
+ * produces a synchronized queue of measurement pairs.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_
+
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "chre/util/array_queue.h"
+
+namespace synchronizer {
+
+/*
+ * Buffers measurements and synchronizes them using interpolation.
+ *
+ * Processes input streams of sensors and queues up output streams.
+ */
+class Synchronizer {
+ public:
+ // Input buffer size = 400 gives 1 second of buffer for a input sensor data
+ // rate of 400 Hz.
+ static constexpr int kInputBufferSize = 400;
+ // The output buffer size can store 10 sample, hist code has to ensure to
+ // grab the data before overflow.
+ static constexpr int kOutputBufferSize = 10;
+
+ Synchronizer();
+
+ Synchronizer(const online_calibration::SensorType& sensor_to_interpolate_to,
+ const online_calibration::SensorType& sensor_to_interpolate);
+
+ // Sets the sensor (first) to which the second sensor is interpolated to.
+ // Hence this sensor timebase is the master of the synchronized data stream.
+ // Notice that "sensor_to_interpolate_to_" cannot be equal
+ // "sensor_to_interpolate_", so this function cannot synchronize two accel's
+ // to each other.
+ // TODO(b/70861106): Change interface to a master slave approach.
+ // Sensors available (sensor_data.h).
+ bool SetDataToInterpolateToSensorType(
+ const online_calibration::SensorType& sensor_type);
+
+ // Sets the sensor (second) which will be interpolated. Notice that
+ // "sensor_to_interpolate_to_" cannot be equal "sensor_to_interpolate_".
+ // Sensors available (sensor_data.h).
+ bool SetDataToInterpolateSensorType(
+ const online_calibration::SensorType& sensor_type);
+
+ // Adds measurements. Uses the type in SensorData to identify
+ // sensor_to_interpolate and sensor_to_interpolate_to sensors.
+ void AddMeasurement(const online_calibration::SensorData& meas);
+
+ // Processes the synchronization queues in chronological order. Processing
+ // involves popping off each queue element and passing it to the specified
+ // function target of corresponding sensor type.
+ //
+ // Returns true if at least one element is in the sensor queue, and populates
+ // the synchronzied sensor data structs.
+ bool ConsumeSynchronizedMeasurementQueues(
+ /*sensor_to_interpolate_to*/
+ online_calibration::SensorData* sensor_to_interpolate_to,
+ /*sensor_to_interpolate*/
+ online_calibration::SensorData* sensor_to_interpolate);
+
+ private:
+ // Attempts to pair up "sensor_to_interpolate_to" (first) and
+ // "sensor_to_interpolate" (second) measurements buffered by the
+ // AddMeasurement() function, interpolating one using the other as a
+ // data_to_interpolate_to timescale if necessary.
+ //
+ // If interpolated measurements are generated, they are queued in this
+ // function.
+
+ void ProcessMeasurementQueues();
+
+ // Queue of synchronized measurements.
+ chre::ArrayQueue<
+ std::pair<online_calibration::SensorData, online_calibration::SensorData>,
+ kOutputBufferSize>
+ synchronized_first_and_second_queues_;
+
+ // Enums set which measurement defines the data_to_interpolate_to (first)
+ // timebase, against which sensor_to_interpolate is interpolated (second).
+ online_calibration::SensorType sensor_to_interpolate_to_;
+ online_calibration::SensorType sensor_to_interpolate_;
+
+ // Queues of time-ordered measurements for interpolation within this class.
+ chre::ArrayQueue<online_calibration::SensorData, kInputBufferSize>
+ input_sensor_to_interpolate_to_;
+ chre::ArrayQueue<online_calibration::SensorData, kInputBufferSize>
+ input_sensor_to_interpolate_;
+};
+
+} // namespace synchronizer
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SYNCHRONIZER_SYNCHRONIZER_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 1bd80fd..593c087 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -22,6 +22,7 @@
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
+// clang-format off
#ifdef GCC_DEBUG_LOG
# include <stdio.h>
# define CAL_DEBUG_LOG(tag, fmt, ...) \
@@ -40,6 +41,7 @@
# define CAL_DEBUG_LOG(tag, fmt, ...) \
chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
#endif // GCC_DEBUG_LOG
+// clang-format on
#ifdef __cplusplus
extern "C" {
@@ -50,9 +52,10 @@
#define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT
// 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))) * powf(10, num_digits)) // NOLINT
+#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.
diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c
new file mode 100644
index 0000000..a24a31b
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.c
@@ -0,0 +1,120 @@
+#include "common/math/kasa.h"
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "common/math/mat.h"
+
+void kasaReset(struct KasaFit *kasa) {
+ kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
+ kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
+ kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
+ kasa->acc_zz = kasa->acc_zw = 0.0f;
+ kasa->nsamples = 0;
+}
+
+void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
+
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
+ float w = x * x + y * y + z * z;
+
+ kasa->acc_x += x;
+ kasa->acc_y += y;
+ kasa->acc_z += z;
+ kasa->acc_w += w;
+
+ kasa->acc_xx += x * x;
+ kasa->acc_xy += x * y;
+ kasa->acc_xz += x * z;
+ kasa->acc_xw += x * w;
+
+ kasa->acc_yy += y * y;
+ kasa->acc_yz += y * z;
+ kasa->acc_yw += y * w;
+
+ kasa->acc_zz += z * z;
+ kasa->acc_zw += z * w;
+
+ kasa->nsamples += 1;
+}
+
+bool kasaNormalize(struct KasaFit *kasa) {
+ if (kasa->nsamples == 0) {
+ return false;
+ }
+
+ float inv = 1.0f / kasa->nsamples;
+
+ kasa->acc_x *= inv;
+ kasa->acc_y *= inv;
+ kasa->acc_z *= inv;
+ kasa->acc_w *= inv;
+
+ kasa->acc_xx *= inv;
+ kasa->acc_xy *= inv;
+ kasa->acc_xz *= inv;
+ kasa->acc_xw *= inv;
+
+ kasa->acc_yy *= inv;
+ kasa->acc_yz *= inv;
+ kasa->acc_yw *= inv;
+
+ kasa->acc_zz *= inv;
+ kasa->acc_zw *= inv;
+
+ return true;
+}
+
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+ float max_fit, float min_fit) {
+ // A * out = b
+ // (4 x 4) (4 x 1) (4 x 1)
+ struct Mat44 A;
+ A.elem[0][0] = kasa->acc_xx;
+ A.elem[0][1] = kasa->acc_xy;
+ A.elem[0][2] = kasa->acc_xz;
+ A.elem[0][3] = kasa->acc_x;
+ A.elem[1][0] = kasa->acc_xy;
+ A.elem[1][1] = kasa->acc_yy;
+ A.elem[1][2] = kasa->acc_yz;
+ A.elem[1][3] = kasa->acc_y;
+ A.elem[2][0] = kasa->acc_xz;
+ A.elem[2][1] = kasa->acc_yz;
+ A.elem[2][2] = kasa->acc_zz;
+ A.elem[2][3] = kasa->acc_z;
+ A.elem[3][0] = kasa->acc_x;
+ A.elem[3][1] = kasa->acc_y;
+ A.elem[3][2] = kasa->acc_z;
+ A.elem[3][3] = 1.0f;
+
+ struct Vec4 b;
+ initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
+
+ struct Size4 pivot;
+ mat44DecomposeLup(&A, &pivot);
+
+ struct Vec4 out;
+ mat44Solve(&A, &out, &b, &pivot);
+
+ // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
+ //
+ // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
+ // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
+
+ struct Vec3 v;
+ initVec3(&v, out.x, out.y, out.z);
+ vec3ScalarMul(&v, -0.5f);
+
+ float r_square = vec3Dot(&v, &v) - out.w;
+ float r = (r_square > 0) ? sqrtf(r_square) : 0;
+
+ initVec3(bias, v.x, v.y, v.z);
+ *radius = r;
+
+ int success = 0;
+ if (r > min_fit && r < max_fit) {
+ success = 1;
+ }
+
+ return success;
+}
diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h
new file mode 100644
index 0000000..e9652d6
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.h
@@ -0,0 +1,54 @@
+/*
+ * This module provides a data structure, initialization, and fit
+ * routine for algorithms that use the Kasa method for determining the
+ * 3-dimensional offset vector from a set of points on a sphere.
+ *
+ * Reference: I. Kåsa, "A circle fitting procedure and its error analysis," in
+ * IEEE Transactions on Instrumentation and Measurement, vol. IM-25, no. 1, pp.
+ * 8-14, March 1976.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+
+#include <stdbool.h>
+
+#include "common/math/vec.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct KasaFit {
+ float acc_x, acc_y, acc_z, acc_w;
+ float acc_xx, acc_xy, acc_xz, acc_xw;
+ float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
+ size_t nsamples;
+};
+
+// Resets the KasaFit data structure (sets all variables to zero).
+void kasaReset(struct KasaFit *kasa);
+
+// Initializes the KasaFit data structure.
+void kasaInit(struct KasaFit *kasa);
+
+// Accumulates the Kasa acc_** variables with the input vector [x, y, z], and
+// updates the number of samples.
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z);
+
+// Normalizes the Kasa acc_** variables. Returns 'false' if the number of
+// samples is zero, otherwise 'true'.
+bool kasaNormalize(struct KasaFit *kasa);
+
+// Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for
+// the best-fit sphere using the normal equations, and the sphere's 'radius'.
+// Returns '1' if the radius of the fit sphere is within the bounds
+// (min_fit, max_fit), otherwise '0'.
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+ float max_fit, float min_fit);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
index c01d2eb..d62308d 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.h
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -109,7 +109,7 @@
// Initializes LM solver with provided parameters and error function.
void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
- ResidualAndJacobianFunction error_func);
+ ResidualAndJacobianFunction func);
void lmSolverDestroy(struct LmSolver *solver);
@@ -133,7 +133,7 @@
*/
enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
void *f_data, size_t state_dim, size_t meas_dim,
- float *est_state);
+ float *state);
////////////////////////// TEST UTILITIES ////////////////////////////////////
// This function is exposed here for testing purposes only.
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index 5c06e24..1d785d1 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -16,6 +16,8 @@
// This file contains helper macros and definitions.
+#include <stdint.h>
+
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
@@ -26,18 +28,28 @@
#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+#define SIGMOID(x) (1 / (1 + expf(-x)))
// Timestamp conversion macros.
#ifdef __cplusplus
-#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000)))
#else
-#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000) // NOLINT
+#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000))) // NOLINT
#endif
-#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * 1000)
-#define MIN_TO_NANOS(x) SEC_TO_NANOS(x * 60)
-#define HRS_TO_NANOS(x) MIN_TO_NANOS(x * 60)
-#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * UINT64_C(1000))
+#define MIN_TO_NANOS(x) SEC_TO_NANOS (x * UINT64_C(60))
+#define HRS_TO_NANOS(x) MIN_TO_NANOS (x * UINT64_C(60))
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24))
+
+// Sample rate to period conversion.
+#ifdef __cplusplus
+#define HZ_TO_PERIOD_NANOS(hz) \
+ (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024)))
+#else
+#define HZ_TO_PERIOD_NANOS(hz) \
+ (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024))) // NOLINT
+#endif
// Unit conversion: nanoseconds to seconds.
#define NANOS_TO_SEC (1.0e-9f)
@@ -57,4 +69,26 @@
#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
(((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// This conversion function may be necessary for embedded hardware that can't
+// cast a uint64_t to a float directly. This conversion function was taken from:
+// /third_party/contexthub/firmware/core/floatRt.c
+static inline float floatFromUint64(uint64_t v) {
+ uint32_t hi = v >> 32;
+ uint32_t lo = (uint32_t) v;
+
+ if (!hi) { // This is very fast for cases where 'v' fits into a uint32_t.
+ return (float)lo;
+ } else {
+ return ((float)hi) * 4294967296.0f + (float)lo;
+ }
+}
+
+#ifdef __cplusplus
+}
+#endif
+
#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/util/array.h b/firmware/os/algos/util/array.h
new file mode 100644
index 0000000..9658be4
--- /dev/null
+++ b/firmware/os/algos/util/array.h
@@ -0,0 +1,6 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+
+#define ARRAY_SIZE(a) (sizeof((a)) / sizeof((a)[0]))
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
index da77749..cb3286e 100644
--- a/firmware/os/algos/util/nano_assert.h
+++ b/firmware/os/algos/util/nano_assert.h
@@ -1,7 +1,7 @@
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
-#if defined(__NANOHUB__) || defined(_OS_BUILD_)
+#ifndef GOOGLE3
// For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided
// by the build system, which has the directory stripped. But allow the
@@ -32,7 +32,7 @@
#include <assert.h>
#define ASSERT_IMPL(x) assert(x)
-#endif
+#endif // GOOGLE3
#ifndef ASSERT
#ifdef NANO_ASSERT_ENABLED
diff --git a/firmware/os/algos/util/nano_log.h b/firmware/os/algos/util/nano_log.h
new file mode 100644
index 0000000..8ec8043
--- /dev/null
+++ b/firmware/os/algos/util/nano_log.h
@@ -0,0 +1,13 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+
+#ifdef GOOGLE3
+#include <stdio.h>
+
+// ignore log level argument
+#define LOG_FUNC(level, ...) printf(__VA_ARGS__)
+#endif
+
+#include "third_party/contexthub/nanoapps/util/log/log.h"
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
