diff options
| author | Meng-hsuan Chung <menghsuan@google.com> | 2016-12-15 01:04:41 +0000 |
|---|---|---|
| committer | Meng-hsuan Chung <menghsuan@google.com> | 2016-12-15 01:04:41 +0000 |
| commit | 73de7fb55e60497a2a189d82cb1d9e1b8549da5d (patch) | |
| tree | af9cdd571dd42c097972abcf9da666d0b2ff6266 /firmware/os/algos/calibration | |
| parent | bad8af6cd1fadca604ed8af6f41e1dae9972ee35 (diff) | |
| download | contexthub-73de7fb55e60497a2a189d82cb1d9e1b8549da5d.tar.gz | |
Revert "Syncs google3 Calibration Code to Android."
This reverts commit bad8af6cd1fadca604ed8af6f41e1dae9972ee35.
Change-Id: I678435108782ae36b5b4ef0fa3e01664f282fb56
Diffstat (limited to 'firmware/os/algos/calibration')
17 files changed, 620 insertions, 3100 deletions
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index a789ce9d..eea4493a 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.c +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c @@ -156,7 +156,12 @@ 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); - initKasa(&acc->akf); + + 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 } // Accel cal init. @@ -260,28 +265,28 @@ static int accelStillnessDetection(struct AccelStillDet *asd, } // Accumulate data for KASA fit. -static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) { +static void accelCalUpdate(struct MagCal *amoc, struct AccelStillDet *asd) { // Run accumulators. float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y + asd->mean_z * asd->mean_z; - akf->acc_x += asd->mean_x; - akf->acc_y += asd->mean_y; - akf->acc_z += asd->mean_z; - akf->acc_w += w; + amoc->acc_x += asd->mean_x; + amoc->acc_y += asd->mean_y; + amoc->acc_z += asd->mean_z; + amoc->acc_w += w; - akf->acc_xx += asd->mean_x * asd->mean_x; - akf->acc_xy += asd->mean_x * asd->mean_y; - akf->acc_xz += asd->mean_x * asd->mean_z; - akf->acc_xw += asd->mean_x * w; + 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_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_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_zz += asd->mean_z * asd->mean_z; - akf->acc_zw += asd->mean_z * w; - akf->nsamples += 1; + amoc->acc_zz += asd->mean_z * asd->mean_z; + amoc->acc_zw += asd->mean_z * w; + amoc->nsamples += 1; } // Good data detection, sorting and accumulate the data for Kasa. @@ -296,42 +301,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->akf, asd); + accelCalUpdate(&ac1->amoc, 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->akf, asd); + accelCalUpdate(&ac1->amoc, 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->akf, asd); + accelCalUpdate(&ac1->amoc, 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->akf, asd); + accelCalUpdate(&ac1->amoc, 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->akf, asd); + accelCalUpdate(&ac1->amoc, 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->akf, asd); + accelCalUpdate(&ac1->amoc, asd); } // The leftover bucket nle. if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y && @@ -340,39 +345,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->akf, asd); + accelCalUpdate(&ac1->amoc, 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 akf->nsamples is zero. - if (ac1->akf.nsamples == 0) { + // Check if amoc->nsamples is zero. + if (ac1->amoc.nsamples == 0) { agdReset(&ac1->agd); - magKasaReset(&ac1->akf); + magCalReset(&ac1->amoc); complete = 0; return complete; } else { // Normalize the data to the sample numbers. - inv = 1.0f / ac1->akf.nsamples; + inv = 1.0f / ac1->amoc.nsamples; } - ac1->akf.acc_x *= inv; - ac1->akf.acc_y *= inv; - ac1->akf.acc_z *= inv; - ac1->akf.acc_w *= inv; + ac1->amoc.acc_x *= inv; + ac1->amoc.acc_y *= inv; + ac1->amoc.acc_z *= inv; + ac1->amoc.acc_w *= inv; - ac1->akf.acc_xx *= inv; - ac1->akf.acc_xy *= inv; - ac1->akf.acc_xz *= inv; - ac1->akf.acc_xw *= inv; + ac1->amoc.acc_xx *= inv; + ac1->amoc.acc_xy *= inv; + ac1->amoc.acc_xz *= inv; + ac1->amoc.acc_xw *= inv; - ac1->akf.acc_yy *= inv; - ac1->akf.acc_yz *= inv; - ac1->akf.acc_yw *= inv; + ac1->amoc.acc_yy *= inv; + ac1->amoc.acc_yz *= inv; + ac1->amoc.acc_yw *= inv; - ac1->akf.acc_zz *= inv; - ac1->akf.acc_zw *= inv; + ac1->amoc.acc_zz *= inv; + ac1->amoc.acc_zw *= inv; // Calculate the temp VAR and MEA.N ac1->agd.var_t = @@ -387,7 +392,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); - magKasaReset(&ac1->akf); + magCalReset(&ac1->amoc); complete = 0; return complete; } @@ -395,15 +400,15 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, } // Eigen value magnitude and ratio test. -static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) { +static int mocEigenTest(struct MagCal *moc, struct AccelGoodData *agd) { // covariance matrix. struct Mat33 S; - 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; + 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; struct Vec3 eigenvals; struct Mat33 eigenvecs; @@ -497,13 +502,13 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nsec, float x, float radius; // Grabbing the fit from the MAG cal. - magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius); + magCalFit(&acc->ac1[temp_gate].amoc, &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 (accEigenTest(&acc->ac1[temp_gate].akf, + if (mocEigenTest(&acc->ac1[temp_gate].amoc, &acc->ac1[temp_gate].agd)) { // Storing the new offsets. acc->x_bias_new = bias.x * KSCALE2; @@ -540,7 +545,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); - magKasaReset(&acc->ac1[temp_gate].akf); + magCalReset(&acc->ac1[temp_gate].amoc); } } } @@ -725,7 +730,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].akf.nsamples); + (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].amoc.nsamples); // Live bucket count hogher. CAL_DEBUG_LOG( "[BMI160]", @@ -734,7 +739,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].akf.nsamples); + (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].amoc.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 03c65e92..83a45070 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.h +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h @@ -1,3 +1,6 @@ +#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 * @@ -22,8 +25,6 @@ * 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 <stdint.h> #include <sys/types.h> @@ -123,8 +124,7 @@ struct AccelStatsMem { // Struct for an accel calibration for a single temperature window. struct AccelCalAlgo { struct AccelGoodData agd; - // TODO(mkramerm): Replace all abbreviations. - struct KasaFit akf; + struct MagCal amoc; }; // Complete accel calibration struct. diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/common/calibration_data.c deleted file mode 100644 index 9ae72d27..00000000 --- a/firmware/os/algos/calibration/common/calibration_data.c +++ /dev/null @@ -1,36 +0,0 @@ -#include "calibration/common/calibration_data.h" - -#include <string.h> - -#include "common/math/vec.h" - -// FUNCTION IMPLEMENTATIONS -////////////////////////////////////////////////////////////////////////////// - -// Set calibration data to identity scale factors, zero skew and -// zero bias. -void calDataReset(struct ThreeAxisCalData *calstruct) { - memset(calstruct, 0, sizeof(struct ThreeAxisCalData)); - calstruct->scale_factor_x = 1.0f; - calstruct->scale_factor_y = 1.0f; - calstruct->scale_factor_z = 1.0f; -} - -void calDataCorrectData(const struct ThreeAxisCalData* calstruct, - const float x_impaired[THREE_AXIS_DIM], - float* x_corrected) { - // x_temp = (x_impaired - bias). - float x_temp[THREE_AXIS_DIM]; - vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM); - - // x_corrected = scale_skew_mat * x_temp, where: - // scale_skew_mat = [scale_factor_x 0 0 - // skew_yx scale_factor_y 0 - // skew_zx skew_zy scale_factor_z]. - x_corrected[0] = calstruct->scale_factor_x * x_temp[0]; - x_corrected[1] = calstruct->skew_yx * x_temp[0] + - calstruct->scale_factor_y * x_temp[1]; - x_corrected[2] = calstruct->skew_zx * x_temp[0] + - calstruct->skew_zy * x_temp[1] + - calstruct->scale_factor_z * x_temp[2]; -} diff --git a/firmware/os/algos/calibration/common/calibration_data.h b/firmware/os/algos/calibration/common/calibration_data.h deleted file mode 100644 index b0e6809b..00000000 --- a/firmware/os/algos/calibration/common/calibration_data.h +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -///////////////////////////////////////////////////////////////////////////// -/* - * This module contains a data structure and corresponding helper functions for - * a three-axis sensor calibration. The calibration consists of a bias vector, - * bias, and a lower-diagonal scaling and skew matrix, scale_skew_mat. - * - * The calibration is applied to impaired sensor data as follows: - * - * corrected_data = scale_skew_mat * (impaired_data - bias). - */ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_ - -#include <stdint.h> - -#ifdef __cplusplus -extern "C" { -#endif - -#define THREE_AXIS_DIM (3) - -// Calibration data structure. -struct ThreeAxisCalData { - // Scale factor and skew terms. Used to construct the following lower - // diagonal scale_skew_mat: - // scale_skew_mat = [scale_factor_x 0 0 - // skew_yx scale_factor_y 0 - // skew_zx skew_zy scale_factor_z]. - float scale_factor_x; - float scale_factor_y; - float scale_factor_z; - float skew_yx; - float skew_zx; - float skew_zy; - - // Sensor bias offset. - float bias[THREE_AXIS_DIM]; - - // Calibration time. - uint64_t calibration_time_nanos; -}; - -// Set calibration data to identity scale factors, zero skew and -// zero bias. -void calDataReset(struct ThreeAxisCalData *calstruct); - -// Apply a stored calibration to correct a single sample of impaired sensor -// data. -void calDataCorrectData(const struct ThreeAxisCalData* calstruct, - const float x_impaired[THREE_AXIS_DIM], - float* x_corrected); - -#ifdef __cplusplus -} -#endif - -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_ diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c deleted file mode 100644 index 8e5c3e28..00000000 --- a/firmware/os/algos/calibration/common/diversity_checker.c +++ /dev/null @@ -1,165 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "calibration/common/diversity_checker.h" - -#include <errno.h> -#include <stdarg.h> -#include <stdio.h> -#include <string.h> -#include "common/math/vec.h" - -// Struct initialization. -void diversityCheckerInit( - struct DiversityChecker* diverse_data, - 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 deleted file mode 100644 index 30f53dba..00000000 --- a/firmware/os/algos/calibration/common/diversity_checker.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -////////////////////////////////////////////////////////////////////////////// -/* - * This 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 <stdbool.h> -#include <stddef.h> -#include <stdint.h> - -#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 deleted file mode 100644 index 2c26af55..00000000 --- a/firmware/os/algos/calibration/common/sphere_fit_calibration.c +++ /dev/null @@ -1,247 +0,0 @@ -#include "calibration/common/sphere_fit_calibration.h" - -#include <errno.h> -#include <stdarg.h> -#include <stdio.h> -#include <string.h> - -#include "calibration/util/cal_log.h" -#include "common/math/mat.h" -#include "common/math/vec.h" - -// FORWARD DECLARATIONS -/////////////////////////////////////////////////////////////////////////////// -// Utility for converting solver state to a calibration data structure. -static void convertStateToCalStruct(const float x[SF_STATE_DIM], - struct ThreeAxisCalData *calstruct); - -static bool runCalibration(struct SphereFitCal *sphere_cal, - const struct SphereFitData *data, - uint64_t timestamp_nanos); - -#define MIN_VALID_DATA_NORM (1e-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 deleted file mode 100644 index e49f225a..00000000 --- a/firmware/os/algos/calibration/common/sphere_fit_calibration.h +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -///////////////////////////////////////////////////////////////////////////// -/* - * This module contains an algorithm for performing a sphere fit calibration. - * A sphere fit calibration solves the following non-linear least squares - * problem: - * - * arg min || ||M(x - b)|| - exp_norm || - * M,b - * - * where: - * x is a 3xN matrix containing N 3-dimensional uncalibrated data points, - * M is a 3x3 lower diagonal scaling matrix - * b is a 3x1 offset vector. - * exp_norm is the expected norm of an individual calibration data point. - * M and b are solved such that the norm of the calibrated data (M(x - b)) is - * near exp_norm. - * - * This module uses a Levenberg-Marquardt nonlinear least squares solver to find - * M and b. M is assumed to be a lower diagonal, consisting of 6 parameters. - * - */ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_ - -#include <stdbool.h> -#include <stdint.h> - -#include "calibration/common/calibration_data.h" -#include "common/math/levenberg_marquardt.h" - -#ifdef __cplusplus -extern "C" { -#endif - -#define MIN_NUM_SPHERE_FIT_POINTS (14) - -// Enum defining the meaning of the state parameters. The 9-parameter -// sphere fit calibration computes a lower-diagonal scaling matrix (M) and -// an offset such that: -// x_corrected = M * (x_impaired - offset) -enum SphereFitParams { - eParamScaleMatrix11 = 0, - eParamScaleMatrix21, - eParamScaleMatrix22, - eParamScaleMatrix31, - eParamScaleMatrix32, - eParamScaleMatrix33, - eParamOffset1, - eParamOffset2, - eParamOffset3, - SF_STATE_DIM -}; - -// Structure containing the data to be used for the sphere fit calibration. -struct SphereFitData { - // Data for fit (assumed to be a matrix of size num_fit_points x SF_DATA_DIM) - const float *fit_data; - - // Pointer to standard deviations of the fit data, used to weight individual - // data points. Assumed to point to a matrix of dimensions - // num_fit_points x THREE_AXIS_DIM. - // If NULL, data will all be used with equal weighting in the fit. - const float *fit_data_std; - - // Number of fit points. - size_t num_fit_points; - - // Expected data norm. - float expected_norm; -}; - -// Structure for a sphere fit calibration, including a non-linear least squares -// solver and the latest state estimate. -struct SphereFitCal { - // Levenberg-Marquardt solver. - struct LmSolver lm_solver; - - // Minimum number of points for computing a calibration. - size_t min_points_for_cal; - - // State estimate. - float x[SF_STATE_DIM]; - uint64_t estimate_time_nanos; - - // Initial state for solver. - float x0[SF_STATE_DIM]; -}; - -// Initialize sphere fit calibration structure with solver and fit params. -void sphereFitInit(struct SphereFitCal *sphere_cal, - const struct LmParams *lm_params, - const size_t min_num_points_for_cal); - -// Clears state estimate and initial state. -void sphereFitReset(struct SphereFitCal *sphere_cal); - -// Sets data pointer for single solve of the Levenberg-Marquardt solver. -// Must be called before calling sphereFitRunCal(). -void sphereFitSetSolverData(struct SphereFitCal *sphere_cal, - struct LmData *lm_data); - -// Sends in a set of calibration data and attempts to run calibration. -// Returns true if a calibration was successfully triggered with this data. -bool sphereFitRunCal(struct SphereFitCal *sphere_cal, - const struct SphereFitData *data, - uint64_t timestamp_nanos); - -// Set an initial condition for the bias state. -void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal, - const float initial_bias[THREE_AXIS_DIM]); - -// Returns the latest calibration data in a ThreeAxisCalData structure. -void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal, - struct ThreeAxisCalData *cal_data); - -///////////////// TEST UTILITIES /////////////////////////////////////////// -// The following functions are exposed in the header for testing only. - -// The ResidualAndJacobianFunction for sphere calibration in the -// Levenberg-Marquardt solver. -void sphereFitResidAndJacobianFunc(const float *state, const void *f_data, - float *residual, float *jacobian); - -#ifdef __cplusplus -} -#endif - -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_ diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c index 1c609551..4e87dc9c 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c @@ -15,126 +15,44 @@ */ #include "calibration/gyroscope/gyro_cal.h" - -#include <float.h> -#include <math.h> -#include <string.h> - #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] - -// 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) +#define MAX_GYRO_BIAS 0.096f // [rad/sec] -// Unit conversion: nanoseconds to seconds. -#define NANOS_TO_SEC (1.0e-9f) +// Helper constants for converting units. +#define RAD_TO_MDEG (float)(1e3 * 180.0 / M_PI) +#define GRAVITY_TO_G (float)(1.0 / 9.80665) /////// FORWARD DECLARATIONS ///////////////////////////////////////// static void deviceStillnessCheck(struct GyroCal* gyro_cal, - uint64_t sample_time_nanos); + uint64_t sample_time); -static void computeGyroCal(struct GyroCal* gyro_cal, - uint64_t calibration_time_nanos); +static void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time); -static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos); +static void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time); #ifdef GYRO_CAL_DBG_ENABLED -static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); - -/* - * 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 gyroCalUpdateDebug(struct GyroCal* gyro_cal, + struct DebugGyroCal* debug_gyro_cal); -/* - * 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; - } -} +static void gyroCalTuneDebugPrint(struct GyroCal* gyro_cal, + uint64_t sample_time); -#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 +static void gyroCalDebugPrintData(int count, struct DebugGyroCal* debug_data); +#endif /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the gyro calibration data structure. -void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, - uint64_t max_still_duration_nanos, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos, - uint64_t window_time_duration_nanos, float gyro_var_threshold, +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, float gyro_confidence_delta, float accel_var_threshold, float accel_confidence_delta, float mag_var_threshold, float mag_confidence_delta, float stillness_threshold, @@ -143,9 +61,9 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, 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, @@ -155,24 +73,23 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, // Reset stillness flag and start timestamp. gyro_cal->prev_still = false; - gyro_cal->start_still_time_nanos = 0; + gyro_cal->start_still_time = 0; // Set the min and max window stillness duration. - gyro_cal->min_still_duration_nanos = min_still_duration_nanos; - gyro_cal->max_still_duration_nanos = max_still_duration_nanos; + gyro_cal->min_still_duration = min_still_duration; + gyro_cal->max_still_duration = max_still_duration; // Sets the duration of the stillness processing windows. - gyro_cal->window_time_duration_nanos = window_time_duration_nanos; + gyro_cal->window_time_duration = window_time_duration; // Set the watchdog timeout duration. - gyro_cal->gyro_watchdog_timeout_duration_nanos = - 2 * window_time_duration_nanos; + gyro_cal->gyro_watchdog_timeout_duration = 2 * window_time_duration; // Load the last valid cal from system memory. gyro_cal->bias_x = bias_x; // [rad/sec] gyro_cal->bias_y = bias_y; // [rad/sec] gyro_cal->bias_z = bias_z; // [rad/sec] - gyro_cal->calibration_time_nanos = calibration_time_nanos; + gyro_cal->calibration_time = calibration_time; // Set the stillness threshold required for gyro bias calibration. gyro_cal->stillness_threshold = stillness_threshold; @@ -181,36 +98,26 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, // 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_nanos = 0; + gyro_cal->stillness_win_endtime = 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: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 + 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 } -// Void all pointers in the gyro calibration data structure (doesn't do anything -// except prevent compiler warnings). +// Void all pointers in the gyro calibration data structure +// (prevents compiler warnings). void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; } // Get the most recent bias calibration value. @@ -220,32 +127,23 @@ 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_nanos) { + float bias_z, uint64_t calibration_time) { gyro_cal->bias_x = bias_x; gyro_cal->bias_y = bias_y; gyro_cal->bias_z = bias_z; - gyro_cal->calibration_time_nanos = calibration_time_nanos; + gyro_cal->calibration_time = calibration_time; #ifdef GYRO_CAL_DBG_ENABLED - 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)); + 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)); #endif } @@ -271,74 +169,60 @@ 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_nanos, - float x, float y, float z, float temperature) { +void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time, 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_nanos <= 0) { - gyro_cal->stillness_win_endtime_nanos = - sample_time_nanos + gyro_cal->window_time_duration_nanos; + if (gyro_cal->stillness_win_endtime <= 0) { + gyro_cal->stillness_win_endtime = + sample_time + gyro_cal->window_time_duration; // Start the watchdog timer. - gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; + gyro_cal->gyro_watchdog_start = sample_time; } -#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_nanos, sample_time_nanos, - x, y, z); + gyro_cal->stillness_win_endtime, sample_time, 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_nanos); + deviceStillnessCheck(gyro_cal, sample_time); } // Update the gyro calibration with mag data [micro Tesla]. -void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, - float x, float y, float z) { +void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x, + float y, float z) { // Pass magnetometer data to stillness detector. gyroStillDetUpdate(&gyro_cal->mag_stillness_detect, - gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, - x, y, z); + gyro_cal->stillness_win_endtime, sample_time, 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_nanos); + deviceStillnessCheck(gyro_cal, sample_time); } // Update the gyro calibration with accel data [m/sec^2]. -void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, - float x, float y, float z) { +void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, float x, + float y, float z) { // Pass accelerometer data to stillnesss detector. gyroStillDetUpdate(&gyro_cal->accel_stillness_detect, - gyro_cal->stillness_win_endtime_nanos, sample_time_nanos, - x, y, z); + gyro_cal->stillness_win_endtime, sample_time, 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_nanos); + deviceStillnessCheck(gyro_cal, sample_time); } // Checks the state of all stillness detectors to determine // whether the device is "still". -void deviceStillnessCheck(struct GyroCal* gyro_cal, - uint64_t sample_time_nanos) { +void deviceStillnessCheck(struct GyroCal* gyro_cal, uint64_t sample_time) { bool stillness_duration_exceeded = false; bool stillness_duration_too_short = false; bool device_is_still = false; @@ -347,7 +231,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, float conf_still = 0; // Check the watchdog timer. - checkWatchdog(gyro_cal, sample_time_nanos); + checkWatchdog(gyro_cal, sample_time); // Is there enough data to do a stillness calculation? if ((!gyro_cal->mag_stillness_detect.stillness_window_ready && @@ -358,8 +242,8 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, } // Set the next window end time for the stillness detectors. - gyro_cal->stillness_win_endtime_nanos = - sample_time_nanos + gyro_cal->window_time_duration_nanos; + gyro_cal->stillness_win_endtime = + sample_time + gyro_cal->window_time_duration; // Update the confidence scores for all sensors. gyroStillDetCompute(&gyro_cal->accel_stillness_detect); @@ -390,15 +274,15 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, 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_nanos = + gyro_cal->start_still_time = gyro_cal->gyro_stillness_detect.window_start_time; } - // Check to see if current stillness period exceeds the desired limit. + // Check to see if current stillness period exceeds the desired limit + // to avoid corrupting the . stillness_duration_exceeded = ((gyro_cal->gyro_stillness_detect.last_sample_time - - gyro_cal->start_still_time_nanos) > - gyro_cal->max_still_duration_nanos); + gyro_cal->start_still_time) > gyro_cal->max_still_duration); if (stillness_duration_exceeded) { // The current stillness has gone too long. Do a calibration with the @@ -406,32 +290,23 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, // Update the gyro bias estimate with the current window data and // reset the stats. - 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); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, 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, - /*reset_stats=*/false); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, - /*reset_stats=*/false); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/false); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, false); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, false); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, false); // Update stillness flag. gyro_cal->prev_still = true; @@ -443,8 +318,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, // "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_nanos) < - gyro_cal->min_still_duration_nanos); + gyro_cal->start_still_time) < gyro_cal->min_still_duration); if (gyro_cal->prev_still && !stillness_duration_too_short) { computeGyroCal(gyro_cal, @@ -452,29 +326,28 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, } // Reset stillness detectors and the stats. - 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 + gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, true); // Update stillness flag. gyro_cal->prev_still = false; } // Reset the watchdog timer after we have processed data. - gyro_cal->gyro_watchdog_start_nanos = sample_time_nanos; + 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 } // Calculates a new gyro bias offset calibration value. -void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { +void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time) { // Current calibration duration. - uint64_t cur_cal_dur_nanos = - calibration_time_nanos - gyro_cal->start_still_time_nanos; + uint64_t cur_cal_dur = calibration_time - gyro_cal->start_still_time; // Check to see if new calibration values is within acceptable range. if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS && @@ -483,15 +356,6 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { 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; } @@ -508,10 +372,10 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Store calibration stillness duration. - gyro_cal->calibration_time_duration_nanos = cur_cal_dur_nanos; + gyro_cal->calibration_time_duration = cur_cal_dur; // Store calibration time stamp. - gyro_cal->calibration_time_nanos = calibration_time_nanos; + gyro_cal->calibration_time = calibration_time; // Set flag to indicate a new gyro calibration value is available. gyro_cal->new_gyro_cal_available = true; @@ -520,42 +384,62 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { // 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); + 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++; + } - // Trigger a printout of the debug information. - gyro_cal->debug_print_trigger = true; + // Update the calibration debug information. + gyroCalUpdateDebug( + gyro_cal, &gyro_cal->debug_cal_data_hist[gyro_cal->debug_head_hist]); + } #endif } // Check for a watchdog timeout condition. -void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { +void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time) { bool watchdog_timeout; // Check for initialization of the watchdog time (=0). - if (gyro_cal->gyro_watchdog_start_nanos <= 0) { + if (gyro_cal->gyro_watchdog_start <= 0) { return; } // Check for timeout condition of watchdog. - watchdog_timeout = - ((sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos) > - gyro_cal->gyro_watchdog_timeout_duration_nanos); + watchdog_timeout = ((sample_time - gyro_cal->gyro_watchdog_start) > + gyro_cal->gyro_watchdog_timeout_duration); // 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, /*reset_stats=*/true); - gyroStillDetReset(&gyro_cal->gyro_stillness_detect, /*reset_stats=*/true); - gyroStillDetReset(&gyro_cal->mag_stillness_detect, /*reset_stats=*/true); + gyroStillDetReset(&gyro_cal->accel_stillness_detect, true); + gyroStillDetReset(&gyro_cal->gyro_stillness_detect, true); + gyroStillDetReset(&gyro_cal->mag_stillness_detect, true); gyro_cal->mag_stillness_detect.stillness_confidence = 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 + gyro_cal->stillness_win_endtime = 0; // Force stillness confidence to zero. gyro_cal->accel_stillness_detect.prev_stillness_confidence = 0; @@ -573,596 +457,340 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { // Assert watchdog timeout flags. gyro_cal->gyro_watchdog_timeout |= watchdog_timeout; - gyro_cal->gyro_watchdog_start_nanos = 0; + gyro_cal->gyro_watchdog_start = 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 -void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { +// Update the calibration debug information. +void gyroCalUpdateDebug(struct GyroCal* gyro_cal, + struct DebugGyroCal* debug_gyro_cal) { // Probability of stillness (acc, rot, still), duration, timestamp. - gyro_cal->debug_gyro_cal.accel_stillness_conf = + debug_gyro_cal->accel_stillness_conf = gyro_cal->accel_stillness_detect.prev_stillness_confidence; - gyro_cal->debug_gyro_cal.gyro_stillness_conf = + debug_gyro_cal->gyro_stillness_conf = gyro_cal->gyro_stillness_detect.prev_stillness_confidence; - gyro_cal->debug_gyro_cal.mag_stillness_conf = + debug_gyro_cal->mag_stillness_conf = gyro_cal->mag_stillness_detect.prev_stillness_confidence; // Magnetometer usage. - gyro_cal->debug_gyro_cal.using_mag_sensor = gyro_cal->using_mag_sensor; + debug_gyro_cal->used_mag_sensor = gyro_cal->using_mag_sensor; // Temperature at calibration time. - 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; + 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; } -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; +// 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"); } - // 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]", " Temperature = %s%d.%06d [C]\n", + CAL_ENCODE_FLOAT(debug_data->temperature_celcius, 6)); - // 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]", " Calibration Timestamp = %llu [nsec]\n", + debug_data->calibration_time); - // Increments the number of samples. - local_temperature_stats.num_temperature_samples++; + CAL_DEBUG_LOG("[GYRO_CAL]", " Stillness Duration = %llu [nsec]\n", + debug_data->calibration_time_duration); - // 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]", + " 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)); - // 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; - } + 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)); } -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; - } - - // 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++; +// 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; - // 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: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: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)); + // 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) { 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)); + "[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)); CAL_DEBUG_LOG( - "[GYRO_CAL:TEMP_STATS]", - "Min/Max Temperature [C]: %s%d.%08d, %s%d.%08d", + "[GYRO_CAL]", " Stillness = {G%s%d.%06d, A%s%d.%06d, M%s%d.%06d}\n", CAL_ENCODE_FLOAT( - debug_data->debug_temperature.temperature_min_max_celsius[0], 8), + gyro_cal->gyro_stillness_detect.stillness_confidence, 6), CAL_ENCODE_FLOAT( - debug_data->debug_temperature.temperature_min_max_celsius[1], 8)); + 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"); CAL_DEBUG_LOG( - "[GYRO_CAL:TEMP_STATS]", "Temperature Mean [C]: %s%d.%08d", + "[GYRO_CAL]", " Stillness = {G%s%d.%06d, A%s%d.%06d, M---}\n", CAL_ENCODE_FLOAT( - debug_data->debug_temperature.temperature_mean_celsius, 8)); - CAL_DEBUG_LOG( - "[GYRO_CAL:TEMP_STATS]", "Temperature Variance [C^2]: %s%d.%08d", + gyro_cal->gyro_stillness_detect.stillness_confidence, 6), CAL_ENCODE_FLOAT( - debug_data->debug_temperature.temperature_var_celsius, 8)); - break; - - 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; + gyro_cal->accel_stillness_detect.stillness_confidence, 6)); + } - 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; + 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 + } +} - default: - 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. } } -// 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; +// 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) { CAL_DEBUG_LOG("[GYRO_CAL]", ""); - gyro_cal->debug_print_trigger = false; // Resets trigger. + 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. } else { - debug_state = IDLE; + 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. } break; - case WAIT_STATE: - // This helps throttle the print statements. - if ((timestamp_nanos - wait_timer_nanos) >= OVERTEMPCAL_WAIT_TIME_NANOS) { - debug_state = next_state; + 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. } break; - 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 PRINT_TIME: - gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, CAL_TIME); - - 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); - - 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 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 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. + 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; + } 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 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_STILLNESS: - gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, STILLNESS_DATA); + 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_SAMPLING_RATE; // Sets the next state. - debug_state = WAIT_STATE; // First, go to wait state. + 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. break; - case PRINT_SAMPLING_RATE: - gyroCalDebugPrintData(&gyro_cal->debug_gyro_cal, SAMPLING_RATE); - debug_state = IDLE; + 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. 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; + 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. } else { - debug_state = IDLE; + // 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. } break; - 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)); + 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); - 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]", "Total Watchdog Timeouts: %lu\n", + gyro_cal->debug_watchdog_count); - 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. + 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. break; - 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)); + 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; } - - 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: - // 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. + // Idle state. + gyro_cal->gyro_debug_state = -1; + + // Report periodic data useful for tuning the stillness detectors. + gyroCalTuneDebugPrint(gyro_cal, sample_time); } } -#endif // GYRO_CAL_DBG_TUNE_ENABLED -#endif // GYRO_CAL_DBG_ENABLED +#endif diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h index 4b5a9c44..f5a9109b 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h @@ -1,3 +1,6 @@ +#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 * @@ -14,6 +17,7 @@ * limitations under the License. */ +/////////////////////////////////////////////////////////////// /* * This module contains the algorithms for producing a * gyroscope offset calibration. The algorithm looks @@ -37,14 +41,8 @@ * 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" @@ -53,26 +51,21 @@ extern "C" { #endif #ifdef GYRO_CAL_DBG_ENABLED -// 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; -}; +// 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] // 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]; @@ -80,7 +73,6 @@ struct DebugGyroCal { float accel_var[3]; float gyro_var[3]; float mag_var[3]; - bool using_mag_sensor; }; #endif @@ -90,6 +82,13 @@ 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; @@ -97,63 +96,65 @@ struct GyroCal { float stillness_threshold; // Min and max durations for gyro bias calibration. - uint64_t min_still_duration_nanos; - uint64_t max_still_duration_nanos; + uint64_t min_still_duration; + uint64_t max_still_duration; // Duration of the stillness processing windows. - uint64_t window_time_duration_nanos; + uint64_t window_time_duration; + + // Flag to indicate if device was previously still. + bool prev_still; // Timestamp when device started a still period. - uint64_t start_still_time_nanos; + uint64_t start_still_time; // gyro bias estimates and last calibration timestamp. float bias_x, bias_y, bias_z; // [rad/sec] - uint64_t calibration_time_nanos; - uint64_t calibration_time_duration_nanos; + uint64_t calibration_time; + uint64_t calibration_time_duration; 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_nanos; + uint64_t stillness_win_endtime; // Watchdog timer to reset to a known good state when data capture stalls. - uint64_t gyro_watchdog_start_nanos; - uint64_t gyro_watchdog_timeout_duration_nanos; + uint64_t gyro_watchdog_start; + uint64_t gyro_watchdog_timeout_duration; 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. - 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 + 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 }; /////// FUNCTION PROTOTYPES ////////////////////////////////////////// // Initialize the gyro calibration data structure. void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, - uint64_t max_still_duration_nanos, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos, - uint64_t window_time_duration_nanos, float gyro_var_threshold, + 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, float gyro_confidence_delta, float accel_var_threshold, float accel_confidence_delta, float mag_var_threshold, float mag_confidence_delta, float stillness_threshold, @@ -168,7 +169,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_nanos); + float bias_z, uint64_t calibration_time); // Remove gyro bias from the calibration [rad/sec]. void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, @@ -178,21 +179,23 @@ 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_nanos, +void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time, 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_nanos, - float x, float y, float z); +void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, 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_nanos, +void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time, 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 timestamp_nanos_nanos); +void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time); #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 afddb481..12763b08 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 stable Welford method would afford. + // numerically stabe 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 9a1d876c..9ddc9075 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h @@ -1,3 +1,6 @@ +#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 * @@ -30,8 +33,6 @@ * 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 <math.h> #include <stdbool.h> diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c index bd4876ca..340d021e 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.c +++ b/firmware/os/algos/calibration/magnetometer/mag_cal.c @@ -30,18 +30,16 @@ #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 KasaFit *kasa) { +static int moc_eigen_test(struct MagCal *moc) { // covariance matrix struct Mat33 S; - S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x; - S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y; - 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; + 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; struct Vec3 eigenvals; struct Mat33 eigenvecs; @@ -62,29 +60,29 @@ static int moc_eigen_test(struct KasaFit *kasa) { } // Kasa sphere fitting with normal equation -int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) { +int magCalFit(struct MagCal *moc, struct Vec3 *bias, float *radius) { // A * out = b // (4 x 4) (4 x 1) (4 x 1) struct Mat44 A; - A.elem[0][0] = kasa->acc_xx; - A.elem[0][1] = kasa->acc_xy; - A.elem[0][2] = kasa->acc_xz; - A.elem[0][3] = kasa->acc_x; - A.elem[1][0] = kasa->acc_xy; - A.elem[1][1] = kasa->acc_yy; - A.elem[1][2] = kasa->acc_yz; - A.elem[1][3] = kasa->acc_y; - A.elem[2][0] = kasa->acc_xz; - A.elem[2][1] = kasa->acc_yz; - A.elem[2][2] = kasa->acc_zz; - A.elem[2][3] = kasa->acc_z; - A.elem[3][0] = kasa->acc_x; - A.elem[3][1] = kasa->acc_y; - A.elem[3][2] = kasa->acc_z; + A.elem[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[3][3] = 1.0f; struct Vec4 b; - initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w); + initVec4(&b, -moc->acc_xw, -moc->acc_yw, -moc->acc_zw, -moc->acc_w); struct Size4 pivot; mat44DecomposeLup(&A, &pivot); @@ -114,18 +112,13 @@ int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) { return success; } -void magKasaReset(struct KasaFit *kasa) { - kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f; - kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f; - kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f; - kasa->acc_zz = kasa->acc_zw = 0.0f; - - kasa->nsamples = 0; -} - void magCalReset(struct MagCal *moc) { - magKasaReset(&moc->kasa); - diversityCheckerReset(&moc->diversity_checker); + 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; + + moc->nsamples = 0; moc->start_time = 0; } @@ -133,32 +126,20 @@ 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->kasa.nsamples > MIN_BATCH_SIZE)) { + (moc->nsamples > MIN_BATCH_SIZE)) { complete = 1; - } 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. + } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { + // not enough samples collected in MAX_BATCH_WINDOW 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 threshold, float max_distance, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, - float var_threshold, - float max_min_threshold) { + float c12, float c20, float c21, float c22) { magCalReset(moc); moc->update_time = 0; moc->radius = 0.0f; @@ -176,15 +157,6 @@ 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; } @@ -193,74 +165,66 @@ 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->kasa.acc_x += x; - moc->kasa.acc_y += y; - moc->kasa.acc_z += z; - moc->kasa.acc_w += w; + moc->acc_x += x; + moc->acc_y += y; + moc->acc_z += z; + moc->acc_w += w; - moc->kasa.acc_xx += x * x; - moc->kasa.acc_xy += x * y; - moc->kasa.acc_xz += x * z; - moc->kasa.acc_xw += x * w; + moc->acc_xx += x * x; + moc->acc_xy += x * y; + moc->acc_xz += x * z; + moc->acc_xw += x * w; - moc->kasa.acc_yy += y * y; - moc->kasa.acc_yz += y * z; - moc->kasa.acc_yw += y * w; + moc->acc_yy += y * y; + moc->acc_yz += y * z; + moc->acc_yw += y * w; - moc->kasa.acc_zz += z * z; - moc->kasa.acc_zw += z * w; + moc->acc_zz += z * z; + moc->acc_zw += z * w; - if (++moc->kasa.nsamples == 1) { + if (++moc->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->kasa.nsamples; + float inv = 1.0f / moc->nsamples; - moc->kasa.acc_x *= inv; - moc->kasa.acc_y *= inv; - moc->kasa.acc_z *= inv; - moc->kasa.acc_w *= inv; + moc->acc_x *= inv; + moc->acc_y *= inv; + moc->acc_z *= inv; + moc->acc_w *= inv; - moc->kasa.acc_xx *= inv; - moc->kasa.acc_xy *= inv; - moc->kasa.acc_xz *= inv; - moc->kasa.acc_xw *= inv; + moc->acc_xx *= inv; + moc->acc_xy *= inv; + moc->acc_xz *= inv; + moc->acc_xw *= inv; - moc->kasa.acc_yy *= inv; - moc->kasa.acc_yz *= inv; - moc->kasa.acc_yw *= inv; + moc->acc_yy *= inv; + moc->acc_yz *= inv; + moc->acc_yw *= inv; - moc->kasa.acc_zz *= inv; - moc->kasa.acc_zw *= inv; + moc->acc_zz *= inv; + moc->acc_zw *= inv; // 3. eigen test - if (moc_eigen_test(&moc->kasa)) { + if (moc_eigen_test(moc)) { struct Vec3 bias; float radius; // 4. Kasa sphere fitting - 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; - } + 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; } } diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h index eccf35ce..a73aa22a 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.h +++ b/firmware/os/algos/calibration/magnetometer/mag_cal.h @@ -1,3 +1,6 @@ +#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 * @@ -13,50 +16,35 @@ * 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 <stdbool.h> #include <stdint.h> #include <sys/types.h> -#include "calibration/common/diversity_checker.h" #include "common/math/mat.h" -#include "common/math/vec.h" #ifdef __cplusplus extern "C" { #endif -struct KasaFit { - float acc_x, acc_y, acc_z, acc_w; - float acc_xx, acc_xy, acc_xz, acc_xw; - float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw; - size_t nsamples; -}; - struct MagCal { - struct DiversityChecker diversity_checker; - struct KasaFit kasa; - uint64_t start_time; uint64_t update_time; + 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; + float x_bias, y_bias, z_bias; float radius; float c00, c01, c02, c10, c11, c12, c20, c21, c22; -}; -void initKasa(struct KasaFit *kasa); + size_t nsamples; +}; 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 threshold, float max_distance, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, - float var_threshold, - float max_min_threshold); + float c12, float c20, float c21, float c22); void magCalDestroy(struct MagCal *moc); @@ -77,11 +65,9 @@ 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 magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius); +int magCalFit(struct MagCal *moc, 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 deleted file mode 100644 index 0b8b6c73..00000000 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c +++ /dev/null @@ -1,940 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "calibration/over_temp/over_temp_cal.h" - -#include <float.h> -#include <math.h> -#include <string.h> - -#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 deleted file mode 100644 index 928576f0..00000000 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h +++ /dev/null @@ -1,327 +0,0 @@ -/* - * Copyright (C) 2016 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* - * This module 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 <stdbool.h> -#include <stddef.h> -#include <stdint.h> - -#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 64f70f8e..dca59fdf 100644 --- a/firmware/os/algos/calibration/util/cal_log.h +++ b/firmware/os/algos/calibration/util/cal_log.h @@ -1,3 +1,6 @@ +#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 * @@ -19,23 +22,16 @@ * 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_ -#ifdef GCC_DEBUG_LOG -# include <stdio.h> -# define CAL_DEBUG_LOG(tag, fmt, ...) \ - printf("%s " fmt "\n", tag, ##__VA_ARGS__); -#else // GCC_DEBUG_LOG -# include <seos.h> -# 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 + +#ifndef LOG_FUNC +#include <seos.h> +#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 __cplusplus extern "C" { |