From bad8af6cd1fadca604ed8af6f41e1dae9972ee35 Mon Sep 17 00:00:00 2001 From: David Jacobs Date: Tue, 29 Nov 2016 11:40:32 -0800 Subject: Syncs google3 Calibration Code to Android. This CL syncs the recent calibration updates made in the google3 directory: /location/lbs/contexthub/nanoapps/calibration/ Test: - Firmware has been built and tested for the Nexus/Pixel devices. - CTS tests have been run (all passed) for PixelXL (OC-release). Tip of G3 CL: 141908053 Change-Id: Ib2f7802fa308e04ec603a942a3ac81e43a480e53 --- .../os/algos/calibration/accelerometer/accel_cal.c | 111 +- .../os/algos/calibration/accelerometer/accel_cal.h | 8 +- .../os/algos/calibration/common/calibration_data.c | 36 + .../os/algos/calibration/common/calibration_data.h | 72 ++ .../algos/calibration/common/diversity_checker.c | 165 +++ .../algos/calibration/common/diversity_checker.h | 133 +++ .../calibration/common/sphere_fit_calibration.c | 247 +++++ .../calibration/common/sphere_fit_calibration.h | 143 +++ firmware/os/algos/calibration/gyroscope/gyro_cal.c | 1168 +++++++++++++------- firmware/os/algos/calibration/gyroscope/gyro_cal.h | 119 +- .../calibration/gyroscope/gyro_stillness_detect.c | 2 +- .../calibration/gyroscope/gyro_stillness_detect.h | 5 +- .../os/algos/calibration/magnetometer/mag_cal.c | 180 +-- .../os/algos/calibration/magnetometer/mag_cal.h | 36 +- .../os/algos/calibration/over_temp/over_temp_cal.c | 940 ++++++++++++++++ .../os/algos/calibration/over_temp/over_temp_cal.h | 327 ++++++ firmware/os/algos/calibration/util/cal_log.h | 28 +- .../os/algos/common/math/levenberg_marquardt.c | 270 +++++ .../os/algos/common/math/levenberg_marquardt.h | 163 +++ firmware/os/algos/common/math/mat.c | 310 ++++-- firmware/os/algos/common/math/mat.h | 162 ++- firmware/os/algos/common/math/quat.h | 5 +- firmware/os/algos/common/math/vec.c | 93 +- firmware/os/algos/common/math/vec.h | 116 +- firmware/os/algos/util/nano_assert.h | 52 + 25 files changed, 4147 insertions(+), 744 deletions(-) create mode 100644 firmware/os/algos/calibration/common/calibration_data.c create mode 100644 firmware/os/algos/calibration/common/calibration_data.h create mode 100644 firmware/os/algos/calibration/common/diversity_checker.c create mode 100644 firmware/os/algos/calibration/common/diversity_checker.h create mode 100644 firmware/os/algos/calibration/common/sphere_fit_calibration.c create mode 100644 firmware/os/algos/calibration/common/sphere_fit_calibration.h create mode 100644 firmware/os/algos/calibration/over_temp/over_temp_cal.c create mode 100644 firmware/os/algos/calibration/over_temp/over_temp_cal.h create mode 100644 firmware/os/algos/common/math/levenberg_marquardt.c create mode 100644 firmware/os/algos/common/math/levenberg_marquardt.h create mode 100644 firmware/os/algos/util/nano_assert.h (limited to 'firmware/os/algos') diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index eea4493a..a789ce9d 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.c +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c @@ -156,12 +156,7 @@ static void accelCalAlgoInit(struct AccelCalAlgo *acc, uint32_t fx, 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); - - initMagCal(&acc->amoc, // mag_cal_t struct need for accel cal - 0.0f, 0.0f, 0.0f, // bias x, y, z - 1.0f, 0.0f, 0.0f, // c00, c01, c02 - 0.0f, 1.0f, 0.0f, // c10, c11, c12 - 0.0f, 0.0f, 1.0f); // c20, c21, c22 + initKasa(&acc->akf); } // Accel cal init. @@ -265,28 +260,28 @@ static int accelStillnessDetection(struct AccelStillDet *asd, } // Accumulate data for KASA fit. -static void accelCalUpdate(struct MagCal *amoc, struct AccelStillDet *asd) { +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; - amoc->acc_x += asd->mean_x; - amoc->acc_y += asd->mean_y; - amoc->acc_z += asd->mean_z; - amoc->acc_w += w; + akf->acc_x += asd->mean_x; + akf->acc_y += asd->mean_y; + akf->acc_z += asd->mean_z; + akf->acc_w += w; - amoc->acc_xx += asd->mean_x * asd->mean_x; - amoc->acc_xy += asd->mean_x * asd->mean_y; - amoc->acc_xz += asd->mean_x * asd->mean_z; - amoc->acc_xw += asd->mean_x * 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; - amoc->acc_yy += asd->mean_y * asd->mean_y; - amoc->acc_yz += asd->mean_y * asd->mean_z; - amoc->acc_yw += asd->mean_y * 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; - amoc->acc_zz += asd->mean_z * asd->mean_z; - amoc->acc_zw += asd->mean_z * w; - amoc->nsamples += 1; + 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. @@ -301,42 +296,42 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, ac1->agd.nx += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // 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->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // 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->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // 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->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // 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->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // 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->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // The leftover bucket nle. if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y && @@ -345,39 +340,39 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, ac1->agd.nle += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->amoc, asd); + accelCalUpdate(&ac1->akf, asd); } // Checking if all buckets are full. if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb && ac1->agd.ny == ac1->agd.nfy && ac1->agd.nyb == ac1->agd.nfyb && ac1->agd.nz == ac1->agd.nfz && ac1->agd.nzb == ac1->agd.nfzb) { - // Check if amoc->nsamples is zero. - if (ac1->amoc.nsamples == 0) { + // Check if akf->nsamples is zero. + if (ac1->akf.nsamples == 0) { agdReset(&ac1->agd); - magCalReset(&ac1->amoc); + magKasaReset(&ac1->akf); complete = 0; return complete; } else { // Normalize the data to the sample numbers. - inv = 1.0f / ac1->amoc.nsamples; + inv = 1.0f / ac1->akf.nsamples; } - ac1->amoc.acc_x *= inv; - ac1->amoc.acc_y *= inv; - ac1->amoc.acc_z *= inv; - ac1->amoc.acc_w *= inv; + ac1->akf.acc_x *= inv; + ac1->akf.acc_y *= inv; + ac1->akf.acc_z *= inv; + ac1->akf.acc_w *= inv; - ac1->amoc.acc_xx *= inv; - ac1->amoc.acc_xy *= inv; - ac1->amoc.acc_xz *= inv; - ac1->amoc.acc_xw *= inv; + ac1->akf.acc_xx *= inv; + ac1->akf.acc_xy *= inv; + ac1->akf.acc_xz *= inv; + ac1->akf.acc_xw *= inv; - ac1->amoc.acc_yy *= inv; - ac1->amoc.acc_yz *= inv; - ac1->amoc.acc_yw *= inv; + ac1->akf.acc_yy *= inv; + ac1->akf.acc_yz *= inv; + ac1->akf.acc_yw *= inv; - ac1->amoc.acc_zz *= inv; - ac1->amoc.acc_zw *= inv; + ac1->akf.acc_zz *= inv; + ac1->akf.acc_zw *= inv; // Calculate the temp VAR and MEA.N ac1->agd.var_t = @@ -392,7 +387,7 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, 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); - magCalReset(&ac1->amoc); + magKasaReset(&ac1->akf); complete = 0; return complete; } @@ -400,15 +395,15 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, } // Eigen value magnitude and ratio test. -static int mocEigenTest(struct MagCal *moc, struct AccelGoodData *agd) { +static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) { // covariance matrix. struct Mat33 S; - S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x; - S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y; - S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z; - S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y; - S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z; - S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z; + S.elem[0][0] = akf->acc_xx - akf->acc_x * akf->acc_x; + S.elem[0][1] = S.elem[1][0] = akf->acc_xy - akf->acc_x * akf->acc_y; + S.elem[0][2] = S.elem[2][0] = akf->acc_xz - akf->acc_x * akf->acc_z; + S.elem[1][1] = akf->acc_yy - akf->acc_y * akf->acc_y; + S.elem[1][2] = S.elem[2][1] = akf->acc_yz - akf->acc_y * akf->acc_z; + S.elem[2][2] = akf->acc_zz - akf->acc_z * akf->acc_z; struct Vec3 eigenvals; struct Mat33 eigenvecs; @@ -502,13 +497,13 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x, float radius; // Grabbing the fit from the MAG cal. - magCalFit(&acc->ac1[temp_gate].amoc, &bias, &radius); + magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius); // If offset is too large don't take. if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF && fabsf(bias.z) < MAX_OFF) { // Eigen Ratio Test. - if (mocEigenTest(&acc->ac1[temp_gate].amoc, + if (accEigenTest(&acc->ac1[temp_gate].akf, &acc->ac1[temp_gate].agd)) { // Storing the new offsets. acc->x_bias_new = bias.x * KSCALE2; @@ -545,7 +540,7 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x, // Resetting the structs for a new accel cal run. agdReset(&acc->ac1[temp_gate].agd); - magCalReset(&acc->ac1[temp_gate].amoc); + magKasaReset(&acc->ac1[temp_gate].akf); } } } @@ -730,7 +725,7 @@ void accelCalDebPrint(struct AccelCal *acc, float temp) { (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb, (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb, (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb, - (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].amoc.nsamples); + (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples); // Live bucket count hogher. CAL_DEBUG_LOG( "[BMI160]", @@ -739,7 +734,7 @@ void accelCalDebPrint(struct AccelCal *acc, float temp) { (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb, (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb, (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb, - (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].amoc.nsamples); + (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples); // Offset used. CAL_DEBUG_LOG( "[BMI160]", diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h index 83a45070..03c65e92 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.h +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -25,6 +22,8 @@ * the vectors should end onto a sphere. Furthermore the offset values are * subtracted from the accelerometer data calibrating the sensor. */ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ACCELEROMETER_ACCEL_CAL_H_ #include #include @@ -124,7 +123,8 @@ struct AccelStatsMem { // Struct for an accel calibration for a single temperature window. struct AccelCalAlgo { struct AccelGoodData agd; - struct MagCal amoc; + // TODO(mkramerm): Replace all abbreviations. + struct KasaFit akf; }; // Complete accel calibration struct. diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/common/calibration_data.c new file mode 100644 index 00000000..9ae72d27 --- /dev/null +++ b/firmware/os/algos/calibration/common/calibration_data.c @@ -0,0 +1,36 @@ +#include "calibration/common/calibration_data.h" + +#include + +#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 new file mode 100644 index 00000000..b0e6809b --- /dev/null +++ b/firmware/os/algos/calibration/common/calibration_data.h @@ -0,0 +1,72 @@ +/* + * 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 + +#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/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c new file mode 100644 index 00000000..8e5c3e28 --- /dev/null +++ b/firmware/os/algos/calibration/common/diversity_checker.c @@ -0,0 +1,165 @@ +/* + * Copyright (C) 2016 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/common/diversity_checker.h" + +#include +#include +#include +#include +#include "common/math/vec.h" + +// Struct initialization. +void diversityCheckerInit( + struct DiversityChecker* diverse_data, + float threshold, + float max_distance, + size_t min_num_diverse_vectors, + size_t max_num_max_distance, + float var_threshold, + float max_min_threshold) { + ASSERT_NOT_NULL(diverse_data); + // Initialize parameters. + diverse_data->threshold = threshold; + diverse_data->max_distance = max_distance; + diverse_data->min_num_diverse_vectors = min_num_diverse_vectors; + // checking for min_num_diverse_vectors = 0 + if (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; + // Setting the rest to zero. + diversityCheckerReset(diverse_data); +} + +// Reset +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)); + // Resetting counters and data full bit. + diverse_data->num_points = 0; + diverse_data->num_max_dist_violations = 0; + diverse_data->data_full = false; +} + +void diversityCheckerUpdate( + struct DiversityChecker* diverse_data, float x, float y, float z) { + ASSERT_NOT_NULL(diverse_data); + // Converting three single inputs to a vector. + const float vec[3] = {x, y, z}; + // Result vector for vector difference. + float vec_diff[3]; + // normSquared result (k) + float norm_squared_result; + + // If memory is full, no need to run through the data. + if (!diverse_data->data_full) { + size_t i; + // 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, + THREE_AXIS_DATA_DIM); + // k = |v|^2 + norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM); + // if k < Threshold then leave the function. + if (norm_squared_result < diverse_data->threshold) { + return; + } + // if k > max_distance, count and leave the function. + if (norm_squared_result > diverse_data->max_distance) { + diverse_data->num_max_dist_violations++; + return; + } + } + // If none of the above caused to leave the function, data is diverse. + // 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); + // Count new data point. + diverse_data->num_points++; + // Setting data_full to 1, if memory is full. + if (diverse_data->num_points == NUM_DIVERSE_VECTORS) { + diverse_data->data_full = true; + } + } +} + +bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, + 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) { + return false; + } + float vec_bias[3] = {x_bias, y_bias, z_bias}; + float vec_bias_removed[3]; + float norm_results; + float acc_norm = 0.0f; + float acc_norm_square = 0.0f; + float max; + float min; + size_t i; + 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, + THREE_AXIS_DATA_DIM); + + // norm = ||v|| + norm_results = vecNorm(vec_bias_removed, THREE_AXIS_DATA_DIM); + + // Accumulate for mean and VAR. + acc_norm += norm_results; + acc_norm_square += norm_results * norm_results ; + + if (i == 0) { + min = norm_results; + max = norm_results; + } + // Finding min + if (norm_results < min) { + min = norm_results; + } + + // Finding max. + if (norm_results > max) { + max = norm_results; + } + // can leave the function if max-min is violated + // no need to continue. + if ((max - min) > diverse_data->max_min_threshold) { + return false; + } + } + + float inv = 1.0f / diverse_data->num_points; + float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv; + return (var < diverse_data->var_threshold); +} diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/common/diversity_checker.h new file mode 100644 index 00000000..30f53dba --- /dev/null +++ b/firmware/os/algos/calibration/common/diversity_checker.h @@ -0,0 +1,133 @@ +/* + * 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 function implements a diversity checker and stores diverse vectors into + * a memory. We assume that the data is located on a sphere, and we use the + * norm of the difference of two vectors to decide if the vectors are diverse + * enough: + * + * k = norm( v1 - v2 )^2 < Threshold + * + * Hence when k < Threshold the data is not stored, because the vectors are too + * similar. We store diverse vectors in memory and all new incoming vectors + * are checked against the already stored data points. + * + * Furthermore we also check if k > max_distance, since that data is most likely + * not located on a sphere anymore and indicates a disturbance. Finally we give + * a "data is full" flag to indicate once the memory is full. + * The diverse data can be used to improve sphere fit calibrations, ensuring + * that the sphere is populated enough resulting in better fits. + * + * Memory is stored in an array initialized to length of + * [THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS], this has been done to be + * compatible with the full sphere fit algorithm. + * + * Notice, this function stops to check if data is diverse, once the memory is + * 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_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define THREE_AXIS_DATA_DIM (3) // data is three-dimensional. +#define NUM_DIVERSE_VECTORS (10) // Storing 10 data points. + +// Main data struct. +struct DiversityChecker { + // Data memory. + float diverse_data[THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS]; + + // Number of data points in the memory. + size_t num_points; + + // Number of data points that violated the max_distance condition. + size_t num_max_dist_violations; + + // Threshold value that is used to check k against. + float threshold; + + // Maximum distance value. + float max_distance; + + // Data full bit. + bool data_full; + + // Setup variables for NormQuality check. + + size_t min_num_diverse_vectors; + size_t max_num_max_distance; + float var_threshold; + float max_min_threshold; +}; + +// Initialization of the function/struct, input: +// threshold -> sets the threshold value, only distances k that are equal +// or higher than that will be stored. +// max_distance -> sets the maximum allowed distance of k. +// 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 +// gate. +// var_threshold -> is a threshold value for a Norm variance gate. +// max_min_threshold -> is a value for a gate that rejects Norm variations +// that are larger than this number. +void diversityCheckerInit(struct DiversityChecker* diverse_data, + float threshold, + float max_distance, + size_t min_num_diverse_vectors, + size_t max_num_max_distance, + float var_threshold, + float max_min_threshold); + +// 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); + +// Main function. Tests the data (x,y,z) against the memory if diverse and +// stores it, if so. +void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, + float y, float z); + +// Removing a constant bias from the diverse_data and check if the norm is +// within a defined bound: +// implemented 4 gates +// -> needs a minimum number of data points in the memory +// (controlled by min_num_divers_vectors). +// -> will return false if maximum number of max_distance is reached +// (controlled by max_num_max_distance). +// -> norm must be within a var window. +// -> 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); + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_ diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/common/sphere_fit_calibration.c new file mode 100644 index 00000000..2c26af55 --- /dev/null +++ b/firmware/os/algos/calibration/common/sphere_fit_calibration.c @@ -0,0 +1,247 @@ +#include "calibration/common/sphere_fit_calibration.h" + +#include +#include +#include +#include + +#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-4) + +// 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 new file mode 100644 index 00000000..e49f225a --- /dev/null +++ b/firmware/os/algos/calibration/common/sphere_fit_calibration.h @@ -0,0 +1,143 @@ +/* + * 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 +#include + +#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/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c index 4e87dc9c..1c609551 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c @@ -15,44 +15,126 @@ */ #include "calibration/gyroscope/gyro_cal.h" + +#include +#include +#include + #include "calibration/util/cal_log.h" +#include "common/math/vec.h" /////// DEFINITIONS AND MACROS /////////////////////////////////////// // Maximum gyro bias correction (should be set based on expected max bias // of the given sensor). -#define MAX_GYRO_BIAS 0.096f // [rad/sec] +#define MAX_GYRO_BIAS (0.096f) // [rad/sec] + +// The time value used to throttle debug messaging. +#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000) + +// Converts units of radians to milli-degrees. +#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI) + +// Unit conversion: m/sec^2 to g's. +#define GRAVITY_TO_G (float)(1e3f * 180.0f / M_PI) -// Helper constants for converting units. -#define RAD_TO_MDEG (float)(1e3 * 180.0 / M_PI) -#define GRAVITY_TO_G (float)(1.0 / 9.80665) +// Unit conversion: nanoseconds to seconds. +#define NANOS_TO_SEC (1.0e-9f) /////// FORWARD DECLARATIONS ///////////////////////////////////////// static void deviceStillnessCheck(struct GyroCal* gyro_cal, - uint64_t sample_time); + uint64_t sample_time_nanos); -static void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time); +static void computeGyroCal(struct GyroCal* gyro_cal, + uint64_t calibration_time_nanos); -static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time); +static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos); #ifdef GYRO_CAL_DBG_ENABLED -static void gyroCalUpdateDebug(struct GyroCal* gyro_cal, - struct DebugGyroCal* debug_gyro_cal); +static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); -static void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal, - uint64_t sample_time); +/* + * Updates running calculation of the temperature statistics. + * + * Behavior: + * 1) If 'debug_temperature' pointer is not NULL then the local calculation + * of the temperature statistics are copied, and the function returns. + * 2) Else, if 'reset_stats' is 'true' then the local statistics are reset + * and the function returns. + * 3) Otherwise, the local temperature statistics are updated according to + * the input value 'temperature'. + * + * INPUTS: + * debug_temperature: Pointer to the temperature stats sturcture to update. + * temperature: Temperature value (Celsius). + * reset_stats: Flag that determines if the local running stats are reset. + */ +static void gyroTempUpdateStats(struct DebugTemperature* debug_temperature, + float temperature, bool reset_stats); -static void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data); -#endif +/* + * Updates 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: + * 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(float* debug_mean_sampling_rate_hz, + uint64_t timestamp_nanos, bool reset_stats); + +// Defines the type of debug data to print. +enum DebugPrintData { + BIAS_CAL = 0, + CAL_TIME, + ACCEL_STATS, + GYRO_STATS, + MAG_STATS, + TEMP_STATS, + STILLNESS_DATA, + SAMPLING_RATE +}; + +// Helper function for printing out common debug data. +static void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data, + 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; + } +} + +#ifdef GYRO_CAL_DBG_TUNE_ENABLED +// Prints debug information useful for tuning the GyroCal parameters. +static void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal, + uint64_t timestamp_nanos); +#endif // GYRO_CAL_DBG_TUNE_ENABLED +#endif // GYRO_CAL_DBG_ENABLED /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the gyro calibration data structure. -void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, - uint64_t max_still_duration, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time, - uint64_t window_time_duration, float gyro_var_threshold, +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, @@ -61,9 +143,9 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, memset(gyro_cal, 0, sizeof(struct GyroCal)); // Initialize the stillness detectors. - // Gyro parameter input units are [rad/sec] - // Accel parameter input units are [m/sec^2] - // Magnetometer parameter input units are [uT] + // 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, @@ -73,23 +155,24 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, // Reset stillness flag and start timestamp. gyro_cal->prev_still = false; - gyro_cal->start_still_time = 0; + gyro_cal->start_still_time_nanos = 0; // Set the min and max window stillness duration. - gyro_cal->min_still_duration = min_still_duration; - gyro_cal->max_still_duration = max_still_duration; + gyro_cal->min_still_duration_nanos = min_still_duration_nanos; + gyro_cal->max_still_duration_nanos = max_still_duration_nanos; // Sets the duration of the stillness processing windows. - gyro_cal->window_time_duration = window_time_duration; + gyro_cal->window_time_duration_nanos = window_time_duration_nanos; // Set the watchdog timeout duration. - gyro_cal->gyro_watchdog_timeout_duration = 2 * window_time_duration; + gyro_cal->gyro_watchdog_timeout_duration_nanos = + 2 * window_time_duration_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 = calibration_time; + gyro_cal->calibration_time_nanos = calibration_time_nanos; // Set the stillness threshold required for gyro bias calibration. gyro_cal->stillness_threshold = stillness_threshold; @@ -98,26 +181,36 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, // collection in sync. Setting this to zero signals that sensor data // will be dropped until a valid end time is set from the first gyro // timestamp received. - gyro_cal->stillness_win_endtime = 0; + gyro_cal->stillness_win_endtime_nanos = 0; // Gyro calibrations will be applied (see, gyroCalRemoveBias()). gyro_cal->gyro_calibration_enable = (remove_bias_enable > 0); #ifdef GYRO_CAL_DBG_ENABLED - CAL_DEBUG_LOG("[GYRO_CAL]", "Gyro Cal: Initialized."); - CAL_DEBUG_LOG("[GYRO_CAL]", - "GyroCalInit = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n", - CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6)); - - // Initialize the debug report state machine. - gyro_cal->gyro_debug_state = -1; -#endif + CAL_DEBUG_LOG("[GYRO_CAL:MEMORY]", "sizeof(struct GyroCal): %lu", + (unsigned long int)sizeof(struct GyroCal)); + + CAL_DEBUG_LOG("[GYRO_CAL:INIT]", + "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d", + CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); + + if (gyro_cal->gyro_calibration_enable) { + CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration ENABLED."); + } else { + CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED."); + } + + // Ensures that the running temperature statistics and gyro sampling rate + // estimate are reset. + gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true); + gyroSamplingRateUpdate(NULL, 0, /*reset_stats=*/true); +#endif // GYRO_CAL_DBG_ENABLED } -// Void all pointers in the gyro calibration data structure -// (prevents compiler warnings). +// Void all pointers in the gyro calibration data structure (doesn't do anything +// except prevent compiler warnings). void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; } // Get the most recent bias calibration value. @@ -127,23 +220,32 @@ void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, *bias_x = gyro_cal->bias_x; *bias_y = gyro_cal->bias_y; *bias_z = gyro_cal->bias_z; + +#ifdef GYRO_CAL_DBG_ENABLED + CAL_DEBUG_LOG( + "[GYRO_CAL:STORED]", + "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d", + CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); +#endif } } // 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) { + float bias_z, 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 = calibration_time; + gyro_cal->calibration_time_nanos = calibration_time_nanos; #ifdef GYRO_CAL_DBG_ENABLED - CAL_DEBUG_LOG("[GYRO_CAL]", - "GyroCalSetBias = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n", - CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6)); + CAL_DEBUG_LOG("[GYRO_CAL:RECALL]", + "Gyro Bias Calibration [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d", + CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); #endif } @@ -169,60 +271,74 @@ bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal) { } // Update the gyro calibration with gyro data [rad/sec]. -void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time, float x, - float y, float z, float temperature) { +void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, + float x, float y, float z, float temperature) { // Make sure that a valid window end time is set, // and start the watchdog timer. - if (gyro_cal->stillness_win_endtime <= 0) { - gyro_cal->stillness_win_endtime = - sample_time + gyro_cal->window_time_duration; + if (gyro_cal->stillness_win_endtime_nanos <= 0) { + gyro_cal->stillness_win_endtime_nanos = + sample_time_nanos + gyro_cal->window_time_duration_nanos; // Start the watchdog timer. - gyro_cal->gyro_watchdog_start = sample_time; + gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; } +#ifdef GYRO_CAL_DBG_ENABLED + // Update the temperature statistics (on temperature change only). + if (NANO_ABS(gyro_cal->latest_temperature_celcius - temperature) > FLT_MIN) { + gyroTempUpdateStats(NULL, temperature, /*reset_stats=*/false); + } + + // Update the gyro sampling rate estimate. + gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/false); +#endif // GYRO_CAL_DBG_ENABLED + // Record the latest temperture sample. gyro_cal->latest_temperature_celcius = temperature; // Pass gyro data to stillness detector gyroStillDetUpdate(&gyro_cal->gyro_stillness_detect, - gyro_cal->stillness_win_endtime, sample_time, x, y, z); + gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, + x, y, z); // Perform a device stillness check, set next window end time, and // possibly do a gyro bias calibration and stillness detector reset. - deviceStillnessCheck(gyro_cal, sample_time); + deviceStillnessCheck(gyro_cal, sample_time_nanos); } // Update the gyro calibration with mag data [micro Tesla]. -void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x, - float y, float z) { +void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, + float x, float y, float z) { // Pass magnetometer data to stillness detector. gyroStillDetUpdate(&gyro_cal->mag_stillness_detect, - gyro_cal->stillness_win_endtime, sample_time, x, y, z); + gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, + x, y, z); // Received a magnetometer sample; incorporate it into detection. gyro_cal->using_mag_sensor = true; // Perform a device stillness check, set next window end time, and // possibly do a gyro bias calibration and stillness detector reset. - deviceStillnessCheck(gyro_cal, sample_time); + deviceStillnessCheck(gyro_cal, sample_time_nanos); } // Update the gyro calibration with accel data [m/sec^2]. -void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, float x, - float y, float z) { +void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, + float x, float y, float z) { // Pass accelerometer data to stillnesss detector. gyroStillDetUpdate(&gyro_cal->accel_stillness_detect, - gyro_cal->stillness_win_endtime, sample_time, x, y, z); + gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, + x, y, z); // Perform a device stillness check, set next window end time, and // possibly do a gyro bias calibration and stillness detector reset. - deviceStillnessCheck(gyro_cal, sample_time); + deviceStillnessCheck(gyro_cal, sample_time_nanos); } // Checks the state of all stillness detectors to determine // whether the device is "still". -void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { +void deviceStillnessCheck(struct GyroCal* gyro_cal, + uint64_t sample_time_nanos) { bool stillness_duration_exceeded = false; bool stillness_duration_too_short = false; bool device_is_still = false; @@ -231,7 +347,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { float conf_still = 0; // Check the watchdog timer. - checkWatchdog(gyro_cal, sample_time); + checkWatchdog(gyro_cal, sample_time_nanos); // Is there enough data to do a stillness calculation? if ((!gyro_cal->mag_stillness_detect.stillness_window_ready && @@ -242,8 +358,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { } // Set the next window end time for the stillness detectors. - gyro_cal->stillness_win_endtime = - sample_time + gyro_cal->window_time_duration; + gyro_cal->stillness_win_endtime_nanos = + sample_time_nanos + gyro_cal->window_time_duration_nanos; // Update the confidence scores for all sensors. gyroStillDetCompute(&gyro_cal->accel_stillness_detect); @@ -274,15 +390,15 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { if (!gyro_cal->prev_still) { // Record the starting timestamp of the current stillness window. // This enables the calculation of total duration of the stillness period. - gyro_cal->start_still_time = + gyro_cal->start_still_time_nanos = gyro_cal->gyro_stillness_detect.window_start_time; } - // Check to see if current stillness period exceeds the desired limit - // to avoid corrupting the . + // Check to see if current stillness period exceeds the desired limit. stillness_duration_exceeded = ((gyro_cal->gyro_stillness_detect.last_sample_time - - gyro_cal->start_still_time) > gyro_cal->max_still_duration); + gyro_cal->start_still_time_nanos) > + gyro_cal->max_still_duration_nanos); if (stillness_duration_exceeded) { // The current stillness has gone too long. Do a calibration with the @@ -290,23 +406,32 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { // Update the gyro bias estimate with the current window data and // reset the stats. - gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, true); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); // Perform calibration. computeGyroCal(gyro_cal, gyro_cal->gyro_stillness_detect.last_sample_time); +#ifdef GYRO_CAL_DBG_ENABLED + // Reset the temperature statistics and sampling rate estimate. + gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true); + gyroSamplingRateUpdate(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 { // Continue collecting stillness data. // Reset stillness detectors, and extend stillness period. - gyroStillDetReset(&gyro_cal->accel_stillness_detect, false); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, false); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, false); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, + /*reset_stats=*/false); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, + /*reset_stats=*/false); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false); // Update stillness flag. gyro_cal->prev_still = true; @@ -318,7 +443,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { // "too short", then do a calibration with the data accumulated thus far. stillness_duration_too_short = ((gyro_cal->gyro_stillness_detect.window_start_time - - gyro_cal->start_still_time) < gyro_cal->min_still_duration); + gyro_cal->start_still_time_nanos) < + gyro_cal->min_still_duration_nanos); if (gyro_cal->prev_still && !stillness_duration_too_short) { computeGyroCal(gyro_cal, @@ -326,28 +452,29 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { } // Reset stillness detectors and the stats. - gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, true); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); + +#ifdef GYRO_CAL_DBG_ENABLED + // Reset the temperature statistics and sampling rate estimate. + gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true); + gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true); +#endif // GYRO_CAL_DBG_ENABLED // Update stillness flag. gyro_cal->prev_still = false; } // Reset the watchdog timer after we have processed data. - gyro_cal->gyro_watchdog_start = sample_time; - -#ifdef GYRO_CAL_DBG_ENABLED - // Debug information available. - gyro_cal->debug_processed_data_available = true; - gyro_cal->debug_processed_data_time = sample_time; -#endif + gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; } // Calculates a new gyro bias offset calibration value. -void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) { +void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { // Current calibration duration. - uint64_t cur_cal_dur = calibration_time - gyro_cal->start_still_time; + uint64_t cur_cal_dur_nanos = + calibration_time_nanos - gyro_cal->start_still_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 && @@ -356,6 +483,15 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) { 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)) { +#ifdef GYRO_CAL_DBG_ENABLED + CAL_DEBUG_LOG( + "[GYRO_CAL:WARNING]", + "Rejected Bias Update [mdps]: %s%d.%06d, %s%d.%06d, %s%d.%06d", + CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); +#endif // GYRO_CAL_DBG_ENABLED + // Outside of range. Ignore, reset, and continue. return; } @@ -372,10 +508,10 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) { gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Store calibration stillness duration. - gyro_cal->calibration_time_duration = cur_cal_dur; + gyro_cal->calibration_time_duration_nanos = cur_cal_dur_nanos; // Store calibration time stamp. - gyro_cal->calibration_time = calibration_time; + gyro_cal->calibration_time_nanos = calibration_time_nanos; // Set flag to indicate a new gyro calibration value is available. gyro_cal->new_gyro_cal_available = true; @@ -384,62 +520,42 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) { // Increment the total count of calibration updates. gyro_cal->debug_calibration_count++; - // Store the last 'DEBUG_GYRO_SHORTTERM_NUM_CAL' calibration debug data - // in a circular buffer, 'debug_cal_data[]'. 'debug_head' is the index - // of the last valid calibration. - gyro_cal->debug_head++; - if (gyro_cal->debug_head >= DEBUG_GYRO_SHORTTERM_NUM_CAL) { - gyro_cal->debug_head = 0; - } - if (gyro_cal->debug_num_cals < DEBUG_GYRO_SHORTTERM_NUM_CAL) { - gyro_cal->debug_num_cals++; - } - // Update the calibration debug information. - gyroCalUpdateDebug(gyro_cal, &gyro_cal->debug_cal_data[gyro_cal->debug_head]); - - // Improve the collection of historic calibrations. Limit frequency to - // every N hours. - if ((gyro_cal->debug_num_cals_hist <= 0) || - (calibration_time - - gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist] - .calibration_time) >= DEBUG_GYRO_CAL_LIMIT) { - gyro_cal->debug_head_hist++; - if (gyro_cal->debug_head_hist >= DEBUG_GYRO_LONGTERM_NUM_CAL) { - gyro_cal->debug_head_hist = 0; - } - if (gyro_cal->debug_num_cals_hist < DEBUG_GYRO_LONGTERM_NUM_CAL) { - gyro_cal->debug_num_cals_hist++; - } + gyroCalUpdateDebug(gyro_cal); - // Update the calibration debug information. - gyroCalUpdateDebug( - gyro_cal, &gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]); - } + // Trigger a printout of the debug information. + gyro_cal->debug_print_trigger = true; #endif } // Check for a watchdog timeout condition. -void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) { +void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { bool watchdog_timeout; // Check for initialization of the watchdog time (=0). - if (gyro_cal->gyro_watchdog_start <= 0) { + if (gyro_cal->gyro_watchdog_start_nanos <= 0) { return; } // Check for timeout condition of watchdog. - watchdog_timeout = ((sample_time - gyro_cal->gyro_watchdog_start) > - gyro_cal->gyro_watchdog_timeout_duration); + watchdog_timeout = + ((sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos) > + gyro_cal->gyro_watchdog_timeout_duration_nanos); // If a timeout occurred then reset to known good state. if (watchdog_timeout) { // Reset stillness detectors and restart data capture. - gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, true); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); gyro_cal->mag_stillness_detect.stillness_confidence = 0; - gyro_cal->stillness_win_endtime = 0; + gyro_cal->stillness_win_endtime_nanos = 0; + +#ifdef GYRO_CAL_DBG_ENABLED + // Reset the temperature statistics and sampling rate estimate. + gyroTempUpdateStats(NULL, 0, /*reset_stats=*/true); + gyroSamplingRateUpdate(NULL, sample_time_nanos, /*reset_stats=*/true); +#endif // GYRO_CAL_DBG_ENABLED // Force stillness confidence to zero. gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0; @@ -457,340 +573,596 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) { // Assert watchdog timeout flags. gyro_cal->gyro_watchdog_timeout |= watchdog_timeout; - gyro_cal->gyro_watchdog_start = 0; + gyro_cal->gyro_watchdog_start_nanos = 0; #ifdef GYRO_CAL_DBG_ENABLED gyro_cal->debug_watchdog_count++; + CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", "Total#, Timestamp [nsec]: %lu, %llu", + (unsigned long int)gyro_cal->debug_watchdog_count, + (unsigned long long int)sample_time_nanos); #endif } } #ifdef GYRO_CAL_DBG_ENABLED -// Update the calibration debug information. -void gyroCalUpdateDebug(struct GyroCal* gyro_cal, - struct DebugGyroCal* debug_gyro_cal) { +void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { // Probability of stillness (acc, rot, still), duration, timestamp. - debug_gyro_cal->accel_stillness_conf = + gyro_cal->debug_gyro_cal.accel_stillness_conf = gyro_cal->accel_stillness_detect.prev_stillness_confidence; - debug_gyro_cal->gyro_stillness_conf = + gyro_cal->debug_gyro_cal.gyro_stillness_conf = gyro_cal->gyro_stillness_detect.prev_stillness_confidence; - debug_gyro_cal->mag_stillness_conf = + gyro_cal->debug_gyro_cal.mag_stillness_conf = gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Magnetometer usage. - debug_gyro_cal->used_mag_sensor = gyro_cal->using_mag_sensor; + gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor; // Temperature at calibration time. - debug_gyro_cal->temperature_celcius = gyro_cal->latest_temperature_celcius; - - // Calibration time and stillness duration. - debug_gyro_cal->calibration_time_duration = - gyro_cal->calibration_time_duration; - debug_gyro_cal->calibration_time = gyro_cal->calibration_time; - - // Record the current calibration values - debug_gyro_cal->calibration[0] = gyro_cal->bias_x; - debug_gyro_cal->calibration[1] = gyro_cal->bias_y; - debug_gyro_cal->calibration[2] = gyro_cal->bias_z; - - // Record the previous window means - debug_gyro_cal->accel_mean[0] = gyro_cal->accel_stillness_detect.prev_mean_x; - debug_gyro_cal->accel_mean[1] = gyro_cal->accel_stillness_detect.prev_mean_y; - debug_gyro_cal->accel_mean[2] = gyro_cal->accel_stillness_detect.prev_mean_z; - - debug_gyro_cal->gyro_mean[0] = gyro_cal->gyro_stillness_detect.prev_mean_x; - debug_gyro_cal->gyro_mean[1] = gyro_cal->gyro_stillness_detect.prev_mean_y; - debug_gyro_cal->gyro_mean[2] = gyro_cal->gyro_stillness_detect.prev_mean_z; - - debug_gyro_cal->mag_mean[0] = gyro_cal->mag_stillness_detect.prev_mean_x; - debug_gyro_cal->mag_mean[1] = gyro_cal->mag_stillness_detect.prev_mean_y; - debug_gyro_cal->mag_mean[2] = gyro_cal->mag_stillness_detect.prev_mean_z; - - // Record the variance data. - debug_gyro_cal->accel_var[0] = gyro_cal->accel_stillness_detect.win_var_x; - debug_gyro_cal->accel_var[1] = gyro_cal->accel_stillness_detect.win_var_y; - debug_gyro_cal->accel_var[2] = gyro_cal->accel_stillness_detect.win_var_z; - - debug_gyro_cal->gyro_var[0] = gyro_cal->gyro_stillness_detect.win_var_x; - debug_gyro_cal->gyro_var[1] = gyro_cal->gyro_stillness_detect.win_var_y; - debug_gyro_cal->gyro_var[2] = gyro_cal->gyro_stillness_detect.win_var_z; - - debug_gyro_cal->mag_var[0] = gyro_cal->mag_stillness_detect.win_var_x; - debug_gyro_cal->mag_var[1] = gyro_cal->mag_stillness_detect.win_var_y; - debug_gyro_cal->mag_var[2] = gyro_cal->mag_stillness_detect.win_var_z; + gyro_cal->debug_gyro_cal.temperature_celcius = + gyro_cal->latest_temperature_celcius; + + // Stillness start, stop, and duration times. + gyro_cal->debug_gyro_cal.start_still_time_nanos = + gyro_cal->start_still_time_nanos; + gyro_cal->debug_gyro_cal.end_still_time_nanos = + gyro_cal->calibration_time_nanos; + gyro_cal->debug_gyro_cal.stillness_duration_nanos = + gyro_cal->calibration_time_duration_nanos; + + // Records the current calibration values. + gyro_cal->debug_gyro_cal.calibration[0] = gyro_cal->bias_x; + gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y; + gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z; + + // Records the complete temperature statistics. + gyroTempUpdateStats(&gyro_cal->debug_gyro_cal.debug_temperature, 0, + /*reset_stats=*/true); + gyroSamplingRateUpdate(&gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0, + /*reset_stats=*/true); + + // Records the previous window means. + gyro_cal->debug_gyro_cal.accel_mean[0] = + gyro_cal->accel_stillness_detect.prev_mean_x; + gyro_cal->debug_gyro_cal.accel_mean[1] = + gyro_cal->accel_stillness_detect.prev_mean_y; + gyro_cal->debug_gyro_cal.accel_mean[2] = + gyro_cal->accel_stillness_detect.prev_mean_z; + + gyro_cal->debug_gyro_cal.gyro_mean[0] = + gyro_cal->gyro_stillness_detect.prev_mean_x; + gyro_cal->debug_gyro_cal.gyro_mean[1] = + gyro_cal->gyro_stillness_detect.prev_mean_y; + gyro_cal->debug_gyro_cal.gyro_mean[2] = + gyro_cal->gyro_stillness_detect.prev_mean_z; + + gyro_cal->debug_gyro_cal.mag_mean[0] = + gyro_cal->mag_stillness_detect.prev_mean_x; + gyro_cal->debug_gyro_cal.mag_mean[1] = + gyro_cal->mag_stillness_detect.prev_mean_y; + gyro_cal->debug_gyro_cal.mag_mean[2] = + gyro_cal->mag_stillness_detect.prev_mean_z; + + // Records the variance data. + gyro_cal->debug_gyro_cal.accel_var[0] = + gyro_cal->accel_stillness_detect.win_var_x; + gyro_cal->debug_gyro_cal.accel_var[1] = + gyro_cal->accel_stillness_detect.win_var_y; + gyro_cal->debug_gyro_cal.accel_var[2] = + gyro_cal->accel_stillness_detect.win_var_z; + + gyro_cal->debug_gyro_cal.gyro_var[0] = + gyro_cal->gyro_stillness_detect.win_var_x; + gyro_cal->debug_gyro_cal.gyro_var[1] = + gyro_cal->gyro_stillness_detect.win_var_y; + gyro_cal->debug_gyro_cal.gyro_var[2] = + gyro_cal->gyro_stillness_detect.win_var_z; + + gyro_cal->debug_gyro_cal.mag_var[0] = + gyro_cal->mag_stillness_detect.win_var_x; + gyro_cal->debug_gyro_cal.mag_var[1] = + gyro_cal->mag_stillness_detect.win_var_y; + gyro_cal->debug_gyro_cal.mag_var[2] = + gyro_cal->mag_stillness_detect.win_var_z; } -// Helper function to print debug statements. -void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data) { - CAL_DEBUG_LOG( - "[GYRO_CAL]", - "#%d Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [mdps]\n", - count, CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MDEG, 6)); - - if (debug_data->used_mag_sensor) { - CAL_DEBUG_LOG("[GYRO_CAL]", " Using Magnetometer.\n"); - } else { - CAL_DEBUG_LOG("[GYRO_CAL]", " Not Using Magnetometer.\n"); +void gyroTempUpdateStats(struct DebugTemperature* debug_temperature, + float temperature, bool reset_stats) { + // Using the method of the assumed mean to preserve some numerical stability + // while avoiding per-sample divisions that the more numerically stable + // Welford method would afford. + + // Reference for the numerical method used below to compute the online mean + // and variance statistics: + // 1). en.wikipedia.org/wiki/assumed_mean + + // This is used for local calculations of temperature statistics. + static struct DebugTemperature local_temperature_stats = {0}; + static bool set_assumed_mean = true; + + // If 'debug_temperature' is not NULL then this function just reads out the + // current statistics, resets, and returns. + if (debug_temperature) { + if (local_temperature_stats.num_temperature_samples > 1) { + // Computes the final calculation of temperature sensor mean and variance. + float tmp = local_temperature_stats.temperature_mean_celsius; + local_temperature_stats.temperature_mean_celsius /= + local_temperature_stats.num_temperature_samples; + local_temperature_stats.temperature_var_celsius = + (local_temperature_stats.temperature_var_celsius - + local_temperature_stats.temperature_mean_celsius * tmp) / + (local_temperature_stats.num_temperature_samples - 1); + + // Adds the assumed mean value back to the total mean calculation. + local_temperature_stats.temperature_mean_celsius += + local_temperature_stats.assumed_mean; + } else { + // Not enough samples to compute a valid variance. Indicate this with a -1 + // value. + local_temperature_stats.temperature_var_celsius = -1.0f; + } + + memcpy(debug_temperature, &local_temperature_stats, + sizeof(struct DebugTemperature)); + reset_stats = true; } - CAL_DEBUG_LOG("[GYRO_CAL]", " Temperature = %s%d.%06d [C]\n", - CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 6)); + // Resets the temperature statistics and returns. + if (reset_stats) { + local_temperature_stats.num_temperature_samples = 0; + local_temperature_stats.temperature_mean_celsius = 0.0f; + local_temperature_stats.temperature_var_celsius = 0.0f; + set_assumed_mean = true; // Sets flag. + + // Initialize the min/max temperatures values. + local_temperature_stats.temperature_min_max_celsius[0] = FLT_MAX; + local_temperature_stats.temperature_min_max_celsius[1] = + -1.0f * (FLT_MAX - 1.0f); + return; + } - CAL_DEBUG_LOG("[GYRO_CAL]", " Calibration Timestamp = %llu [nsec]\n", - debug_data->calibration_time); + // The first sample received is taken as the "assumed mean". + if (set_assumed_mean) { + local_temperature_stats.assumed_mean = temperature; + set_assumed_mean = false; // Resets flag. + } - CAL_DEBUG_LOG("[GYRO_CAL]", " Stillness Duration = %llu [nsec]\n", - debug_data->calibration_time_duration); + // Increments the number of samples. + local_temperature_stats.num_temperature_samples++; - CAL_DEBUG_LOG("[GYRO_CAL]", - " Accel Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [g]\n", - CAL_ENCODE_FLOAT(debug_data->accel_mean[0] * GRAVITY_TO_G, 6), - CAL_ENCODE_FLOAT(debug_data->accel_mean[1] * GRAVITY_TO_G, 6), - CAL_ENCODE_FLOAT(debug_data->accel_mean[2] * GRAVITY_TO_G, 6)); + // Online computation of mean and variance for the running stillness period. + float delta = (temperature - local_temperature_stats.assumed_mean); + local_temperature_stats.temperature_var_celsius += delta * delta; + local_temperature_stats.temperature_mean_celsius += delta; - CAL_DEBUG_LOG("[GYRO_CAL]", - " Mag Mean = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]\n", - CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 6), - CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 6), - CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 6)); + // Track the min and max temperature values. + if (local_temperature_stats.temperature_min_max_celsius[0] > temperature) { + local_temperature_stats.temperature_min_max_celsius[0] = temperature; + } + if (local_temperature_stats.temperature_min_max_celsius[1] < temperature) { + local_temperature_stats.temperature_min_max_celsius[1] = temperature; + } } -// Print Debug data useful for tuning the stillness detectors. -void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) { - static uint64_t start_time = 0; +void gyroSamplingRateUpdate(float* debug_mean_sampling_rate_hz, + uint64_t timestamp_nanos, bool reset_stats) { + // This is used for local calculations of average sampling rate. + static uint64_t last_timestamp_nanos = 0; + static uint64_t time_delta_accumulator = 0; + static size_t num_samples = 0; + + // 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 (num_samples > 1 && time_delta_accumulator > 0) { + // Computes the final mean calculation. + *debug_mean_sampling_rate_hz = + num_samples / + (floatFromUint64(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; + } - // Output sensor variance levels to assist with tuning thresholds. - // i. Within the first 60 seconds of boot: output interval = 5 seconds. - // ii. Thereafter: output interval is set by DEBUG_GYRO_TUNE_UPDATES. - bool condition_i = ((sample_time <= 60000000000) && - ((sample_time - start_time) > 5000000000)); // nsec - bool condition_ii = ((sample_time > 60000000000) && - ((sample_time - start_time) > DEBUG_GYRO_TUNE_UPDATES)); - - if (condition_i || condition_ii) { - CAL_DEBUG_LOG("[GYRO_CAL]", ""); - CAL_DEBUG_LOG( - "[GYRO_CAL]", - "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} " - "[mdps]\n", - gyro_cal->debug_calibration_count, - CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6)); - CAL_DEBUG_LOG( - "[GYRO_CAL]", - " Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n", - CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8), - CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8), - CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8)); - CAL_DEBUG_LOG( - "[GYRO_CAL]", - " Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [m/sec^2]^2\n", - CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8), - CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8), - CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8)); - if (gyro_cal->using_mag_sensor) { + // Resets the sampling rate mean estimator data if: + // 1. The 'reset_stats' flag is set. + // 2. A bad timestamp was received (i.e., time not monotonic). + // 3. 'last_timestamp_nanos' is zero. + if (reset_stats || (timestamp_nanos <= last_timestamp_nanos) || + last_timestamp_nanos == 0) { + last_timestamp_nanos = timestamp_nanos; + time_delta_accumulator = 0; + num_samples = 0; + return; + } + + // Increments the number of samples. + num_samples++; + + // Accumulate the time steps. + time_delta_accumulator += timestamp_nanos - last_timestamp_nanos; + last_timestamp_nanos = timestamp_nanos; +} + +void gyroCalDebugPrintData(const struct DebugGyroCal* debug_data, + enum DebugPrintData print_data) { + // Prints out the desired debug data. + float mag_data; + switch (print_data) { + case BIAS_CAL: CAL_DEBUG_LOG( - "[GYRO_CAL]", - " Mag Variance = {%s%d.%06d, %s%d.%06d, %s%d.%06d} [uT]^2\n", - CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 6), - CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 6), - CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 6)); + "[GYRO_CAL:BIAS]", + "Gyro Bias Calibration [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->calibration[0] * RAD_TO_MILLI_DEGREES, + 8), + CAL_ENCODE_FLOAT(debug_data->calibration[1] * RAD_TO_MILLI_DEGREES, + 8), + CAL_ENCODE_FLOAT(debug_data->calibration[2] * RAD_TO_MILLI_DEGREES, + 8)); + break; + + case CAL_TIME: + CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness Start Time [nsec]: %llu", + (unsigned long long int)debug_data->start_still_time_nanos); + + CAL_DEBUG_LOG("[GYRO_CAL:TIME]", "Stillness End Time [nsec]: %llu", + (unsigned long long int)debug_data->end_still_time_nanos); + CAL_DEBUG_LOG( - "[GYRO_CAL]", " Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n", + "[GYRO_CAL:TIME]", "Stillness Duration [nsec]: %llu", + (unsigned long long int)debug_data->stillness_duration_nanos); + break; + + case ACCEL_STATS: + CAL_DEBUG_LOG("[GYRO_CAL:ACCEL_STATS]", + "Accel Mean [m/sec^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->accel_mean[0], 8), + CAL_ENCODE_FLOAT(debug_data->accel_mean[1], 8), + CAL_ENCODE_FLOAT(debug_data->accel_mean[2], 8)); + CAL_DEBUG_LOG( + "[GYRO_CAL:ACCEL_STATS]", + "Accel Variance [(m/sec^2)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->accel_var[0], 8), + CAL_ENCODE_FLOAT(debug_data->accel_var[1], 8), + CAL_ENCODE_FLOAT(debug_data->accel_var[2], 8)); + break; + + case GYRO_STATS: + CAL_DEBUG_LOG( + "[GYRO_CAL:GYRO_STATS]", + "Gyro Mean [mdps]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->gyro_mean[0] * RAD_TO_MILLI_DEGREES, 8), + CAL_ENCODE_FLOAT(debug_data->gyro_mean[1] * RAD_TO_MILLI_DEGREES, 8), + CAL_ENCODE_FLOAT(debug_data->gyro_mean[2] * RAD_TO_MILLI_DEGREES, 8)); + CAL_DEBUG_LOG( + "[GYRO_CAL:GYRO_STATS]", + "Gyro Variance [(rad/sec)^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->gyro_var[0], 8), + CAL_ENCODE_FLOAT(debug_data->gyro_var[1], 8), + CAL_ENCODE_FLOAT(debug_data->gyro_var[2], 8)); + break; + + case MAG_STATS: + if (debug_data->using_mag_sensor) { + CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", + "Mag Mean [uT]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->mag_mean[0], 8), + CAL_ENCODE_FLOAT(debug_data->mag_mean[1], 8), + CAL_ENCODE_FLOAT(debug_data->mag_mean[2], 8)); + CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", + "Mag Variance [uT^2]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->mag_var[0], 8), + CAL_ENCODE_FLOAT(debug_data->mag_var[1], 8), + CAL_ENCODE_FLOAT(debug_data->mag_var[2], 8)); + } else { + CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", "Mag Mean [uT]: 0, 0, 0"); + // The -1's indicate that the magnetometer sensor was not used. + CAL_DEBUG_LOG("[GYRO_CAL:MAG_STATS]", + "Mag Variance [uT^2]: -1.0, -1.0, -1.0"); + } + break; + + case TEMP_STATS: + CAL_DEBUG_LOG("[GYRO_CAL:TEMP_STATS]", + "Latest Temperature [C]: %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 8)); + CAL_DEBUG_LOG( + "[GYRO_CAL:TEMP_STATS]", + "Min/Max Temperature [C]: %s%d.%08d, %s%d.%08d", CAL_ENCODE_FLOAT( - gyro_cal->gyro_stillness_detect.stillness_confidence, 6), + debug_data->debug_temperature.temperature_min_max_celsius[0], 8), CAL_ENCODE_FLOAT( - gyro_cal->accel_stillness_detect.stillness_confidence, 6), - CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.stillness_confidence, - 6)); - } else { - CAL_DEBUG_LOG("[GYRO_CAL]", " Mag Variance = {---, ---, ---} [uT]^2\n"); + debug_data->debug_temperature.temperature_min_max_celsius[1], 8)); CAL_DEBUG_LOG( - "[GYRO_CAL]", " Stillness = {G%s%d.%06d, A%s%d.%06d, M---}\n", + "[GYRO_CAL:TEMP_STATS]", "Temperature Mean [C]: %s%d.%08d", CAL_ENCODE_FLOAT( - gyro_cal->gyro_stillness_detect.stillness_confidence, 6), + debug_data->debug_temperature.temperature_mean_celsius, 8)); + CAL_DEBUG_LOG( + "[GYRO_CAL:TEMP_STATS]", "Temperature Variance [C^2]: %s%d.%08d", CAL_ENCODE_FLOAT( - gyro_cal->accel_stillness_detect.stillness_confidence, 6)); - } + debug_data->debug_temperature.temperature_var_celsius, 8)); + break; - CAL_DEBUG_LOG("[GYRO_CAL]", " Temperature = %s%d.%06d [C]\n", - CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6)); - CAL_DEBUG_LOG("[GYRO_CAL]", " Timestamp = %llu [nsec]\n", sample_time); - CAL_DEBUG_LOG("[GYRO_CAL]", " Total Gyro Calibrations: %lu\n", - gyro_cal->debug_calibration_count); - start_time = sample_time; // reset - } -} + case STILLNESS_DATA: + mag_data = (debug_data->using_mag_sensor) + ? debug_data->mag_stillness_conf + : -1.0f; // Signals that magnetometer was not used. + CAL_DEBUG_LOG("[GYRO_CAL:STILLNESS]", + "Stillness [G/A/M]: %s%d.%08d, %s%d.%08d, %s%d.%08d", + CAL_ENCODE_FLOAT(debug_data->gyro_stillness_conf, 8), + CAL_ENCODE_FLOAT(debug_data->accel_stillness_conf, 8), + CAL_ENCODE_FLOAT(mag_data, 8)); + break; -// Start the debug data report state machine. -void gyroCalDebugPrintStart(struct GyroCal* gyro_cal) { - if (gyro_cal->gyro_debug_state < 0) { // if currently in idle state. - gyro_cal->gyro_debug_state = 0; // start printing. + case SAMPLING_RATE: + CAL_DEBUG_LOG("[GYRO_CAL:SAMPLING_RATE]", + "Gyro Sampling Rate [Hz]: %s%d.%06d", + CAL_ENCODE_FLOAT( + debug_data->mean_sampling_rate_hz, 6)); + break; + + default: + break; } } -// Print Debug data report. -void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time) { - static int head_index = 0; - static uint64_t wait_timer = 0; - static int i = 0; - static int next_state = 0; - - // State machine to control reporting out debug print data. - switch (gyro_cal->gyro_debug_state) { - case 0: - // STATE 0 : Print out header - if (gyro_cal->debug_num_cals == 0) { +// Debug printout state enumeration. +enum GyroCalDebugState { + IDLE = 0, + WAIT_STATE, + PRINT_BIAS, + PRINT_TIME, + PRINT_TEMP, + PRINT_ACCEL, + PRINT_GYRO, + PRINT_MAG, + PRINT_STILLNESS, + PRINT_SAMPLING_RATE +}; + +void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t timestamp_nanos) { + static enum GyroCalDebugState debug_state = IDLE; + static enum GyroCalDebugState next_state = IDLE; + static uint64_t wait_timer_nanos = 0; + + // This is a state machine that controls the reporting out of debug data. + switch (debug_state) { + case IDLE: + // Wait for a trigger and start the debug printout sequence. + if (gyro_cal->debug_print_trigger) { + debug_state = PRINT_BIAS; CAL_DEBUG_LOG("[GYRO_CAL]", ""); - CAL_DEBUG_LOG("[GYRO_CAL]", "No Gyro Calibrations To Report.\n"); - CAL_DEBUG_LOG( - "[GYRO_CAL]", - "#0 Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} " - "[mdps]\n", - CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 6), - CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 6)); - wait_timer = sample_time; // start the wait timer. - gyro_cal->gyro_debug_state = 7; // go to next state. + gyro_cal->debug_print_trigger = false; // Resets trigger. } else { - CAL_DEBUG_LOG("[GYRO_CAL]", ""); - CAL_DEBUG_LOG("[GYRO_CAL]", - "---DEBUG REPORT-------------------------------------\n"); - CAL_DEBUG_LOG("[GYRO_CAL]", - "Gyro Calibrations To Report (newest to oldest): %d\n", - gyro_cal->debug_num_cals); - head_index = gyro_cal->debug_head; - i = 1; // initialize report count - wait_timer = sample_time; // start the wait timer. - next_state = 1; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + debug_state = IDLE; } break; - case 1: - // STATE 1: Print out past N recent calibrations. - if (i <= gyro_cal->debug_num_cals) { - // Print the data. - gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data[head_index]); - - // Move to the previous calibration data set. - head_index--; - if (head_index < 0) { - head_index = DEBUG_GYRO_SHORTTERM_NUM_CAL - 1; - } - - // Increment the report count. - i++; - - // Go to wait state. - wait_timer = sample_time; // start the wait timer. - next_state = 1; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. - } else { - // Go to wait state. - wait_timer = sample_time; // start the wait timer. - next_state = 3; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + case WAIT_STATE: + // This helps throttle the print statements. + if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) { + debug_state = next_state; } break; - case 2: - // STATE 2 : Wait state. - // Wait for 500msec. - // This helps throttle the print statements. - if ((sample_time - wait_timer) >= 500000000) { - gyro_cal->gyro_debug_state = next_state; - } + case PRINT_BIAS: + CAL_DEBUG_LOG("[GYRO_CAL:BIAS]", "Total # Calibrations: %lu", + (unsigned long int)gyro_cal->debug_calibration_count); + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, BIAS_CAL); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_TIME; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. break; - case 3: - // STATE 3 : Print the end of the debug report. - CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n", - gyro_cal->debug_calibration_count); + case PRINT_TIME: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, CAL_TIME); - CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n", - gyro_cal->debug_watchdog_count); + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_TEMP; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_TEMP: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, TEMP_STATS); - CAL_DEBUG_LOG("[GYRO_CAL]", - "---END DEBUG REPORT---------------------------------\n"); - wait_timer = sample_time; // start the wait timer. - next_state = 4; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_ACCEL; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. break; - case 4: - // STATE 4 : Print out header (historic data) - CAL_DEBUG_LOG("[GYRO_CAL]", ""); - CAL_DEBUG_LOG("[GYRO_CAL]", - "---DEBUG REPORT (HISTORY)---------------------------\n"); - CAL_DEBUG_LOG("[GYRO_CAL]", - "Gyro Calibrations To Report (newest to oldest): %d\n", - gyro_cal->debug_num_cals_hist); - head_index = gyro_cal->debug_head_hist; - i = 1; // initialize report count - wait_timer = sample_time; // start the wait timer. - next_state = 5; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + case PRINT_ACCEL: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, ACCEL_STATS); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_GYRO; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. break; - case 5: - // STATE 5: Print out past N historic calibrations. - if (i <= gyro_cal->debug_num_cals_hist) { - // Print the data. - gyroCalDebugPrintData(i, &gyro_cal->debug_cal_data_hist[head_index]); - - // Move to the previous calibration data set. - head_index--; - if (head_index < 0) { - head_index = DEBUG_GYRO_LONGTERM_NUM_CAL - 1; - } - - // Increment the report count. - i++; - - // Go to wait state. - wait_timer = sample_time; // start the wait timer. - next_state = 5; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + case PRINT_GYRO: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, GYRO_STATS); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_MAG; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_MAG: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, MAG_STATS); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_STILLNESS; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_STILLNESS: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, STILLNESS_DATA); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_SAMPLING_RATE; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_SAMPLING_RATE: + gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, SAMPLING_RATE); + debug_state = IDLE; + break; + + default: + // Sends this state machine to its idle state. + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = IDLE; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + } + +#ifdef GYRO_CAL_DBG_TUNE_ENABLED + if (debug_state == IDLE) { + // This check keeps the tuning printout from interleaving with the above + // debug print data. + gyroCalTuneDebugPrint(gyro_cal, timestamp_nanos); + } +#endif // GYRO_CAL_DBG_TUNE_ENABLED +} + +#ifdef GYRO_CAL_DBG_TUNE_ENABLED +void gyroCalTuneDebugPrint(const struct GyroCal* gyro_cal, + uint64_t timestamp_nanos) { + static enum GyroCalDebugState debug_state = IDLE; + static enum GyroCalDebugState next_state = IDLE; + static uint64_t wait_timer_nanos = 0; + + // Output sensor variance levels to assist with tuning thresholds. + // i. Within the first 180 seconds of boot: output interval = 5 + // seconds. + // ii. Thereafter: output interval is 60 seconds. + bool condition_i = + ((timestamp_nanos <= 180000000000) && + ((timestamp_nanos - wait_timer_nanos) > 5000000000)); // nsec + bool condition_ii = ((timestamp_nanos > 60000000000) && + ((timestamp_nanos - wait_timer_nanos) > 60000000000)); + + // This is a state machine that controls the reporting out of debug data. + switch (debug_state) { + case IDLE: + // Wait for a trigger and start the debug printout sequence. + if (condition_i || condition_ii) { + debug_state = PRINT_BIAS; } else { - // Go to wait state. - wait_timer = sample_time; // start the wait timer. - next_state = 6; // set the next state. - gyro_cal->gyro_debug_state = 2; // but first, go to wait state. + debug_state = IDLE; } break; - case 6: - // STATE 6 : Print the end of the debug report. - CAL_DEBUG_LOG("[GYRO_CAL]", "Total Gyro Calibrations: %lu\n", - gyro_cal->debug_calibration_count); + case WAIT_STATE: + // This helps throttle the print statements. + if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) { + debug_state = next_state; + } + break; + + case PRINT_BIAS: + CAL_DEBUG_LOG("[GYRO_CAL]", ""); + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + "#%lu Gyro Bias Calibration = {%s%d.%06d, %s%d.%06d, %s%d.%06d} " + "[mdps]\n", + (unsigned long int)gyro_cal->debug_calibration_count, + CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MILLI_DEGREES, 6)); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_TIME; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_TIME: + CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Timestamp = %llu [nsec]\n", + (unsigned long long int)timestamp_nanos); + CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Total Gyro Calibrations: %lu\n", + (unsigned long int)gyro_cal->debug_calibration_count); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_TEMP; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_TEMP: + CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", " Temperature = %s%d.%06d [C]\n", + CAL_ENCODE_FLOAT(gyro_cal->latest_temperature_celcius, 6)); - CAL_DEBUG_LOG("[GYRO_CAL]", "Total Watchdog Timeouts: %lu\n", - gyro_cal->debug_watchdog_count); + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_ACCEL; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; - CAL_DEBUG_LOG("[GYRO_CAL]", - "---END DEBUG REPORT---------------------------------\n"); - wait_timer = sample_time; // start the wait timer. - gyro_cal->gyro_debug_state = 7; // go to next state. + case PRINT_ACCEL: + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + " Accel Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} " + "[m/sec^2]^2\n", + CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_x, 8), + CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_y, 8), + CAL_ENCODE_FLOAT(gyro_cal->accel_stillness_detect.win_var_z, 8)); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_GYRO; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. break; - case 7: - // STATE 7 : Final wait state. - // Wait for 10 seconds. - // This helps throttle the print statements. - if ((sample_time - wait_timer) >= 10000000000) { - gyro_cal->gyro_debug_state = -1; + case PRINT_GYRO: + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + " Gyro Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [rad/sec]^2\n", + CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_x, 8), + CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_y, 8), + CAL_ENCODE_FLOAT(gyro_cal->gyro_stillness_detect.win_var_z, 8)); + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_MAG; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_MAG: + if (gyro_cal->using_mag_sensor) { + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + " Mag Variance = {%s%d.%08d, %s%d.%08d, %s%d.%08d} [uT]^2\n", + CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_x, 8), + CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_y, 8), + CAL_ENCODE_FLOAT(gyro_cal->mag_stillness_detect.win_var_z, 8)); + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + " Stillness = {G%s%d.%03d, A%s%d.%03d, M%s%d.%03d}\n", + CAL_ENCODE_FLOAT( + gyro_cal->gyro_stillness_detect.stillness_confidence, 3), + CAL_ENCODE_FLOAT( + gyro_cal->accel_stillness_detect.stillness_confidence, 3), + CAL_ENCODE_FLOAT( + gyro_cal->mag_stillness_detect.stillness_confidence, 3)); + } else { + CAL_DEBUG_LOG("[GYRO_CAL:TUNE]", + " Mag Variance = {---, ---, ---} [uT]^2\n"); + CAL_DEBUG_LOG( + "[GYRO_CAL:TUNE]", + " Stillness = {G%s%d.%03d, A%s%d.%03d, M---}\n", + CAL_ENCODE_FLOAT( + gyro_cal->gyro_stillness_detect.stillness_confidence, 3), + CAL_ENCODE_FLOAT( + gyro_cal->accel_stillness_detect.stillness_confidence, 3)); } + + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = IDLE; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. break; default: - // Idle state. - gyro_cal->gyro_debug_state = -1; - - // Report periodic data useful for tuning the stillness detectors. - gyroCalTuneDebugPrint(gyro_cal, sample_time); + // Sends this state machine to its idle state. + wait_timer_nanos = timestamp_nanos; // Starts the wait timer. + next_state = IDLE; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. } } -#endif +#endif // GYRO_CAL_DBG_TUNE_ENABLED +#endif // GYRO_CAL_DBG_ENABLED diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h index f5a9109b..4b5a9c44 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -17,7 +14,6 @@ * limitations under the License. */ -/////////////////////////////////////////////////////////////// /* * This module contains the algorithms for producing a * gyroscope offset calibration. The algorithm looks @@ -41,8 +37,14 @@ * Optional Sensors and Units: * - Magnetometer [micro-Tesla, uT] * - Temperature [Celcius] + * + * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements. + * #define GYRO_CAL_DBG_TUNE_ENABLED to periodically printout sensor variance + * data to assist in tuning the GyroCal parameters. */ -/////////////////////////////////////////////////////////////// + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ #include "calibration/gyroscope/gyro_stillness_detect.h" @@ -51,21 +53,26 @@ extern "C" { #endif #ifdef GYRO_CAL_DBG_ENABLED -// Debug: Number of past calibrations to store. -#define DEBUG_GYRO_SHORTTERM_NUM_CAL 30 -#define DEBUG_GYRO_LONGTERM_NUM_CAL 30 -#define DEBUG_GYRO_CAL_LIMIT 7200000000000 // 2hours [nsec] -#define DEBUG_GYRO_TUNE_UPDATES 120000000000 // 120sec [nsec] +// Temperature debug statistics. +struct DebugTemperature { + size_t num_temperature_samples; + float temperature_min_max_celsius[2]; + float temperature_mean_celsius; + float temperature_var_celsius; + float assumed_mean; +}; // Gyro Cal debug information/data tracking structure. struct DebugGyroCal { + struct DebugTemperature debug_temperature; + uint64_t start_still_time_nanos; + uint64_t end_still_time_nanos; + uint64_t stillness_duration_nanos; + float mean_sampling_rate_hz; float temperature_celcius; float accel_stillness_conf; float gyro_stillness_conf; float mag_stillness_conf; - uint64_t calibration_time; - uint64_t calibration_time_duration; - bool used_mag_sensor; float calibration[3]; float accel_mean[3]; float gyro_mean[3]; @@ -73,6 +80,7 @@ struct DebugGyroCal { float accel_var[3]; float gyro_var[3]; float mag_var[3]; + bool using_mag_sensor; }; #endif @@ -82,13 +90,6 @@ struct GyroCal { struct GyroStillDet mag_stillness_detect; struct GyroStillDet gyro_stillness_detect; - // Flag is "true" when the magnetometer is used. - bool using_mag_sensor; - - // Flag set by user to control whether calibrations are used - // (default: "true"). - bool gyro_calibration_enable; - // Latest temperature measurement. float latest_temperature_celcius; @@ -96,65 +97,63 @@ struct GyroCal { float stillness_threshold; // Min and max durations for gyro bias calibration. - uint64_t min_still_duration; - uint64_t max_still_duration; + uint64_t min_still_duration_nanos; + uint64_t max_still_duration_nanos; // Duration of the stillness processing windows. - uint64_t window_time_duration; - - // Flag to indicate if device was previously still. - bool prev_still; + uint64_t window_time_duration_nanos; // Timestamp when device started a still period. - uint64_t start_still_time; + uint64_t start_still_time_nanos; // gyro bias estimates and last calibration timestamp. float bias_x, bias_y, bias_z; // [rad/sec] - uint64_t calibration_time; - uint64_t calibration_time_duration; + uint64_t calibration_time_nanos; + uint64_t calibration_time_duration_nanos; float stillness_confidence; - bool new_gyro_cal_available; // true when a new cal is ready. // Current window end time for all sensors. Used to assist in keeping // sensor data collection in sync. On initialization this will be set to // zero indicating that sensor data will be dropped until a valid end time // is set from the first gyro timestamp received. - uint64_t stillness_win_endtime; + uint64_t stillness_win_endtime_nanos; // Watchdog timer to reset to a known good state when data capture stalls. - uint64_t gyro_watchdog_start; - uint64_t gyro_watchdog_timeout_duration; + 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; + + // Flag set by user to control whether calibrations are used (default: + // "true"). + bool gyro_calibration_enable; + + // Flag is 'true' when a new calibration update is ready. + bool new_gyro_cal_available; + + // Flag to indicate if device was previously still. + bool prev_still; + //---------------------------------------------------------------- #ifdef GYRO_CAL_DBG_ENABLED // Debug info. - bool debug_processed_data_available; // flag on a per window basis. - uint64_t debug_processed_data_time; // flag time stamp. - uint32_t debug_calibration_count; // total number of cals performed. - uint32_t debug_watchdog_count; // total number of watchdog timeouts. - int8_t gyro_debug_state; // debug report state machine. - - // Debug short-term history data. - struct DebugGyroCal debug_cal_data[DEBUG_GYRO_SHORTTERM_NUM_CAL]; - uint8_t debug_num_cals; // number of calibrations collected. - uint8_t debug_head; // index of last valid calibration. - - // Debug long-term history data (limited collection frequency). - struct DebugGyroCal debug_cal_data_hist[DEBUG_GYRO_LONGTERM_NUM_CAL]; - uint8_t debug_num_cals_hist; // number of calibrations collected. - uint8_t debug_head_hist; // index of last valid calibration. -#endif + struct DebugGyroCal debug_gyro_cal; // Debug data structure. + 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. +#endif // GYRO_CAL_DBG_ENABLED }; /////// FUNCTION PROTOTYPES ////////////////////////////////////////// // Initialize the gyro calibration data structure. void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, - uint64_t max_still_duration, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time, - uint64_t window_time_duration, float gyro_var_threshold, + 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, @@ -169,7 +168,7 @@ void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, // 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); + float bias_z, 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, @@ -179,23 +178,21 @@ void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal); // Update the gyro calibration with gyro data [rad/sec]. -void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time, +void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, float x, float y, float z, float temperature); // Update the gyro calibration with mag data [micro Tesla]. -void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x, - float y, float z); +void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, + float x, float y, float z); // Update the gyro calibration with accel data [m/sec^2]. -void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, +void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, float x, float y, float z); #ifdef GYRO_CAL_DBG_ENABLED -// Start the debug data report state machine. -void gyroCalDebugPrintStart(struct GyroCal* gyro_cal); - // Print debug data report. -void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time); +void gyroCalDebugPrint(struct GyroCal* gyro_cal, + uint64_t timestamp_nanos_nanos); #endif #ifdef __cplusplus diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c index 12763b08..afddb481 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c @@ -52,7 +52,7 @@ void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det, float x, float y, float z) { // Using the method of the assumed mean to preserve some numerical // stability while avoiding per-sample divisions that the more - // numerically stabe Welford method would afford. + // numerically stable Welford method would afford. // Reference for the numerical method used below to compute the // online mean and variance statistics: diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h index 9ddc9075..9a1d876c 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -33,6 +30,8 @@ * are nanoseconds. */ /////////////////////////////////////////////////////////////// +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_STILLNESS_DETECT_H_ #include #include diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c index 340d021e..bd4876ca 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.c +++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c @@ -30,16 +30,18 @@ #define MAX_BATCH_WINDOW 15000000UL // 15 sec #define MIN_BATCH_SIZE 25 // samples +#define MAX_DISTANCE_VIOLATIONS 2 + // eigen value magnitude and ratio test -static int moc_eigen_test(struct MagCal *moc) { +static int moc_eigen_test(struct KasaFit *kasa) { // covariance matrix struct Mat33 S; - S.elem[0][0] = moc->acc_xx - moc->acc_x * moc->acc_x; - S.elem[0][1] = S.elem[1][0] = moc->acc_xy - moc->acc_x * moc->acc_y; - S.elem[0][2] = S.elem[2][0] = moc->acc_xz - moc->acc_x * moc->acc_z; - S.elem[1][1] = moc->acc_yy - moc->acc_y * moc->acc_y; - S.elem[1][2] = S.elem[2][1] = moc->acc_yz - moc->acc_y * moc->acc_z; - S.elem[2][2] = moc->acc_zz - moc->acc_z * moc->acc_z; + S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x; + S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y; + S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z; + S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y; + S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z; + S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z; struct Vec3 eigenvals; struct Mat33 eigenvecs; @@ -60,29 +62,29 @@ static int moc_eigen_test(struct MagCal *moc) { } // Kasa sphere fitting with normal equation -int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius) { +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] = moc->acc_xx; - A.elem[0][1] = moc->acc_xy; - A.elem[0][2] = moc->acc_xz; - A.elem[0][3] = moc->acc_x; - A.elem[1][0] = moc->acc_xy; - A.elem[1][1] = moc->acc_yy; - A.elem[1][2] = moc->acc_yz; - A.elem[1][3] = moc->acc_y; - A.elem[2][0] = moc->acc_xz; - A.elem[2][1] = moc->acc_yz; - A.elem[2][2] = moc->acc_zz; - A.elem[2][3] = moc->acc_z; - A.elem[3][0] = moc->acc_x; - A.elem[3][1] = moc->acc_y; - A.elem[3][2] = moc->acc_z; + 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, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w); + initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w); struct Size4 pivot; mat44DecomposeLup(&A, &pivot); @@ -112,13 +114,18 @@ int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius) { return success; } -void magCalReset(struct MagCal *moc) { - moc->acc_x = moc->acc_y = moc->acc_z = moc->acc_w = 0.0f; - moc->acc_xx = moc->acc_xy = moc->acc_xz = moc->acc_xw = 0.0f; - moc->acc_yy = moc->acc_yz = moc->acc_yw = 0.0f; - moc->acc_zz = moc->acc_zw = 0.0f; +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; +} - moc->nsamples = 0; +void magCalReset(struct MagCal *moc) { + magKasaReset(&moc->kasa); + diversityCheckerReset(&moc->diversity_checker); moc->start_time = 0; } @@ -126,20 +133,32 @@ static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { int complete = 0; if ((sample_time_us - moc->start_time > MIN_BATCH_WINDOW) && - (moc->nsamples > MIN_BATCH_SIZE)) { + (moc->kasa.nsamples > MIN_BATCH_SIZE)) { complete = 1; - } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { - // not enough samples collected in MAX_BATCH_WINDOW + } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW || + moc->diversity_checker.num_max_dist_violations + >= MAX_DISTANCE_VIOLATIONS) { + // not enough samples collected in MAX_BATCH_WINDOW or too many + // maximum distance violations detected. magCalReset(moc); } 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) { + float c12, float c20, float c21, float c22, + float threshold, float max_distance, + size_t min_num_diverse_vectors, + size_t max_num_max_distance, + float var_threshold, + float max_min_threshold) { magCalReset(moc); moc->update_time = 0; moc->radius = 0.0f; @@ -157,6 +176,15 @@ void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias, moc->c20 = c20; moc->c21 = c21; moc->c22 = c22; + + // Diversity Checker Init + diversityCheckerInit(&moc->diversity_checker, + threshold, + max_distance, + min_num_diverse_vectors, + max_num_max_distance, + var_threshold, + max_min_threshold); } void magCalDestroy(struct MagCal *moc) { (void)moc; } @@ -165,66 +193,74 @@ bool magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, float x, float y, float z) { bool new_bias = false; + // Diversity Checker Update. + diversityCheckerUpdate(&moc->diversity_checker, x, y, z); + // 1. run accumulators float w = x * x + y * y + z * z; - moc->acc_x += x; - moc->acc_y += y; - moc->acc_z += z; - moc->acc_w += w; + moc->kasa.acc_x += x; + moc->kasa.acc_y += y; + moc->kasa.acc_z += z; + moc->kasa.acc_w += w; - moc->acc_xx += x * x; - moc->acc_xy += x * y; - moc->acc_xz += x * z; - moc->acc_xw += x * 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->acc_yy += y * y; - moc->acc_yz += y * z; - moc->acc_yw += y * w; + moc->kasa.acc_yy += y * y; + moc->kasa.acc_yz += y * z; + moc->kasa.acc_yw += y * w; - moc->acc_zz += z * z; - moc->acc_zw += z * w; + moc->kasa.acc_zz += z * z; + moc->kasa.acc_zw += z * w; - if (++moc->nsamples == 1) { + if (++moc->kasa.nsamples == 1) { moc->start_time = sample_time_us; } // 2. batch has enough samples? if (moc_batch_complete(moc, sample_time_us)) { - float inv = 1.0f / moc->nsamples; + float inv = 1.0f / moc->kasa.nsamples; - moc->acc_x *= inv; - moc->acc_y *= inv; - moc->acc_z *= inv; - moc->acc_w *= inv; + moc->kasa.acc_x *= inv; + moc->kasa.acc_y *= inv; + moc->kasa.acc_z *= inv; + moc->kasa.acc_w *= inv; - moc->acc_xx *= inv; - moc->acc_xy *= inv; - moc->acc_xz *= inv; - moc->acc_xw *= inv; + moc->kasa.acc_xx *= inv; + moc->kasa.acc_xy *= inv; + moc->kasa.acc_xz *= inv; + moc->kasa.acc_xw *= inv; - moc->acc_yy *= inv; - moc->acc_yz *= inv; - moc->acc_yw *= inv; + moc->kasa.acc_yy *= inv; + moc->kasa.acc_yz *= inv; + moc->kasa.acc_yw *= inv; - moc->acc_zz *= inv; - moc->acc_zw *= inv; + moc->kasa.acc_zz *= inv; + moc->kasa.acc_zw *= inv; // 3. eigen test - if (moc_eigen_test(moc)) { + if (moc_eigen_test(&moc->kasa)) { struct Vec3 bias; float radius; // 4. Kasa sphere fitting - if (magCalFit(moc, &bias, &radius)) { - moc->x_bias = bias.x; - moc->y_bias = bias.y; - moc->z_bias = bias.z; - - moc->radius = radius; - moc->update_time = sample_time_us; - - new_bias = true; + if (magKasaFit(&moc->kasa, &bias, &radius)) { + if (diversityCheckerNormQuality(&moc->diversity_checker, + bias.x, + bias.y, + bias.z)) { + moc->x_bias = bias.x; + moc->y_bias = bias.y; + moc->z_bias = bias.z; + + moc->radius = radius; + moc->update_time = sample_time_us; + + new_bias = true; + } } } diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h index a73aa22a..eccf35ce 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.h +++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -16,35 +13,50 @@ * 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_ #include #include #include +#include "calibration/common/diversity_checker.h" #include "common/math/mat.h" +#include "common/math/vec.h" #ifdef __cplusplus extern "C" { #endif -struct MagCal { - uint64_t start_time; - uint64_t update_time; - +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; +}; + +struct MagCal { + struct DiversityChecker diversity_checker; + struct KasaFit kasa; + + uint64_t start_time; + uint64_t update_time; float x_bias, y_bias, z_bias; float radius; float c00, c01, c02, c10, c11, c12, c20, c21, c22; - - size_t nsamples; }; +void initKasa(struct KasaFit *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); + float c12, float c20, float c21, float c22, + float threshold, float max_distance, + size_t min_num_diverse_vectors, + size_t max_num_max_distance, + float var_threshold, + float max_min_threshold); void magCalDestroy(struct MagCal *moc); @@ -65,9 +77,11 @@ void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02, 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 magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius); +int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius); #ifdef __cplusplus } diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c new file mode 100644 index 00000000..0b8b6c73 --- /dev/null +++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c @@ -0,0 +1,940 @@ +/* + * Copyright (C) 2016 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/over_temp/over_temp_cal.h" + +#include +#include +#include + +#include "calibration/util/cal_log.h" +#include "common/math/vec.h" +#include "util/nano_assert.h" + +/////// DEFINITIONS AND MACROS //////////////////////////////////////////////// + +// The 'temp_sensitivity' parameters are set to this value to indicate that the +// model is in its initial state. +#define MODEL_INITIAL_STATE (1e6f) + +// Rate-limits the search for the nearest offset estimate to every 10 seconds +// when no model has been updated yet. +#define OVERTEMPCAL_NEAREST_NANOS (10000000000) + +// Rate-limits the check of old data to every 2 hours. +#define OVERTEMPCAL_STALE_CHECK_TIME_NANOS (7200000000000) + +// A common sensor operating temperature at which to start producing the model +// jump-start data. +#define JUMPSTART_START_TEMP_CELSIUS (30.0f) + +#ifdef OVERTEMPCAL_DBG_ENABLED +// A debug version label to help with tracking results. +#define OVERTEMPCAL_DEBUG_VERSION_STRING "[Dec 12, 2016]" + +// The time value used to throttle debug messaging. +#define OVERTEMPCAL_WAIT_TIME_NANOS (250000000) + +// Debug log tag string used to identify debug report output data. +#define OVERTEMPCAL_REPORT_TAG "[OVER_TEMP_CAL:REPORT]" + +// Converts units of radians to milli-degrees. +#define RAD_TO_MILLI_DEGREES (float)(1e3f * 180.0f / M_PI) + +// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z. +static const char kDebugAxisLabel[3] = "XYZ"; +#endif // OVERTEMPCAL_DBG_ENABLED + +/////// FORWARD DECLARATIONS ////////////////////////////////////////////////// + +// Updates the most recent model estimate data. +static void setLatestEstimate(struct OverTempCal *over_temp_cal, + const float *offset, float offset_temp, + uint64_t timestamp_nanos); + +/* + * Determines if a new over-temperature model fit should be performed, and then + * updates the model as needed. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * timestamp_nanos: Current timestamp for the model update. + */ +static void computeModelUpdate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos); + +/* + * Searches 'model_data' for the sensor offset estimate closest to the current + * temperature. Sets the 'latest_offset' pointer to the result. + */ +static void setNearestEstimate(struct OverTempCal *over_temp_cal); + +/* + * Removes the "old" offset estimates from 'model_data' (i.e., eliminates the + * drift-compromised data). Returns 'true' if any data was removed. + */ +static bool removeStaleModelData(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos); + +/* + * Removes the offset estimates from 'model_data' at index, 'model_index'. + * Returns 'true' if data was removed. + */ +static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, + size_t model_index); + +/* + * 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. + */ +static bool jumpStartModelData(struct OverTempCal *over_temp_cal); + +/* + * Provides updated model parameters for the over-temperature model data. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * OUTPUTS: + * temp_sensitivity: Updated modeled temperature sensitivity (array). + * sensor_intercept: Updated model intercept (array). + * + * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + * + * Reference: "Comparing two ways to fit a line to data", John D. Cook. + * http://www.johndcook.com/blog/2008/10/20/comparing-two-ways-to-fit-a-line-to-data/ + */ +static void updateModel(const struct OverTempCal *over_temp_cal, + float *temp_sensitivity, float *sensor_intercept); + +/* + * Removes the over-temp compensated offset from the input sensor data. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * vi: Single axis sensor data to be compensated. + * index: Index for model parameter compensation (0=x, 1=y, 2=z). + * OUTPUTS: + * vo: Single axis sensor data that has been compensated. + */ +static void removeSensorOffset(const struct OverTempCal *over_temp_cal, + float vi, size_t index, float *vo); + +/* + * Computes the over-temperature compensated sensor offset estimate based on the + * input model parameters. Note, this is a single axis calculation. + * comp_offset = (temp_sensitivity * temperature + sensor_intercept) + * + * INPUTS: + * temperature: Temperature value at which to compute the estimate. + * temp_sensitivity: Temperature sensitivity model parameter. + * sensor_intercept: Sensor intercept model parameter. + * RETURNS: + * comp_offset: Over-temperature compensated sensor offset estimate. + */ +static float getCompensatedOffset(float temperature, float temp_sensitivity, + float sensor_intercept); + +/////// FUNCTION DEFINITIONS ////////////////////////////////////////////////// + +void overTempCalInit(struct OverTempCal *over_temp_cal, + size_t min_num_model_pts, + uint64_t min_update_interval_nanos, + float delta_temp_per_bin, float max_error_limit, + uint64_t age_limit_nanos, float temp_sensitivity_limit, + float sensor_intercept_limit, bool over_temp_enable) { + ASSERT_NOT_NULL(over_temp_cal); + + // Clears OverTempCal memory. + memset(over_temp_cal, 0, sizeof(struct OverTempCal)); + + // Initializes the pointer to the most recent sensor offset estimate. Sets it + // as the first element in 'model_data'. + over_temp_cal->latest_offset = &over_temp_cal->model_data[0]; + + // Sets the temperature sensitivity model parameters to MODEL_INITIAL_STATE to + // indicate that the model is in an "initial" state. + over_temp_cal->temp_sensitivity[0] = MODEL_INITIAL_STATE; + over_temp_cal->temp_sensitivity[1] = MODEL_INITIAL_STATE; + over_temp_cal->temp_sensitivity[2] = MODEL_INITIAL_STATE; + + // Initializes the model identification parameters. + over_temp_cal->new_overtemp_model_available = false; + over_temp_cal->min_num_model_pts = min_num_model_pts; + over_temp_cal->min_update_interval_nanos = min_update_interval_nanos; + over_temp_cal->delta_temp_per_bin = delta_temp_per_bin; + over_temp_cal->max_error_limit = max_error_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->over_temp_enable = over_temp_enable; + +#ifdef OVERTEMPCAL_DBG_ENABLED + CAL_DEBUG_LOG("[OVER_TEMP_CAL:MEMORY]", "sizeof(struct OverTempCal): %lu", + (unsigned long int)sizeof(struct OverTempCal)); + CAL_DEBUG_LOG("[OVER_TEMP_CAL:INIT]", "Over-Temp Cal: Initialized."); + +#endif // OVERTEMPCAL_DBG_ENABLED +} + +void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, + float offset_temp, uint64_t timestamp_nanos, + const float *temp_sensitivity, + const float *sensor_intercept, bool jump_start_model) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(offset); + ASSERT_NOT_NULL(temp_sensitivity); + ASSERT_NOT_NULL(sensor_intercept); + + // Sets the model parameters if they are within the acceptable limits. + size_t i; + for (i = 0; i < 3; i++) { + if (NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit && + NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) { + over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; + over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; + } + } + + // Sets the model update time to the current timestamp. + over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos; + + // Model "Jump-Start". + const bool model_jump_started = + (jump_start_model) ? jumpStartModelData(over_temp_cal) : false; + + if (!model_jump_started) { + // Sets the initial over-temp calibration estimate and model data. + setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos); + + // Now there is one offset estimate in the model. + over_temp_cal->num_model_pts = 1; + } + +#ifdef OVERTEMPCAL_DBG_ENABLED + // Prints the updated model data. + CAL_DEBUG_LOG("[OVER_TEMP_CAL:RECALL]", + "Model parameters recalled from memory."); + + // Trigger a debug print out to view the new model parameters. + over_temp_cal->debug_print_trigger = true; +#endif // OVERTEMPCAL_DBG_ENABLED +} + +void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, + float *offset_temp, uint64_t *timestamp_nanos, + float *temp_sensitivity, float *sensor_intercept) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + ASSERT_NOT_NULL(offset); + ASSERT_NOT_NULL(offset_temp); + ASSERT_NOT_NULL(timestamp_nanos); + ASSERT_NOT_NULL(temp_sensitivity); + ASSERT_NOT_NULL(sensor_intercept); + + // Updates the latest offset so that it is the one nearest to the current + // temperature. + setNearestEstimate(over_temp_cal); + + // Gets the over-temp calibration estimate and model data. + memcpy(offset, over_temp_cal->latest_offset->offset, 3 * sizeof(float)); + memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, 3 * sizeof(float)); + memcpy(sensor_intercept, over_temp_cal->sensor_intercept, 3 * sizeof(float)); + *offset_temp = over_temp_cal->latest_offset->offset_temp; + *timestamp_nanos = over_temp_cal->latest_offset->timestamp_nanos; +} + +void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, float xi, float yi, + float zi, float *xo, float *yo, float *zo) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + ASSERT_NOT_NULL(xo); + ASSERT_NOT_NULL(yo); + ASSERT_NOT_NULL(zo); + + // Determines whether over-temp compensation will be applied. + if (!over_temp_cal->over_temp_enable) { + return; + } + + // Removes very old data from the collected model estimates (eliminates + // drift-compromised data). Only does this when there is more 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 - over_temp_cal->stale_data_timer) >= + OVERTEMPCAL_STALE_CHECK_TIME_NANOS && + over_temp_cal->num_model_pts > 1) { + over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer. + + if (removeStaleModelData(over_temp_cal, timestamp_nanos)) { + // If anything was removed, then this attempts to recompute the model. + computeModelUpdate(over_temp_cal, timestamp_nanos); + } + } + + // Removes the over-temperature compensated offset from the input sensor data. + removeSensorOffset(over_temp_cal, xi, 0, xo); + removeSensorOffset(over_temp_cal, yi, 1, yo); + removeSensorOffset(over_temp_cal, zi, 2, zo); +} + +bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) { + ASSERT_NOT_NULL(over_temp_cal); + const bool update_available = over_temp_cal->new_overtemp_model_available && + over_temp_cal->over_temp_enable; + + // The 'new_overtemp_model_available' flag is reset when it is read here. + over_temp_cal->new_overtemp_model_available = false; + + return update_available; +} + +void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + const float *offset, + float temperature_celsius) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + ASSERT_NOT_NULL(offset); + ASSERT(over_temp_cal->delta_temp_per_bin > 0); + + // Prevent a divide by zero below. + if (over_temp_cal->delta_temp_per_bin <= 0) { + return; + } + +#ifdef OVERTEMPCAL_DBG_ENABLED + // Updates the total count of offset estimates. + over_temp_cal->debug_num_estimates++; + + // If there are fewer than the minimum number of points to produce a model, + // then trigger a debug printout to view the model building process. + over_temp_cal->debug_print_trigger |= + (over_temp_cal->num_model_pts <= over_temp_cal->min_num_model_pts); +#endif // OVERTEMPCAL_DBG_ENABLED + + // Provides an early escape if this is the first model estimate. + if (over_temp_cal->num_model_pts == 0) { + setLatestEstimate(over_temp_cal, offset, temperature_celsius, + timestamp_nanos); + over_temp_cal->num_model_pts = 1; // one estimate was added above. + return; + } + + // Computes the temperature bin range data. + const int32_t bin_num = + CAL_FLOOR(temperature_celsius / over_temp_cal->delta_temp_per_bin); + const float temp_lo_check = bin_num * over_temp_cal->delta_temp_per_bin; + const float temp_hi_check = (bin_num + 1) * over_temp_cal->delta_temp_per_bin; + + // The rules for accepting new offset estimates into the 'model_data' + // collection: + // 1) The temperature domain is divided into bins each spanning + // 'delta_temp_per_bin'. + // 2) Find and replace the i'th 'model_data' estimate data if: + // Let, bin_num = floor(temperature_celsius / delta_temp_per_bin) + // temp_lo_check = bin_num * delta_temp_per_bin + // temp_hi_check = (bin_num + 1) * delta_temp_per_bin + // Check condition: + // temp_lo_check <= model_data[i].offset_temp < temp_hi_check + bool replaced_one = false; + size_t i = 0; + for (i = 0; i < over_temp_cal->num_model_pts; i++) { + if (over_temp_cal->model_data[i].offset_temp < temp_hi_check && + over_temp_cal->model_data[i].offset_temp >= temp_lo_check) { + // NOTE - the pointer to the estimate that is getting replaced is set + // here; the offset values are set below in the call to + // 'setLatestEstimate'. + over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; + replaced_one = true; + break; + } + } + + // 3) If nothing was replaced, and the 'model_data' buffer is not full + // then add the estimate data to the array. + // 4) Otherwise (nothing was replaced and buffer is full), replace the + // 'latest_offset' with the incoming one. This is done below. + if (!replaced_one && over_temp_cal->num_model_pts < OVERTEMPCAL_MODEL_SIZE) { + // NOTE - the pointer to the next available array location is set here; + // the offset values are set below in the call to 'setLatestEstimate'. + over_temp_cal->latest_offset = + &over_temp_cal->model_data[over_temp_cal->num_model_pts]; + over_temp_cal->num_model_pts++; + } + + // Updates the latest model estimate data. + setLatestEstimate(over_temp_cal, offset, temperature_celsius, + timestamp_nanos); + + // Conditionally updates the over-temp model. See 'computeModelUpdate' for + // update conditions. + computeModelUpdate(over_temp_cal, timestamp_nanos); + +} + +void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + float temperature_celsius) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + +#ifdef OVERTEMPCAL_DBG_ENABLED +#ifdef OVERTEMPCAL_DBG_LOG_TEMP + static uint64_t wait_timer = 0; + // Prints the sensor temperature trajectory for debugging purposes. + // This throttles the print statements. + if ((timestamp_nanos - wait_timer) >= 1000000000) { + wait_timer = timestamp_nanos; // Starts the wait timer. + + // Prints out temperature and the current timestamp. + CAL_DEBUG_LOG("[OVER_TEMP_CAL:TEMP]", + "Temperature|Time [C|nsec] = %s%d.%06d, %llu", + CAL_ENCODE_FLOAT(temperature_celsius, 6), + (unsigned long long int)timestamp_nanos); + } +#endif // OVERTEMPCAL_DBG_LOG_TEMP +#endif // OVERTEMPCAL_DBG_ENABLED + + // Updates the sensor temperature. + over_temp_cal->temperature_celsius = temperature_celsius; + + // If any of the models for the sensor axes are in an initial state, then + // this searches for the sensor offset estimate closest to the current + // temperature. A timer is used to limit the rate at which this search is + // performed. + if (over_temp_cal->num_model_pts > 1 && + (over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE || + over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE || + over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) && + (timestamp_nanos - over_temp_cal->nearest_search_timer) >= + OVERTEMPCAL_NEAREST_NANOS) { + setNearestEstimate(over_temp_cal); + over_temp_cal->nearest_search_timer = timestamp_nanos; // Reset timer. + } +} + +void getModelError(const struct OverTempCal *over_temp_cal, + 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); + ASSERT_NOT_NULL(max_error); + + size_t i; + size_t j; + float max_error_test; + memset(max_error, 0, 3 * sizeof(float)); + + for (i = 0; i < over_temp_cal->num_model_pts; i++) { + for (j = 0; j < 3; j++) { + max_error_test = + NANO_ABS(over_temp_cal->model_data[i].offset[j] - + getCompensatedOffset(over_temp_cal->model_data[i].offset_temp, + temp_sensitivity[j], sensor_intercept[j])); + if (max_error_test > max_error[j]) { + max_error[j] = max_error_test; + } + } + } +} + +/////// LOCAL HELPER FUNCTION DEFINITIONS ///////////////////////////////////// + +void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, + float offset_temp, uint64_t timestamp_nanos) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(offset); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + + // Sets the latest over-temp calibration estimate. + over_temp_cal->latest_offset->offset[0] = offset[0]; + over_temp_cal->latest_offset->offset[1] = offset[1]; + over_temp_cal->latest_offset->offset[2] = offset[2]; + over_temp_cal->latest_offset->offset_temp = offset_temp; + over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos; +} + +void computeModelUpdate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos) { + ASSERT_NOT_NULL(over_temp_cal); + + // The rules for determining whether a new model fit is computed are: + // 1) A minimum number of data points must have been collected: + // num_model_pts >= min_num_model_pts + // NOTE: Collecting 'num_model_pts' and given that only one point is + // kept per temperature bin (spanning a thermal range specified by + // 'delta_temp_per_bin'), implies that model data covers at least, + // model_temp_span >= 'num_model_pts' * delta_temp_per_bin + // 2) New model updates will not occur for intervals less than: + // (current_timestamp_nanos - modelupdate_timestamp_nanos) < + // min_update_interval_nanos + if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts || + (timestamp_nanos - over_temp_cal->modelupdate_timestamp_nanos) < + over_temp_cal->min_update_interval_nanos) { + return; + } + + // Updates the linear model fit. + float temp_sensitivity[3]; + float sensor_intercept[3]; + updateModel(over_temp_cal, temp_sensitivity, sensor_intercept); + + // Computes the maximum error over all of the model data. + float max_error[3]; + getModelError(over_temp_cal, temp_sensitivity, sensor_intercept, max_error); + + // 3) A new set of model parameters are accepted if: + // i. The model fit error is less than, 'max_error_limit'. See + // getModelError() for error metric description. + // ii. 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 + size_t i; + for (i = 0; i < 3; i++) { + if (max_error[i] < over_temp_cal->max_error_limit && + NANO_ABS(temp_sensitivity[i]) < over_temp_cal->temp_sensitivity_limit && + NANO_ABS(sensor_intercept[i]) < over_temp_cal->sensor_intercept_limit) { + over_temp_cal->temp_sensitivity[i] = temp_sensitivity[i]; + over_temp_cal->sensor_intercept[i] = sensor_intercept[i]; + } else { +#ifdef OVERTEMPCAL_DBG_ENABLED + CAL_DEBUG_LOG( + "[OVER_TEMP_CAL:REJECT]", + "Rejected %c-Axis Parameters|Max Error [mdps/C|mdps|mdps] = " + "%s%d.%06d, %s%d.%06d, %s%d.%06d", + kDebugAxisLabel[i], + CAL_ENCODE_FLOAT(temp_sensitivity[i] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(sensor_intercept[i] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(max_error[i] * RAD_TO_MILLI_DEGREES, 6)); +#endif // OVERTEMPCAL_DBG_ENABLED + } + } + + // Resets the timer and sets the update flag. + over_temp_cal->modelupdate_timestamp_nanos = timestamp_nanos; + over_temp_cal->new_overtemp_model_available = true; + + // Track the total number of model updates, and set trigger to print log data. +#ifdef OVERTEMPCAL_DBG_ENABLED + over_temp_cal->debug_num_model_updates++; + over_temp_cal->debug_print_trigger |= true; +#endif // OVERTEMPCAL_DBG_ENABLED +} + +void setNearestEstimate(struct OverTempCal *over_temp_cal) { + ASSERT_NOT_NULL(over_temp_cal); + + size_t i = 0; + float dtemp_new = 0.0f; + float dtemp_old = FLT_MAX; + struct OverTempCalDataPt *nearest_estimate = &over_temp_cal->model_data[0]; + for (i = 1; i < over_temp_cal->num_model_pts; i++) { + dtemp_new = NANO_ABS(over_temp_cal->model_data[i].offset_temp - + over_temp_cal->temperature_celsius); + if (dtemp_new < dtemp_old) { + nearest_estimate = &over_temp_cal->model_data[i]; + dtemp_old = dtemp_new; + } + } + + // Set the 'latest_offset' to the estimate nearest the current temperature. + over_temp_cal->latest_offset = nearest_estimate; +} + +bool removeStaleModelData(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos) { + ASSERT_NOT_NULL(over_temp_cal); + + size_t i; + bool removed_one = false; + for (i = 0; i < over_temp_cal->num_model_pts; i++) { + if ((timestamp_nanos - over_temp_cal->model_data[i].timestamp_nanos) > + over_temp_cal->age_limit_nanos) { + removed_one |= removeModelDataByIndex(over_temp_cal, i); + } + } + + // Updates the latest offset so that it is the one nearest to the current + // temperature. + setNearestEstimate(over_temp_cal); + + return removed_one; +} + +bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, + size_t model_index) { + ASSERT_NOT_NULL(over_temp_cal); + + // This function will not remove all of the model data. At least one model + // sample will be left. + if (over_temp_cal->num_model_pts <= 1) { + return false; + } + + // Remove the model data at 'model_index'. + size_t i; + for (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)); + } + over_temp_cal->num_model_pts--; + +#ifdef OVERTEMPCAL_DBG_ENABLED + CAL_DEBUG_LOG("[OVER_TEMP_CAL:REMOVE]", + "Removed Stale Data: Model Index = %lu", + (unsigned long int)model_index); +#endif // OVERTEMPCAL_DBG_ENABLED + + return true; +} + +bool jumpStartModelData(struct OverTempCal *over_temp_cal) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT(over_temp_cal->delta_temp_per_bin > 0); + + // Prevent a divide by zero below. + if (over_temp_cal->delta_temp_per_bin <= 0) { + return false; + } + + // In normal operation the offset estimates enter into the 'model_data' array + // complete (i.e., x, y, z values are all provided). Therefore, the jumpstart + // data produced here requires that the model parameters have all been fully + // defined (i.e., no models in an initial state) and are all within the valid + // range (this is assumed to have been checked prior to this function). There + // must also be no preexisting model data; that is, this function will not + // replace any actual offset estimates already buffered. + if (over_temp_cal->num_model_pts > 0 || + over_temp_cal->temp_sensitivity[0] >= MODEL_INITIAL_STATE || + over_temp_cal->temp_sensitivity[1] >= MODEL_INITIAL_STATE || + over_temp_cal->temp_sensitivity[2] >= MODEL_INITIAL_STATE) { + return false; + } + + // This defines the minimum contiguous set of points to allow a model update + // when the next offset estimate is received. They are placed at a common + // temperature range that is likely to get replaced with actual data soon. + const int32_t start_bin_num = CAL_FLOOR(JUMPSTART_START_TEMP_CELSIUS / + over_temp_cal->delta_temp_per_bin); + float offset_temp = + start_bin_num * (1.5f * over_temp_cal->delta_temp_per_bin); + + size_t i; + size_t j; + for (i = 0; i < over_temp_cal->min_num_model_pts; i++) { + float offset[3]; + const uint64_t timestamp_nanos = over_temp_cal->modelupdate_timestamp_nanos; + for (j = 0; j < 3; j++) { + offset[j] = + getCompensatedOffset(offset_temp, over_temp_cal->temp_sensitivity[j], + over_temp_cal->sensor_intercept[j]); + } + over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; + setLatestEstimate(over_temp_cal, offset, offset_temp, timestamp_nanos); + offset_temp += over_temp_cal->delta_temp_per_bin; + over_temp_cal->num_model_pts++; + } + +#ifdef OVERTEMPCAL_DBG_ENABLED + if (over_temp_cal->min_num_model_pts > 0) { + CAL_DEBUG_LOG("[OVER_TEMP_CAL]", "Jump-Started Model: #Points = %lu.", + (unsigned long int)over_temp_cal->min_num_model_pts); + } +#endif // OVERTEMPCAL_DBG_ENABLED + + return (over_temp_cal->min_num_model_pts > 0); +} + +void updateModel(const struct OverTempCal *over_temp_cal, + float *temp_sensitivity, float *sensor_intercept) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(temp_sensitivity); + ASSERT_NOT_NULL(sensor_intercept); + ASSERT(over_temp_cal->num_model_pts > 0); + + float st = 0.0f, stt = 0.0f; + float sx = 0.0f, stsx = 0.0f; + float sy = 0.0f, stsy = 0.0f; + float sz = 0.0f, stsz = 0.0f; + const size_t n = over_temp_cal->num_model_pts; + size_t i = 0; + + // First pass computes the mean values. + for (i = 0; i < n; ++i) { + st += over_temp_cal->model_data[i].offset_temp; + sx += over_temp_cal->model_data[i].offset[0]; + sy += over_temp_cal->model_data[i].offset[1]; + sz += over_temp_cal->model_data[i].offset[2]; + } + + // Second pass computes the mean corrected second moment values. + const float inv_n = 1.0f / n; + for (i = 0; i < n; ++i) { + const float t = over_temp_cal->model_data[i].offset_temp - st * inv_n; + stt += t * t; + stsx += t * over_temp_cal->model_data[i].offset[0]; + stsy += t * over_temp_cal->model_data[i].offset[1]; + stsz += t * over_temp_cal->model_data[i].offset[2]; + } + + // Calculates the linear model fit parameters. + ASSERT(stt > 0); + const float inv_stt = 1.0f / stt; + temp_sensitivity[0] = stsx * inv_stt; + sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_n; + temp_sensitivity[1] = stsy * inv_stt; + sensor_intercept[1] = (sy - st * temp_sensitivity[1]) * inv_n; + temp_sensitivity[2] = stsz * inv_stt; + sensor_intercept[2] = (sz - st * temp_sensitivity[2]) * inv_n; +} + +void removeSensorOffset(const struct OverTempCal *over_temp_cal, float vi, + size_t index, float *vo) { + // Removes the over-temperature compensated offset from the input sensor data. + if (over_temp_cal->temp_sensitivity[index] >= MODEL_INITIAL_STATE) { + // If this axis is in its initial state, use the nearest estimate to perform + // the compensation (in this case the latest estimate will be the nearest): + // sensor_out = sensor_in - nearest_offset + *vo = vi - over_temp_cal->latest_offset->offset[index]; + } else { + // sensor_out = sensor_in - compensated_offset + // Where, + // compensated_offset = (temp_sensitivity * temp_meas + sensor_intercept) + *vo = vi - getCompensatedOffset(over_temp_cal->temperature_celsius, + over_temp_cal->temp_sensitivity[index], + over_temp_cal->sensor_intercept[index]); + } +} + +float getCompensatedOffset(float temperature, float temp_sensitivity, + float sensor_intercept) { + return temp_sensitivity * temperature + sensor_intercept; +} + +/////// DEBUG FUNCTION DEFINITIONS //////////////////////////////////////////// + +#ifdef OVERTEMPCAL_DBG_ENABLED +// Debug printout state enumeration. +enum DebugState { + IDLE = 0, + WAIT_STATE, + PRINT_HEADER, + PRINT_OFFSET, + PRINT_SENSITIVITY, + PRINT_INTERCEPT, + PRINT_ERROR, + PRINT_MODEL_PTS, + PRINT_MODEL_DATA +}; + +void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(over_temp_cal->latest_offset); + + static enum DebugState debug_state = IDLE; + static enum DebugState next_state = 0; + static uint64_t wait_timer = 0; + static size_t i = 0; // Counter. + + // NOTE - The un-initialized model state is indicated by + // temp_sensitivity=MODEL_INITIAL_STATE. The following filters out this + // condition for the data printout below. + float compensated_offset[3]; + float temp_sensitivity[3]; + float max_error[3]; + size_t j; + for (j = 0; j < 3; j++) { + temp_sensitivity[j] = + (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE) + ? 0.0f + : over_temp_cal->temp_sensitivity[j]; + } + + // This is a state machine that controls the reporting out of debug data. + switch (debug_state) { + case IDLE: + // Wait for a trigger and start the debug printout sequence. + if (over_temp_cal->debug_print_trigger) { + debug_state = PRINT_HEADER; + CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, ""); + over_temp_cal->debug_print_trigger = false; // Resets trigger. + } else { + debug_state = IDLE; + } + break; + + case PRINT_HEADER: + // Print out header. + CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Debug Version: %s", + OVERTEMPCAL_DEBUG_VERSION_STRING); + + // Prints out number of offsets. + CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Total Offsets = %lu", + (unsigned long int)over_temp_cal->debug_num_estimates); + + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_OFFSET; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_OFFSET: + // Computes the compensated sensor offset based on the current + // temperature. + for (j = 0; j < 3; j++) { + compensated_offset[j] = + (over_temp_cal->temp_sensitivity[j] >= MODEL_INITIAL_STATE) + ? over_temp_cal->latest_offset->offset[j] + : getCompensatedOffset(over_temp_cal->temperature_celsius, + over_temp_cal->temp_sensitivity[j], + over_temp_cal->sensor_intercept[j]); + } + + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, + "Offset|Temp|Time [mdps|C|nsec] = %s%d.%06d, %s%d.%06d, " + "%s%d.%06d, %s%d.%06d, %llu", + CAL_ENCODE_FLOAT(compensated_offset[0] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(compensated_offset[1] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(compensated_offset[2] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(over_temp_cal->temperature_celsius, 6), + (unsigned long long int) + over_temp_cal->latest_offset->timestamp_nanos); + + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_SENSITIVITY; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case WAIT_STATE: + // This helps throttle the print statements. + if ((timestamp_nanos - wait_timer) >= OVERTEMPCAL_WAIT_TIME_NANOS) { + debug_state = next_state; + } + break; + + case PRINT_SENSITIVITY: + // Prints out the modeled temperature sensitivity. + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, + "Modeled Temperature Sensitivity [mdps/C] = %s%d.%06d, %s%d.%06d, " + "%s%d.%06d", + CAL_ENCODE_FLOAT(temp_sensitivity[0] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(temp_sensitivity[1] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(temp_sensitivity[2] * RAD_TO_MILLI_DEGREES, 6)); + + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_INTERCEPT; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_INTERCEPT: + // Prints out the modeled temperature intercept. + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, + "Modeled Temperature Intercept [mdps] = %s%d.%06d, %s%d.%06d, " + "%s%d.%06d", + CAL_ENCODE_FLOAT( + over_temp_cal->sensor_intercept[0] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT( + over_temp_cal->sensor_intercept[1] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT( + over_temp_cal->sensor_intercept[2] * RAD_TO_MILLI_DEGREES, 6)); + + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_ERROR; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_ERROR: + // Computes the maximum error over all of the model data. + if (over_temp_cal->num_model_pts > 0) { + getModelError(over_temp_cal, temp_sensitivity, + over_temp_cal->sensor_intercept, max_error); + + // Reports the resulting model error. + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, + "Model Error [mdps] = %s%d.%06d, %s%d.%06d, %s%d.%06d", + CAL_ENCODE_FLOAT(max_error[0] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(max_error[1] * RAD_TO_MILLI_DEGREES, 6), + CAL_ENCODE_FLOAT(max_error[2] * RAD_TO_MILLI_DEGREES, 6)); + } + + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_MODEL_PTS; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_MODEL_PTS: + // Prints out the number of model points/updates. + CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Points = %lu", + (unsigned long int)over_temp_cal->num_model_pts); + + CAL_DEBUG_LOG(OVERTEMPCAL_REPORT_TAG, "Number of Model Updates = %lu", + (unsigned long int)over_temp_cal->debug_num_model_updates); + + i = 0; // Resets the counter. + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_MODEL_DATA; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + break; + + case PRINT_MODEL_DATA: + // Prints out all of the model data. + if (i < over_temp_cal->num_model_pts) { + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, + " Model[%lu] [mdps|C] = %s%d.%06d, %s%d.%06d, %s%d.%06d, " + "%s%d.%03d ", + (unsigned long int)i, + CAL_ENCODE_FLOAT( + over_temp_cal->model_data[i].offset[0] * RAD_TO_MILLI_DEGREES, + 6), + CAL_ENCODE_FLOAT( + over_temp_cal->model_data[i].offset[1] * RAD_TO_MILLI_DEGREES, + 6), + CAL_ENCODE_FLOAT( + over_temp_cal->model_data[i].offset[2] * RAD_TO_MILLI_DEGREES, + 6), + CAL_ENCODE_FLOAT(over_temp_cal->model_data[i].offset_temp, 3)); + + i++; + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = PRINT_MODEL_DATA; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + } else { + debug_state = IDLE; // Goes to idle state. + CAL_DEBUG_LOG( + OVERTEMPCAL_REPORT_TAG, "Last Model Update [nsec] = %llu", + (unsigned long long int)over_temp_cal->modelupdate_timestamp_nanos); + } + break; + + default: + // Sends this state machine to its idle state. + wait_timer = timestamp_nanos; // Starts the wait timer. + next_state = IDLE; // Sets the next state. + debug_state = WAIT_STATE; // First, go to wait state. + } +} + +#endif // OVERTEMPCAL_DBG_ENABLED diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h new file mode 100644 index 00000000..928576f0 --- /dev/null +++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h @@ -0,0 +1,327 @@ +/* + * 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 provides an online algorithm for compensating a 3-axis sensor's + * offset over its operating temperature: + * + * 1) Estimates of sensor offset with associated temperature are consumed, + * {offset, offset_temperature}. + * 2) A temperature dependence model is extracted from the collected set of + * data pairs. + * 3) Until a "complete" model has been built and a model equation has been + * computed, the compensation will use the collected offset nearest in + * temperature. If a model is available, then the compensation will take + * the form of: + * + * Linear Compensation Model Equation: + * sensor_out = sensor_in - compensated_offset + * Where, + * compensated_offset = (temp_sensitivity * current_temp + sensor_intercept) + * + * NOTE - 'current_temp' is the current measured temperature. 'temp_sensitivity' + * is the modeled temperature sensitivity (i.e., linear slope). + * 'sensor_intercept' is linear model intercept. + * + * Assumptions: + * + * 1) Sensor hysteresis is negligible. + * 2) Sensor offset temperature dependence is sufficiently "linear". + * 3) The impact of long-term offset drift/aging compared to the magnitude of + * deviation resulting from the thermal sensitivity of the offset is + * relatively small. + * + * Sensor Input and Units: + * - General 3-axis sensor data. + * - Temperature measurements [Celsius]. + * + * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + * + * #define OVERTEMPCAL_DBG_ENABLED to enable debug printout statements. + * #define OVERTEMPCAL_DBG_LOG_TEMP to periodically printout sensor temperature. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// Defines the maximum size of the 'model_data' array. +#define OVERTEMPCAL_MODEL_SIZE (20) + +// Over-temperature sensor offset estimate structure. +struct OverTempCalDataPt { + // Sensor offset estimate, temperature, and timestamp. + float offset[3]; + float offset_temp; // [Celsius] + uint64_t timestamp_nanos; // [nanoseconds] + // TODO(davejacobs) - Design option: add variance to provide weighting info. +}; + +// 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[OVERTEMPCAL_MODEL_SIZE]; + + // Total number of model data points collected. + size_t num_model_pts; + + // Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius]. + float temp_sensitivity[3]; + + // Sensor model equation intercept [sensor_units]. + float sensor_intercept[3]; + + // Timestamp of the last model update. + uint64_t modelupdate_timestamp_nanos; // [nanoseconds] + + // The temperature at which the offset compensation is performed. + float temperature_celsius; + + // Pointer to the most recent sensor offset estimate. This is also updated + // periodically to point to the offset estimate closest to the current sensor + // temperature. + struct OverTempCalDataPt *latest_offset; + + ///// Online Model Identification Parameters //////////////////////////////// + // + // The rules for determining whether a new model fit is computed and the + // resulting fit parameters are accepted are: + // 1) A minimum number of data points must have been collected: + // num_model_pts >= min_num_model_pts + // NOTE: Collecting 'num_model_pts' and given that only one point is + // kept per temperature bin (spanning a thermal range specified by + // 'delta_temp_per_bin'), implies that model data covers at least, + // model_temp_span >= 'num_model_pts' * delta_temp_per_bin + // 2) New model updates will not occur for intervals less than: + // (current_timestamp_nanos - modelupdate_timestamp_nanos) < + // min_update_interval_nanos + // 3) A new set of model parameters are accepted if: + // i. The model fit error is less than, 'max_error_limit'. See + // getModelError() for error metric description. + // ii. The model fit parameters must be within certain absolute + // bounds: + // a. ABS(temp_sensitivity) < temp_sensitivity_limit + // b. ABS(sensor_intercept) < sensor_intercept_limit + size_t min_num_model_pts; + uint64_t min_update_interval_nanos; // [nanoseconds] + float max_error_limit; // [sensor units] + float temp_sensitivity_limit; // [sensor units/Celsius] + float sensor_intercept_limit; // [sensor units] + + // The rules for accepting new offset estimates into the 'model_data' + // collection: + // 1) The temperature domain is divided into bins each spanning + // 'delta_temp_per_bin'. + // 2) Find and replace the i'th 'model_data' estimate data if: + // Let, bin_num = floor(current_temp / delta_temp_per_bin) + // temp_lo_check = bin_num * delta_temp_per_bin + // temp_hi_check = (bin_num + 1) * delta_temp_per_bin + // Check condition: + // temp_lo_check <= model_data[i].offset_temp < temp_hi_check + // 3) If nothing was replaced, and the 'model_data' buffer is not full then + // add the sensor offset estimate to the array. + // 4) Otherwise (nothing was replaced and buffer is full), replace the + // 'latest_offset' with the incoming one. + // This approach ensures a uniform spread of collected data, keeps the most + // recent estimates in cases where they arrive frequently near a given + // temperature, and prevents model oversampling (i.e., dominance of estimates + // concentrated at given set of temperatures). + float delta_temp_per_bin; // [Celsius/bin] + + // Timer used to limit the rate at which a search for the nearest offset + // estimate is performed. + uint64_t nearest_search_timer; // [nanoseconds] + + // Timer used to limit the rate at which old estimates are removed from + // the 'model_data' collection. + uint64_t stale_data_timer; // [nanoseconds] + + // Duration beyond which data will be removed to avoid corrupting the model + // with drift-compromised data. + uint64_t age_limit_nanos; // [nanoseconds] + + // Flag set by user to control whether over-temp compensation is used. + bool over_temp_enable; + + // True when new compensation model values have been computed; and reset when + // overTempCalNewModelUpdateAvailable() is called. This variable indicates + // that the following should be stored/updated in persistent system memory: + // 1) 'temp_sensitivity' and 'sensor_intercept'. + // 2) The sensor offset data pointed to by 'latest_offset' + // (saving timestamp information is not required). + bool new_overtemp_model_available; + +#ifdef OVERTEMPCAL_DBG_ENABLED + 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 /////////////////////////////////////////////////// + +/* + * Initializes the over-temp calibration model identification parameters. + * + * INPUTS: + * over_temp_cal: Over-temp main data structure. + * min_num_model_pts: Minimum number of model points per model + * calculation update. + * min_update_interval_nanos: Minimum model update interval. + * delta_temp_per_bin: Temperature span that defines the spacing of + * collected model estimates. + * max_error_limit: Model acceptance fit error tolerance. + * age_limit_nanos: Sets the age limit beyond which a offset + * estimate is removed from 'model_data'. + * 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 + * 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_update_interval_nanos, + float delta_temp_per_bin, float max_error_limit, + uint64_t age_limit_nanos, float temp_sensitivity_limit, + float sensor_intercept_limit, bool over_temp_enable); + +/* + * Sets the over-temp calibration model parameters. + * + * INPUTS: + * over_temp_cal: Over-temp main data structure. + * offset: Update values for the latest offset estimate (array). + * offset_temp: Measured temperature for the offset estimate. + * timestamp_nanos: Timestamp for the offset estimate [nanoseconds]. + * temp_sensitivity: Modeled temperature sensitivity (array). + * sensor_intercept: Linear model intercept for the over-temp model (array). + * jump_start_model: When 'true' populates an empty 'model_data' array using + * valid input model parameters. + * + * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + */ +void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, + float offset_temp, uint64_t timestamp_nanos, + const float *temp_sensitivity, + const float *sensor_intercept, bool jump_start_model); + +/* + * Gets the over-temp calibration model parameters. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * OUTPUTS: + * offset: Offset values for the latest offset estimate (array). + * offset_temp: Measured temperature for the offset estimate. + * timestamp_nanos: Timestamp for the offset estimate [nanoseconds]. + * temp_sensitivity: Modeled temperature sensitivity (array). + * sensor_intercept: Linear model intercept for the over-temp model (array). + * + * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + */ +void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, + float *offset_temp, uint64_t *timestamp_nanos, + float *temp_sensitivity, float *sensor_intercept); + +/* + * Removes the over-temp compensated offset from the input sensor data. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * timestamp_nanos: Timestamp of the sensor estimate update. + * xi, yi, zi: 3-axis sensor data to be compensated. + * OUTPUTS: + * xo, yo, zo: 3-axis sensor data that has been compensated. + */ +void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, float xi, float yi, + float zi, float *xo, float *yo, float *zo); + +// Returns true when a new over-temp model update is available; and the +// 'new_overtemp_model_available' flag is reset. +bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal); + +/* + * Updates the sensor's offset estimate and conditionally assimilates it into + * the over-temp model data set, 'model_data'. + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * timestamp_nanos: Timestamp of the sensor estimate update. + * offset: 3-axis sensor data to be compensated (array). + * temperature_celsius: Measured temperature for the new sensor estimate. + * + * NOTE: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + */ +void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + const float *offset, + float temperature_celsius); + +// 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. +void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + float temperature_celsius); + +/* + * Computes the maximum absolute error between the 'model_data' estimates and + * the estimate determined by the input model parameters. + * max_error (over all i) + * |model_data[i]->offset_xyz - + * getCompensatedOffset(model_data[i]->offset_temp, temp_sensitivity, + * sensor_intercept)| + * + * INPUTS: + * over_temp_cal: Over-temp data structure. + * temp_sensitivity: Model temperature sensitivity to test (array). + * sensor_intercept: Model intercept to test (array). + * OUTPUTS: + * max_error: Maximum absolute error for the candidate model (array). + * + * NOTE 1: Arrays are all 3-dimensional with indices: 0=x, 1=y, 2=z. + * NOTE 2: This function is provided for testing purposes. + */ +void getModelError(const struct OverTempCal *over_temp_cal, + const float *temp_sensitivity, const float *sensor_intercept, + float *max_error); + +#ifdef OVERTEMPCAL_DBG_ENABLED +// This debug printout function assumes the input sensor data is a gyroscope +// [rad/sec]. +void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos); +#endif // OVERTEMPCAL_DBG_ENABLED + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_CAL_H_ diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h index dca59fdf..64f70f8e 100644 --- a/firmware/os/algos/calibration/util/cal_log.h +++ b/firmware/os/algos/calibration/util/cal_log.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -22,16 +19,23 @@ * Logging macros for printing formatted strings to Nanohub. */ /////////////////////////////////////////////////////////////// +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ - -#ifndef LOG_FUNC -#include -#define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__) -#define LOGD_TAG(tag, fmt, ...) \ - LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__) -#endif - -#define CAL_DEBUG_LOG LOGD_TAG +#ifdef GCC_DEBUG_LOG +# include +# define CAL_DEBUG_LOG(tag, fmt, ...) \ + printf("%s " fmt "\n", tag, ##__VA_ARGS__); +#else // GCC_DEBUG_LOG +# include +# ifndef LOG_FUNC +# define LOG_FUNC(level, fmt, ...) osLog(level, fmt, ##__VA_ARGS__) +# endif // LOG_FUNC +# define LOGD_TAG(tag, fmt, ...) \ + LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__) +# define CAL_DEBUG_LOG(tag, fmt, ...) \ + osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__); +#endif // GCC_DEBUG_LOG #ifdef __cplusplus extern "C" { diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c new file mode 100644 index 00000000..9c179ac0 --- /dev/null +++ b/firmware/os/algos/common/math/levenberg_marquardt.c @@ -0,0 +1,270 @@ +#include "common/math/levenberg_marquardt.h" + +#include +#include +#include + +#include "common/math/mat.h" +#include "common/math/vec.h" + +// FORWARD DECLARATIONS +//////////////////////////////////////////////////////////////////////// +static bool checkRelativeStepSize(const float *step, const float *state, + size_t dim, float relative_error_threshold); + +static bool computeResidualAndGradients(ResidualAndJacobianFunction func, + const float *state, const void *f_data, + float *jacobian, + float gradient_threshold, + size_t state_dim, size_t meas_dim, + float *residual, float *gradient, + float *hessian); + +static bool computeStep(const float *gradient, float *hessian, float *L, + float damping_factor, size_t dim, float *step); + +const static float kEps = 1e-10f; + +// FUNCTION IMPLEMENTATIONS +//////////////////////////////////////////////////////////////////////// +void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, + ResidualAndJacobianFunction func) { + ASSERT_NOT_NULL(solver); + ASSERT_NOT_NULL(params); + ASSERT_NOT_NULL(func); + memset(solver, 0, sizeof(struct LmSolver)); + memcpy(&solver->params, params, sizeof(struct LmParams)); + solver->func = func; + solver->num_iter = 0; +} + +void lmSolverDestroy(struct LmSolver *solver) { + (void)solver; +} + +void lmSolverSetData(struct LmSolver *solver, struct LmData *data) { + ASSERT_NOT_NULL(solver); + ASSERT_NOT_NULL(data); + solver->data = data; +} + +enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state, + void *f_data, size_t state_dim, size_t meas_dim, + float *state) { + // Initialize parameters. + float damping_factor = 0.0f; + float v = 2.0f; + + // Check dimensions. + if (meas_dim > MAX_LM_MEAS_DIMENSION || state_dim > MAX_LM_STATE_DIMENSION) { + return INVALID_DATA_DIMENSIONS; + } + + // Check pointers (note that f_data can be null if no additional data is + // required by the error function). + ASSERT_NOT_NULL(solver); + ASSERT_NOT_NULL(initial_state); + ASSERT_NOT_NULL(state); + ASSERT_NOT_NULL(solver->data); + + // Allocate memory for intermediate variables. + float state_new[MAX_LM_STATE_DIMENSION]; + struct LmData *data = solver->data; + + // state = initial_state, num_iter = 0 + memcpy(state, initial_state, sizeof(float) * state_dim); + solver->num_iter = 0; + + // Compute initial cost function gradient and return if already sufficiently + // small to satisfy solution. + if (computeResidualAndGradients(solver->func, state, f_data, data->temp, + solver->params.gradient_threshold, state_dim, + meas_dim, data->residual, + data->gradient, + data->hessian)) { + return GRADIENT_SUFFICIENTLY_SMALL; + } + + // Initialize damping parameter. + damping_factor = solver->params.initial_u_scale * + matMaxDiagonalElement(data->hessian, state_dim); + + // Iterate solution. + for (solver->num_iter = 0; + solver->num_iter < solver->params.max_iterations; + ++solver->num_iter) { + + // Compute new solver step. + if (!computeStep(data->gradient, data->hessian, data->temp, damping_factor, + state_dim, data->step)) { + return CHOLESKY_FAIL; + } + + // If the new step is already sufficiently small, we have a solution. + if (checkRelativeStepSize(data->step, state, state_dim, + solver->params.relative_step_threshold)) { + return RELATIVE_STEP_SUFFICIENTLY_SMALL; + } + + // state_new = state + step. + vecAdd(state_new, state, data->step, state_dim); + + // Compute new cost function residual. + solver->func(state_new, f_data, data->residual_new, NULL); + + // Compute ratio of expected to actual cost function gain for this step. + const float gain_ratio = computeGainRatio(data->residual, + data->residual_new, + data->step, data->gradient, + damping_factor, state_dim, + meas_dim); + + // If gain ratio is positive, the step size is good, otherwise adjust + // damping factor and compute a new step. + if (gain_ratio > 0.0f) { + // Set state to new state vector: state = state_new. + memcpy(state, state_new, sizeof(float) * state_dim); + + // Check if cost function gradient is now sufficiently small, + // in which case we have a local solution. + if (computeResidualAndGradients(solver->func, state, f_data, data->temp, + solver->params.gradient_threshold, + state_dim, meas_dim, data->residual, + data->gradient, data->hessian)) { + return GRADIENT_SUFFICIENTLY_SMALL; + } + + // Update damping factor based on gain ratio. + // Note, this update logic comes from Equation 2.21 in the following: + // [Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff. + // "Methods for non-linear least squares problems." (2004)]. + const float tmp = 2.f * gain_ratio - 1.f; + damping_factor *= NANO_MAX(0.33333f, 1.f - tmp * tmp * tmp); + v = 2.f; + } else { + // Update damping factor and try again. + damping_factor *= v; + v *= 2.f; + } + } + + return HIT_MAX_ITERATIONS; +} + +float computeGainRatio(const float *residual, const float *residual_new, + const float *step, const float *gradient, + float damping_factor, size_t state_dim, + size_t meas_dim) { + // Compute true_gain = residual' residual - residual_new' residual_new. + const float true_gain = vecDot(residual, residual, meas_dim) + - vecDot(residual_new, residual_new, meas_dim); + + // predicted gain = 0.5 * step' * (damping_factor * step + gradient). + float tmp[MAX_LM_STATE_DIMENSION]; + vecScalarMul(tmp, step, damping_factor, state_dim); + vecAddInPlace(tmp, gradient, state_dim); + const float predicted_gain = 0.5f * vecDot(step, tmp, state_dim); + + // Check that we don't divide by zero! If denominator is too small, + // set gain_ratio = 1 to use the current step. + if (predicted_gain < kEps) { + return 1.f; + } + + return true_gain / predicted_gain; +} + +/* + * Tests if a solution is found based on the size of the step relative to the + * current state magnitude. Returns true if a solution is found. + * + * TODO(dvitus): consider optimization of this function to use squared norm + * rather than norm for relative error computation to avoid square root. + */ +bool checkRelativeStepSize(const float *step, const float *state, + size_t dim, float relative_error_threshold) { + // r = eps * (||x|| + eps) + const float relative_error = relative_error_threshold * + (vecNorm(state, dim) + relative_error_threshold); + + // solved if ||step|| <= r + // use squared version of this compare to avoid square root. + return (vecNormSquared(step, dim) <= relative_error * relative_error); +} + +/* + * Computes the residual, f(x), as well as the gradient and hessian of the cost + * function for the given state. + * + * Returns a boolean indicating if the computed gradient is sufficiently small + * to indicate that a solution has been found. + * + * INPUTS: + * state: state estimate (x) for which to compute the gradient & hessian. + * f_data: pointer to parameter data needed for the residual or jacobian. + * jacobian: pointer to temporary memory for storing jacobian. + * Must be at least MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION. + * gradient_threshold: if gradient is below this threshold, function returns 1. + * + * OUTPUTS: + * residual: f(x). + * gradient: - J' f(x), where J = df(x)/dx + * hessian: df^2(x)/dx^2 = J' J + */ +bool computeResidualAndGradients(ResidualAndJacobianFunction func, + const float *state, const void *f_data, + float *jacobian, float gradient_threshold, + size_t state_dim, size_t meas_dim, + float *residual, float *gradient, + float *hessian) { + // Compute residual and Jacobian. + ASSERT_NOT_NULL(state); + ASSERT_NOT_NULL(residual); + ASSERT_NOT_NULL(gradient); + ASSERT_NOT_NULL(hessian); + func(state, f_data, residual, jacobian); + + // Compute the cost function hessian = jacobian' jacobian and + // gradient = -jacobian' residual + matTransposeMultiplyMat(hessian, jacobian, meas_dim, state_dim); + matTransposeMultiplyVec(gradient, jacobian, residual, meas_dim, state_dim); + vecScalarMulInPlace(gradient, -1.f, state_dim); + + // Check if solution is found (cost function gradient is sufficiently small). + return (vecMaxAbsoluteValue(gradient, state_dim) < gradient_threshold); +} + +/* + * Computes the Levenberg-Marquardt solver step to satisfy the following: + * (J'J + uI) * step = - J' f + * + * INPUTS: + * gradient: -J'f + * hessian: J'J + * L: temp memory of at least MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION. + * damping_factor: u + * dim: state dimension + * + * OUTPUTS: + * step: solution to the above equation. + * Function returns false if the solution fails (due to cholesky failure), + * otherwise returns true. + * + * Note that the hessian is modified in this function in order to reduce + * local memory requirements. + */ +bool computeStep(const float *gradient, float *hessian, float *L, + float damping_factor, size_t dim, float *step) { + + // 1) A = hessian + damping_factor * Identity. + matAddConstantDiagonal(hessian, damping_factor, dim); + + // 2) Solve A * step = gradient for step. + // a) compute cholesky decomposition of A = L L^T. + if (!matCholeskyDecomposition(L, hessian, dim)) { + return false; + } + + // b) solve for step via back-solve. + return matLinearSolveCholesky(step, L, gradient, dim); +} diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h new file mode 100644 index 00000000..2ffc05f3 --- /dev/null +++ b/firmware/os/algos/common/math/levenberg_marquardt.h @@ -0,0 +1,163 @@ +/* + * This module contains the definition of a Levenberg-Marquardt solver for + * non-linear least squares problems of the following form: + * + * arg min ||f(X)|| = arg min f(X)^T f(X) + * X X + * + * where X is an Nx1 state vector and f(X) is an Mx1 nonlinear measurement error + * function of X which we wish to minimize the norm of. + * + * Levenberg-Marquardt solves the above problem through a damped Gauss-Newton + * method where the solver step on each iteration is chosen such that: + * (J' J + uI) * step = - J' f(x) + * where J = df(x)/dx is the Jacobian of the error function, u is a damping + * factor, and I is the identity matrix. + * + * The algorithm implemented here follows Algorithm 3.16 outlined in this paper: + * Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff. + * "Methods for non-linear least squares problems." (2004). + * + * This algorithm uses a variant of the Marquardt method for updating the + * damping factor which ensures that changes in the factor have no + * discontinuities or fluttering behavior between solver steps. + */ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +// Function pointer for computing residual, f(X), and Jacobian, J = df(X)/dX +// for a given state value, X, and other parameter data needed for computing +// these terms, f_data. +// +// Note if f_data is not needed, it is allowable for f_data to be passed in +// as NULL. +// +// jacobian is also allowed to be passed in as NULL, and in this case only the +// residual will be computed and returned. +typedef void (*ResidualAndJacobianFunction)(const float *state, + const void *f_data, + float *residual, float *jacobian); + + +#define MAX_LM_STATE_DIMENSION (10) +#define MAX_LM_MEAS_DIMENSION (20) + +// Structure containing fixed parameters for a single LM solve. +struct LmParams { + // maximum number of iterations allowed by the solver. + uint32_t max_iterations; + + // initial scaling factor for setting the damping factor, i.e.: + // damping_factor = initial_u_scale * max(J'J). + float initial_u_scale; + + // magnitude of the cost function gradient required for solution, i.e. + // max(gradient) = max(J'f(x)) < gradient_threshold. + float gradient_threshold; + + // magnitude of relative error required for solution step, i.e. + // ||step|| / ||state|| < relative_step_thresold. + float relative_step_threshold; +}; + +// Structure containing data needed for a single LM solve. +// Defining the data here allows flexibility in how the memory is allocated +// for the solver. +struct LmData { + float step[MAX_LM_STATE_DIMENSION]; + float residual[MAX_LM_MEAS_DIMENSION]; + float residual_new[MAX_LM_MEAS_DIMENSION]; + float gradient[MAX_LM_MEAS_DIMENSION]; + float hessian[MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION]; + float temp[MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION]; +}; + +// Enumeration indicating status of the LM solver. +enum LmStatus { + RUNNING = 0, + // Successful solve conditions: + RELATIVE_STEP_SUFFICIENTLY_SMALL, // solver step is sufficiently small. + GRADIENT_SUFFICIENTLY_SMALL, // cost function gradient is below threshold. + HIT_MAX_ITERATIONS, + + // Solver failure modes: + CHOLESKY_FAIL, + INVALID_DATA_DIMENSIONS, +}; + +// Structure for containing variables needed for a Levenberg-Marquardt solver. +struct LmSolver { + // Solver parameters. + struct LmParams params; + + // Pointer to solver data. + struct LmData *data; + + // Function for computing residual (f(x)) and jacobian (df(x)/dx). + ResidualAndJacobianFunction func; + + // Number of iterations in current solution. + uint32_t num_iter; +}; + +// Initializes LM solver with provided parameters and error function. +void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, + ResidualAndJacobianFunction error_func); + +void lmSolverDestroy(struct LmSolver *solver); + +// Sets pointer for temporary data needed for an individual LM solve. +// Note, this must be called prior to calling lmSolverSolve(). +void lmSolverSetData(struct LmSolver *solver, struct LmData *data); + +/* + * Runs the LM solver for a given set of data and error function. + * + * INPUTS: + * solver : pointer to an struct LmSolver structure. + * initial_state : initial guess of the estimation state. + * f_data : pointer to additional data needed by the error function. + * state_dim : dimension of X. + * meas_dim : dimension of f(X), defined in the error function. + * + * OUTPUTS: + * LmStatus : enum indicating state of the solver on completion. + * est_state : estimated state. + */ +enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state, + void *f_data, size_t state_dim, size_t meas_dim, + float *est_state); + +////////////////////////// TEST UTILITIES //////////////////////////////////// +// This function is exposed here for testing purposes only. +/* + * Computes the ratio of actual cost function gain over expected cost + * function gain for the given solver step. This ratio is used to adjust + * the solver step size for the next solver iteration. + * + * INPUTS: + * residual: f(x) for the current state x. + * residual_new: f(x + step) for the new state after the solver step. + * step: the solver step. + * gradient: gradient of the cost function F(x). + * damping_factor: the current damping factor used in the solver. + * state_dim: dimension of the state, x. + * meas_dim: dimension of f(x). + */ +float computeGainRatio(const float *residual, const float *residual_new, + const float *step, const float *gradient, + float damping_factor, size_t state_dim, + size_t meas_dim); + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_ diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c index 8f4270b5..5ab66254 100644 --- a/firmware/os/algos/common/math/mat.c +++ b/firmware/os/algos/common/math/mat.c @@ -20,15 +20,28 @@ #include #include #include +#include #include -#include -#define kEps 1E-5 +#define EPSILON 1E-5 +#define CHOLESKY_TOLERANCE 1E-6 -void initZeroMatrix(struct Mat33 *A) { memset(A->elem, 0.0f, sizeof(A->elem)); } +// Forward declarations. +static void mat33SwapRows(struct Mat33 *A, uint32_t i, uint32_t j); +static uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k); +static void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, + uint32_t l, uint32_t i, uint32_t j); + +static void mat44SwapRows(struct Mat44 *A, uint32_t i, uint32_t j); + +void initZeroMatrix(struct Mat33 *A) { + ASSERT_NOT_NULL(A); + memset(A->elem, 0.0f, sizeof(A->elem)); +} UNROLLED void initDiagonalMatrix(struct Mat33 *A, float x) { + ASSERT_NOT_NULL(A); initZeroMatrix(A); uint32_t i; @@ -39,6 +52,10 @@ void initDiagonalMatrix(struct Mat33 *A, float x) { void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, const struct Vec3 *v2, const struct Vec3 *v3) { + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(v1); + ASSERT_NOT_NULL(v2); + ASSERT_NOT_NULL(v3); A->elem[0][0] = v1->x; A->elem[0][1] = v2->x; A->elem[0][2] = v3->x; @@ -53,7 +70,9 @@ void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, } void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) { - // assert(out != v); + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(v); out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z; out->y = A->elem[1][0] * v->x + A->elem[1][1] * v->y + A->elem[1][2] * v->z; out->z = A->elem[2][0] * v->x + A->elem[2][1] * v->y + A->elem[2][2] * v->z; @@ -62,8 +81,11 @@ void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) { UNROLLED void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - // assert(out != A); - // assert(out != B); + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(B); + ASSERT(out != A); + ASSERT(out != B); uint32_t i; for (i = 0; i < 3; ++i) { @@ -82,6 +104,7 @@ void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33ScalarMul(struct Mat33 *A, float c) { + ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -93,6 +116,8 @@ void mat33ScalarMul(struct Mat33 *A, float c) { UNROLLED void mat33Add(struct Mat33 *out, const struct Mat33 *A) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -104,6 +129,8 @@ void mat33Add(struct Mat33 *out, const struct Mat33 *A) { UNROLLED void mat33Sub(struct Mat33 *out, const struct Mat33 *A) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -115,6 +142,7 @@ void mat33Sub(struct Mat33 *out, const struct Mat33 *A) { UNROLLED int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) { + ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { if (A->elem[i][i] < 0.0f) { @@ -137,9 +165,11 @@ int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) { UNROLLED void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - // assert(out != A); - // assert(out != B); - + ASSERT(out != A); + ASSERT(out != B); + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(B); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -158,9 +188,11 @@ void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - // assert(out != A); - // assert(out != B); - + ASSERT(out != A); + ASSERT(out != B); + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(B); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -178,6 +210,8 @@ void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); float t; initDiagonalMatrix(out, 1.0f); @@ -204,7 +238,11 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { out->elem[swap][k] = t; } } - + // divide by zero guard. + ASSERT(fabs(tmp.elem[i][i]) > 0); + if(!(fabs(tmp.elem[i][i]) > 0)) { + return; + } t = 1.0f / tmp.elem[i][i]; for (k = 0; k < 3; ++k) { tmp.elem[i][k] *= t; @@ -225,6 +263,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { UNROLLED void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -234,43 +274,9 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) { } } -UNROLLED -void mat33DecomposeLup(struct Mat33 *LU, struct Size3 *pivot) { - const uint32_t N = 3; - uint32_t i, j, k; - - for (k = 0; k < N; ++k) { - pivot->elem[k] = k; - float max = fabsf(LU->elem[k][k]); - for (j = k + 1; j < N; ++j) { - if (max < fabsf(LU->elem[j][k])) { - max = fabsf(LU->elem[j][k]); - pivot->elem[k] = j; - } - } - - if (pivot->elem[k] != k) { - mat33SwapRows(LU, k, pivot->elem[k]); - } - - if (fabsf(LU->elem[k][k]) < kEps) { - continue; - } - - for (j = k + 1; j < N; ++j) { - LU->elem[k][j] /= LU->elem[k][k]; - } - - for (i = k + 1; i < N; ++i) { - for (j = k + 1; j < 3; ++j) { - LU->elem[i][j] -= LU->elem[i][k] * LU->elem[k][j]; - } - } - } -} - UNROLLED void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) { + ASSERT_NOT_NULL(A); const uint32_t N = 3; uint32_t k; @@ -285,50 +291,12 @@ void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) { } } -UNROLLED -void mat33Solve(const struct Mat33 *A, struct Vec3 *x, const struct Vec3 *b, - const struct Size3 *pivot) { - const uint32_t N = 3; - uint32_t i, k; - - float bCopy[N]; - bCopy[0] = b->x; - bCopy[1] = b->y; - bCopy[2] = b->z; - - float _x[N]; - for (k = 0; k < N; ++k) { - if (pivot->elem[k] != k) { - float tmp = bCopy[k]; - bCopy[k] = bCopy[pivot->elem[k]]; - bCopy[pivot->elem[k]] = tmp; - } - - _x[k] = bCopy[k]; - for (i = 0; i < k; ++i) { - _x[k] -= _x[i] * A->elem[k][i]; - } - _x[k] /= A->elem[k][k]; - } - - for (k = N; k-- > 0;) { - for (i = k + 1; i < N; ++i) { - _x[k] -= _x[i] * A->elem[k][i]; - } - } - - initVec3(x, _x[0], _x[1], _x[2]); -} - -/* Returns the eigenvalues and corresponding eigenvectors of the _symmetric_ - matrix. - The i-th eigenvalue corresponds to the eigenvector in the i-th _row_ of - "eigenvecs". - */ - UNROLLED void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, struct Mat33 *eigenvecs) { + ASSERT_NOT_NULL(S); + ASSERT_NOT_NULL(eigenvals); + ASSERT_NOT_NULL(eigenvecs); const uint32_t N = 3; uint32_t i, j, k, l, m; @@ -354,7 +322,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, l = ind[m]; float p = S->elem[k][l]; - if (fabsf(p) < kEps) { + if (fabsf(p) < EPSILON) { break; } @@ -405,7 +373,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, } } - if (sum < kEps) { + if (sum < EPSILON) { break; } } @@ -433,6 +401,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, // index of largest off-diagonal element in row k UNROLLED uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) { + ASSERT_NOT_NULL(A); const uint32_t N = 3; uint32_t m = k + 1; @@ -449,13 +418,16 @@ uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) { void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l, uint32_t i, uint32_t j) { + ASSERT_NOT_NULL(A); float tmp = c * A->elem[k][l] - s * A->elem[i][j]; A->elem[i][j] = s * A->elem[k][l] + c * A->elem[i][j]; A->elem[k][l] = tmp; } void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) { - // assert(out != v); + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(v); out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z + A->elem[0][3] * v->w; @@ -472,6 +444,8 @@ void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) { UNROLLED void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) { + ASSERT_NOT_NULL(LU); + ASSERT_NOT_NULL(pivot); const uint32_t N = 4; uint32_t i, j, k; @@ -489,7 +463,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) { mat44SwapRows(LU, k, pivot->elem[k]); } - if (fabsf(LU->elem[k][k]) < kEps) { + if (fabsf(LU->elem[k][k]) < EPSILON) { continue; } @@ -507,6 +481,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) { UNROLLED void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) { + ASSERT_NOT_NULL(A); const uint32_t N = 4; uint32_t k; @@ -524,6 +499,10 @@ void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) { UNROLLED void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, const struct Size4 *pivot) { + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(x); + ASSERT_NOT_NULL(b); + ASSERT_NOT_NULL(pivot); const uint32_t N = 4; uint32_t i, k; @@ -556,3 +535,148 @@ void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, initVec4(x, _x[0], _x[1], _x[2], _x[3]); } + +float matMaxDiagonalElement(const float *square_mat, size_t n) { + ASSERT_NOT_NULL(square_mat); + ASSERT(n > 0); + size_t i; + float max = square_mat[0]; + const size_t n_square = n * n; + const size_t offset = n + 1; + for (i = offset; i < n_square; i += offset) { + if (square_mat[i] > max) { + max = square_mat[i]; + } + } + return max; +} + +void matAddConstantDiagonal(float *square_mat, float u, size_t n) { + ASSERT_NOT_NULL(square_mat); + size_t i; + const size_t n_square = n * n; + const size_t offset = n + 1; + for (i = 0; i < n_square; i += offset) { + square_mat[i] += u; + } +} + +void matTransposeMultiplyMat(float *out, const float *A, + size_t nrows, size_t ncols) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + size_t i; + size_t j; + size_t k; + memset(out, 0, sizeof(float) * ncols * ncols); + for (i = 0; i < ncols; ++i) { + for (j = 0; j < ncols; ++j) { + // Since A' * A is symmetric, can use upper diagonal elements + // to fill in the lower diagonal without recomputing. + if (j < i) { + out[i * ncols + j] = out[j * ncols + i]; + } else { + // mat_out[i, j] = ai ' * aj + out[i * ncols + j] = 0; + for (k = 0; k < nrows; ++k) { + out[i * ncols + j] += A[k * ncols + i] * + A[k * ncols + j]; + } + } + } + } +} + +void matMultiplyVec(float *out, const float *A, const float *v, + size_t nrows, size_t ncols) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(v); + size_t i; + for (i = 0; i < nrows; ++i) { + const float *row = &A[i * ncols]; + out[i] = vecDot(row, v, ncols); + } +} + +void matTransposeMultiplyVec(float *out, const float *A, const float *v, + size_t nrows, size_t ncols) { + ASSERT_NOT_NULL(out); + ASSERT_NOT_NULL(A); + ASSERT_NOT_NULL(v); + size_t i, j; + for (i = 0; i < ncols; ++i) { + out[i] = 0; + for (j = 0; j < nrows; ++j) { + out[i] += A[j * ncols + i] * v[j]; + } + } +} + +bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n) { + ASSERT_NOT_NULL(x); + ASSERT_NOT_NULL(L); + ASSERT_NOT_NULL(b); + int32_t i, j; // Loops below require signed integers. + float sum = 0.0f; + // 1. Solve Ly = b through forward substitution. Use x[] to store y. + for (i = 0; i < n; ++i) { + sum = 0.0f; + for (j = 0; j < i; ++j) { + sum += L[i * n + j] * x[j]; + } + // Check for non-zero diagonals (don't divide by zero). + if (L[i * n + i] < EPSILON) { + return false; + } + x[i] = (b[i] - sum) / L[i * n + i]; + } + + // 2. Solve L'x = y through backwards substitution. Use x[] to store both + // y and x. + for (i = n - 1; i >= 0; --i) { + sum = 0.0f; + for (j = i + 1; j < n; ++j) { + sum += L[j * n + i] * x[j]; + } + x[i] = (x[i] - sum) / L[i * n + i]; + } + + return true; +} + +bool matCholeskyDecomposition(float *L, const float *A, size_t n) { + ASSERT_NOT_NULL(L); + ASSERT_NOT_NULL(A); + size_t i, j, k; + float sum = 0.0f; + // initialize L to zero. + memset(L, 0, sizeof(float) * n * n); + + for (i = 0; i < n; ++i) { + // compute L[i][i] = sqrt(A[i][i] - sum_k = 1:i-1 L^2[i][k]) + sum = 0.0f; + for (k = 0; k < i; ++k) { + sum += L[i * n + k] * L[i * n + k]; + } + sum = A[i * n + i] - sum; + // If diagonal element of L is too small, cholesky fails. + if (sum < CHOLESKY_TOLERANCE) { + return false; + } + L[i * n + i] = sqrtf(sum); + + // for j = i+1:N, compute L[j][i] = + // (1/L[i][i]) * (A[i][j] - sum_k = 1:i-1 L[i][k] * L[j][k]) + for (j = i + 1; j < n; ++j) { + sum = 0.0f; + for (k = 0; k < i; ++k) { + sum += L[i * n + k] * L[j * n + k]; + } + // division okay because magnitude of L[i][i] already checked above. + L[j * n + i] = (A[i * n + j] - sum) / L[i * n + i]; + } + } + + return true; +} diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h index f8aad436..13494f55 100644 --- a/firmware/os/algos/common/math/mat.h +++ b/firmware/os/algos/common/math/mat.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -16,9 +13,28 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +///////////////////////////////////////////////////////////////////////// +/* + * This module contains matrix math utilities for the following datatypes: + * -) Mat33 structures for 3x3 dimensional matrices + * -) Mat44 structures for 4x4 dimensional matrices + * -) floating point arrays for NxM dimensional matrices. + * + * Note that the Mat33 and Mat44 utilities were ported from the Android + * repository and maintain dependencies in that separate codebase. As a + * result, the function signatures were left untouched for compatibility with + * this legacy code, despite certain style violations. In particular, for this + * module the function argument ordering is outputs before inputs. This style + * violation will be addressed once the full set of dependencies in Android + * have been brought into this repository. + */ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MAT_H_ +#include +#include #include -#include + #include "common/math/vec.h" #ifdef __cplusplus @@ -41,60 +57,172 @@ struct Size4 { uint32_t elem[4]; }; +// 3x3 MATRIX MATH ///////////////////////////////////////////////////////////// void initZeroMatrix(struct Mat33 *A); + +// Updates A with the value x on the main diagonal and 0 on the off diagonals, +// i.e.: +// A = [x 0 0 +// 0 x 0 +// 0 0 x] void initDiagonalMatrix(struct Mat33 *A, float x); +// Updates A such that the columns are given by the provided vectors, i.e.: +// A = [v1 v2 v3]. void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, const struct Vec3 *v2, const struct Vec3 *v3); +// Updates out with the multiplication of A with v, i.e.: +// out = A v. void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v); + +// Updates out with the multiplication of A with B, i.e.: +// out = A B. void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B); + +// Updates A by scaling all entries by the provided scalar c, i.e.: +// A = A c. void mat33ScalarMul(struct Mat33 *A, float c); +// Updates out by adding A to out, i.e.: +// out = out + A. void mat33Add(struct Mat33 *out, const struct Mat33 *A); + +// Updates out by subtracting A from out, i.e.: +// out = out - A. void mat33Sub(struct Mat33 *out, const struct Mat33 *A); +// Returns 1 if the minimum eigenvalue of the matrix A is greater than the +// given tolerance. Note that the tolerance is assumed to be greater than 0. +// I.e., returns: 1[min(eig(A)) > tolerance]. +// NOTE: this function currently only checks matrix symmetry and positivity +// of the diagonals which is insufficient for testing positive semidefinite. int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance); +// Updates out with the inverse of the matrix A, i.e.: // out = A^(-1) void mat33Invert(struct Mat33 *out, const struct Mat33 *A); +// Updates out with the multiplication of A's transpose with B, i.e.: // out = A^T B void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B); +// Updates out with the multiplication of A with B's transpose, i.e.: // out = A B^T void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B); +// Updates out with the transpose of A, i.e.: // out = A^T void mat33Transpose(struct Mat33 *out, const struct Mat33 *A); -void mat33DecomposeLup(struct Mat33 *LU, struct Size3 *pivot); - -void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j); - -void mat33Solve(const struct Mat33 *A, struct Vec3 *x, const struct Vec3 *b, - const struct Size3 *pivot); - +// Returns the eigenvalues and corresponding eigenvectors of the symmetric +// matrix S. +// The i-th eigenvalue corresponds to the eigenvector in the i-th row of +// the matrix eigenvecs. void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, struct Mat33 *eigenvecs); -uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k); - -void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l, - uint32_t i, uint32_t j); +// 4x4 MATRIX MATH ///////////////////////////////////////////////////////////// +// Updates out with the multiplication of A and v, i.e.: +// out = Av. void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v); +// Decomposes the given matrix LU inplace, such that: +// LU = P' * L * U. +// where L is a lower-diagonal matrix, U is an upper-diagonal matrix, and P is a +// permutation matrix. +// +// L and U are stored compactly in the returned LU matrix such that: +// -) the superdiagonal elements make up "U" (with a diagonal of 1.0s), +// -) the subdiagonal and diagonal elements make up "L". +// e.g. if the returned LU matrix is: +// LU = [A11 A12 A13 A14 +// A21 A22 A23 A24 +// A31 A32 A33 A34 +// A41 A42 A43 A44], then: +// L = [A11 0 0 0 and U = [ 1 A12 A13 A14 +// A21 A22 0 0 0 1 A23 A24 +// A31 A32 A33 0 0 0 1 A34 +// A41 A42 A43 A44] 0 0 0 1 ] +// +// The permutation matrix P can be reproduced from returned pivot vector as: +// matrix P(N); +// P.identity(); +// for (size_t i = 0; i < N; ++i) { +// P.swapRows(i, pivot[i]); +// } void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot); -void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j); - +// Solves the linear system A x = b for x, where A is a compact LU decomposition +// (i.e. the LU matrix from mat44DecomposeLup) and pivot is the corresponding +// row pivots for the permutation matrix (also from mat44DecomposeLup). void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, const struct Size4 *pivot); +// MXN MATRIX MATH ///////////////////////////////////////////////////////////// +/* + * The following functions define basic math functionality for matrices of + * arbitrary dimension. + * + * All matrices used in these functions are assumed to be row major, i.e. if: + * A = [1 2 3 + * 4 5 6 + * 7 8 9] + * then when A is passed into one of the functions below, the order of + * elements is assumed to be [1 2 3 4 5 6 7 8 9]. + */ + +// Returns the maximum diagonal element of the given matrix. +// The matrix is assumed to be square, of size n x n. +float matMaxDiagonalElement(const float *square_mat, size_t n); + +// Adds a constant value to the diagonal of the given square n x n matrix and +// returns the updated matrix in place: +// A = A + uI +void matAddConstantDiagonal(float *square_mat, float u, size_t n); + +// Updates out with the result of A's transpose multiplied with A (i.e. A^T A). +// A is a matrix with dimensions nrows x ncols. +// out is a matrix with dimensions ncols x ncols. +void matTransposeMultiplyMat(float *out, const float *A, + size_t nrows, size_t ncols); + +// Updates out with the result of A's transpose multiplied with v (i.e. A^T v). +// A is a matrix with dimensions nrows x ncols. +// v is a vector of dimension nrows. +// out is a vector of dimension ncols. +void matTransposeMultiplyVec(float* out, const float *A, const float *v, + size_t nrows, size_t ncols); + +// Updates out with the result of A multiplied with v (i.e. out = Av). +// A is a matrix with dimensions nrows x ncols. +// v is a vector of dimension ncols. +// out is a vector of dimension nrows. +void matMultiplyVec(float *out, const float *A, const float *v, + size_t nrows, size_t ncols); + +// Solves the linear system L L^T x = b for x, where L is a lower diagonal, +// symmetric matrix, i.e. the Cholesky factor of a matrix A = L L^T. +// L is a lower-diagonal matrix of dimension n x n. +// b is a vector of dimension n. +// x is a vector of dimension n. +// Returns true if the solver succeeds. +bool matLinearSolveCholesky(float *x, const float *L, const float *b, + size_t n); + +// Performs the Cholesky decomposition on the given matrix A such that: +// A = L L^T, where L, the Cholesky factor, is a lower diagonal matrix. +// Updates the provided L matrix with the Cholesky factor. +// This decomposition is only successful for symmetric, positive definite +// matrices A. +// Returns true if the solver succeeds (will fail if the matrix is not +// symmetric, positive definite). +bool matCholeskyDecomposition(float *L, const float *A, size_t n); + #ifdef __cplusplus } #endif diff --git a/firmware/os/algos/common/math/quat.h b/firmware/os/algos/common/math/quat.h index f23fce4b..3b6937b1 100644 --- a/firmware/os/algos/common/math/quat.h +++ b/firmware/os/algos/common/math/quat.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -16,6 +13,8 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_QUAT_H_ #include "common/math/mat.h" #include "common/math/vec.h" diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c index 85533cac..97b2b8cb 100644 --- a/firmware/os/algos/common/math/vec.c +++ b/firmware/os/algos/common/math/vec.c @@ -19,6 +19,9 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX, float *outY, float *outZ) { + ASSERT_NOT_NULL(outX); + ASSERT_NOT_NULL(outY); + ASSERT_NOT_NULL(outZ); float x, y, z; // discard the one with the smallest absolute value @@ -37,9 +40,95 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX, } float magSquared = x * x + y * y + z * z; - float invMag = 1.0f / sqrtf(magSquared); - + ASSERT(magSquared > 0); + // Only set invMag if magSquared is non-zero. + float invMag = 1.0f; + if (magSquared > 0) { + invMag = 1.0f / sqrtf(magSquared); + } *outX = x * invMag; *outY = y * invMag; *outZ = z * invMag; } + +void vecAdd(float *u, const float *v, const float *w, int dim) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + int i; + for (i = 0; i < dim; i++) { + u[i] = v[i] + w[i]; + } +} + +void vecAddInPlace(float *v, const float *w, int dim) { + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + int i; + for (i = 0; i < dim; i++) { + v[i] += w[i]; + } +} + +void vecSub(float *u, const float *v, const float *w, int dim) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + int i; + for (i = 0; i < dim; i++) { + u[i] = v[i] - w[i]; + } +} + +void vecScalarMul(float *u, const float *v, float c, int dim) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + int i; + for (i = 0; i < dim; i++) { + u[i] = c * v[i]; + } +} + +void vecScalarMulInPlace(float *u, float c, int dim) { + ASSERT_NOT_NULL(u); + int i; + for (i = 0; i < dim; i++) { + u[i] *= c; + } +} + +float vecNorm(const float *v, int dim) { + ASSERT_NOT_NULL(v); + float norm_sq = vecNormSquared(v, dim); + return sqrtf(norm_sq); +} + +float vecNormSquared(const float *v, int dim) { + ASSERT_NOT_NULL(v); + return vecDot(v, v, dim); +} + +float vecDot(const float *v, const float *w, int dim) { + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + int i; + float result = 0; + for (i = 0; i < dim; ++i) { + result += v[i] * w[i]; + } + return result; +} + +float vecMaxAbsoluteValue(const float *v, int dim) { + ASSERT_NOT_NULL(v); + float max = NANO_ABS(v[0]); + float tmp; + int i; + for (i = 1; i < dim; ++i) { + tmp = NANO_ABS(v[i]); + if(tmp > max) { + max = tmp; + } + } + return max; +} diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h index cb5f08dd..77ab492d 100644 --- a/firmware/os/algos/common/math/vec.h +++ b/firmware/os/algos/common/math/vec.h @@ -1,6 +1,3 @@ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_ - /* * Copyright (C) 2016 The Android Open Source Project * @@ -17,7 +14,27 @@ * limitations under the License. */ +///////////////////////////////////////////////////////////////////////// +/* + * This module contains vector math utilities for the following datatypes: + * -) Vec3 structures for 3-dimensional vectors + * -) Vec4 structures for 4-dimensional vectors + * -) floating point arrays for N-dimensional vectors. + * + * Note that the Vec3 and Vec4 utilties were ported from the Android + * repository and maintain dependenices in that separate codebase. As a + * result, the function signatures were left untouched for compatibility with + * this legacy code, despite certain style violations. In particular, for this + * module the function argument ordering is outputs before inputs. This style + * violation will be addressed once the full set of dependencies in Android + * have been brought into this repository. + */ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_VEC_H_ + #include +#include +#include "util/nano_assert.h" #ifdef __cplusplus extern "C" { @@ -31,66 +48,147 @@ struct Vec4 { float x, y, z, w; }; +#ifndef NANO_ABS +#define NANO_ABS(x) ((x) > 0 ? (x) : -(x)) +#endif + +#ifndef NANO_MAX +#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b) +#endif + +// 3-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// static inline void initVec3(struct Vec3 *v, float x, float y, float z) { + ASSERT_NOT_NULL(v); v->x = x; v->y = y; v->z = z; } +// Updates v as the sum of v and w. static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) { + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); v->x += w->x; v->y += w->y; v->z += w->z; } +// Updates v as the subtraction of w from v. static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); v->x -= w->x; v->y -= w->y; v->z -= w->z; } +// Scales v by the scalar c, i.e. v = c * v. static inline void vec3ScalarMul(struct Vec3 *v, float c) { + ASSERT_NOT_NULL(v); v->x *= c; v->y *= c; v->z *= c; } +// Returns the dot product of v and w. static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) { + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); return v->x * w->x + v->y * w->y + v->z * w->z; } +// Returns the square of the L2-norm of the given vector. static inline float vec3NormSquared(const struct Vec3 *v) { + ASSERT_NOT_NULL(v); return vec3Dot(v, v); } +// Returns the L2-norm of the given vector. static inline float vec3Norm(const struct Vec3 *v) { + ASSERT_NOT_NULL(v); return sqrtf(vec3NormSquared(v)); } +// Normalizes the provided vector to unit norm. If the provided vector has a +// norm of zero, the vector will be unchanged. static inline void vec3Normalize(struct Vec3 *v) { - float invNorm = 1.0f / vec3Norm(v); - v->x *= invNorm; - v->y *= invNorm; - v->z *= invNorm; + ASSERT_NOT_NULL(v); + float norm = vec3Norm(v); + ASSERT(norm > 0); + // Only normalize if norm is non-zero. + if (norm > 0) { + float invNorm = 1.0f / norm; + v->x *= invNorm; + v->y *= invNorm; + v->z *= invNorm; + } } +// Updates u as the cross product of v and w. static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v, const struct Vec3 *w) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); u->x = v->y * w->z - v->z * w->y; u->y = v->z * w->x - v->x * w->z; u->z = v->x * w->y - v->y * w->x; } +// Finds a vector orthogonal to the vector [inX, inY, inZ] and returns +// this in the components [outX, outY, outZ]. The vector is chosen such +// that the smallest component of [inX, inY, inZ] is set to zero in the +// output vector. For example, for the in vector [0.01, 4.0, 5.0], this +// function will return [0, 5.0, -4.0]. +void findOrthogonalVector(float inX, float inY, float inZ, float *outX, + float *outY, float *outZ); + + +// 4-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// +// Initialize the Vec4 structure with the provided component values. static inline void initVec4(struct Vec4 *v, float x, float y, float z, float w) { + ASSERT_NOT_NULL(v); v->x = x; v->y = y; v->z = z; v->w = w; } -void findOrthogonalVector(float inX, float inY, float inZ, float *outX, - float *outY, float *outZ); +// N-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// +// Dimension specified by the last argument in all functions below. + +// Adds two vectors and returns the sum in the provided vector, i.e. +// u = v + w. +void vecAdd(float *u, const float *v, const float *w, int dim); + +// Adds two vectors and returns the sum in the first vector, i.e. +// v = v + w. +void vecAddInPlace(float *v, const float *w, int dim); + +// Subtracts two vectors and returns in the provided vector, i.e. +// u = v - w. +void vecSub(float *u, const float *v, const float *w, int dim); + +// Scales vector by a scalar and returns in the provided vector, i.e. +// u = c * v. +void vecScalarMul(float *u, const float *v, float c, int dim); + +// Scales vector by a scalar and returns in the same vector, i.e. +// v = c * v. +void vecScalarMulInPlace(float *v, float c, int dim); + +// Returns the L2-norm of the given vector. +float vecNorm(const float *v, int dim); + +// Returns the square of the L2-norm of the given vector. +float vecNormSquared(const float *v, int dim); + +// Returns the dot product of v and w. +float vecDot(const float *v, const float *w, int dim); + +// Returns the maximum absolute value in vector. +float vecMaxAbsoluteValue(const float *v, int dim); #ifdef __cplusplus } diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h new file mode 100644 index 00000000..c6389c33 --- /dev/null +++ b/firmware/os/algos/util/nano_assert.h @@ -0,0 +1,52 @@ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_ + +#if defined(__NANOHUB__) || defined(_OS_BUILD_) + +// For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided +// by the build system, which has the directory stripped. But allow the +// full filename for OS builds, where ASSERT should only be used for local +// development. +#if !defined(SRC_FILENAME) && defined(_OS_BUILD_) +#define SRC_FILENAME __FILENAME__ +#endif + +#if defined(_OS_BUILD_) + #include + #define ASSERT_LOG_FUNC(fmt, ...) osLog(LOG_ERROR, fmt, ##__VA_ARGS__) +#elif defined(__NANOHUB__) + #include + #define ASSERT_LOG_FUNC(fmt, ...) eOsLog(LOG_ERROR, fmt, ##__VA_ARGS__) +#endif + +// Note that this just logs and doesn't actually stop execution on Nanohub +#define ASSERT_IMPL(x) do { \ + if (!(x)) { \ + ASSERT_LOG_FUNC("Assertion: %s:%d\n", SRC_FILENAME, __LINE__); \ + } \ + } while (0) + +#else // Off-target testing, e.g. Google3 + +#define NANO_ASSERT_ENABLED +#include +#define ASSERT_IMPL(x) assert(x) + +#endif + +#ifdef NANO_ASSERT_ENABLED +#define ASSERT(x) ASSERT_IMPL(x) +#else +#define ASSERT(x) ((void)(x)) +#endif + +// Use NULL when compiling for C and nullptr for C++. +#ifdef __cplusplus +#define ASSERT_NOT_NULL(ptr) ASSERT((ptr) != nullptr) +#else +#define ASSERT_NOT_NULL(ptr) ASSERT((ptr) != NULL) +#endif + +#define UNUSED(x) ((void)(sizeof(x))) + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_ -- cgit v1.2.3