diff options
Diffstat (limited to 'firmware/os')
63 files changed, 5503 insertions, 1899 deletions
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index c7002535..99e96ef7 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.c +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c @@ -15,13 +15,16 @@ */ #include "calibration/accelerometer/accel_cal.h" + #include <errno.h> +#include <inttypes.h> #include <math.h> #include <stdio.h> #include <string.h> -#include "calibration/magnetometer/mag_cal.h" + #include "calibration/util/cal_log.h" +// clang-format off #define KSCALE \ 0.101936799f // Scaling from m/s^2 to g (0.101 = 1/(9.81 m/s^2)). #define KSCALE2 9.81f // Scaling from g to m/s^2. @@ -36,8 +39,9 @@ #define MIN_TEMP 20.0f // No Data is collected below 20 degree C. #define MAX_TEMP 45.0f // No Data is collected above 45 degree C. #define TEMP_CUT 30 // Separation point for temperature buckets 30 degree C. -#define EIGEN_RATIO 0.35 // EIGEN_RATIO (must be greater than 0.35). -#define EIGEN_MAG 0.97 // Eigen value magnitude (must be greater than 0.97). +#define EIGEN_RATIO 0.35f // EIGEN_RATIO (must be greater than 0.35). +#define EIGEN_MAG 0.97f // Eigen value magnitude (must be greater than 0.97). +#define ACCEL_NEW_BIAS_THRESHOLD (0.0f) // Bias update detection threshold. #ifdef ACCEL_CAL_DBG_ENABLED #define TEMP_HIST_LOW \ 16 // Putting all Temp counts in first bucket for temp < 16 degree C. @@ -47,8 +51,9 @@ #endif #ifdef IMU_TEMP_DBG_ENABLED #define IMU_TEMP_DELTA_TIME_NANOS \ - 5000000000 // Printing every 5 seconds IMU temp. + 5000000000 // Printing every 5 seconds IMU temp. #endif +// clang-format on /////////// Start Debug ////////////////////// @@ -159,19 +164,29 @@ 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); + kasaInit(&acc->akf); +} + +// Returns true when a new accel calibration is available. +bool accelCalNewBiasAvailable(struct AccelCal *acc) { + return fabsf(acc->x_bias - acc->x_bias_new) > ACCEL_NEW_BIAS_THRESHOLD || + fabsf(acc->y_bias - acc->y_bias_new) > ACCEL_NEW_BIAS_THRESHOLD || + fabsf(acc->z_bias - acc->z_bias_new) > ACCEL_NEW_BIAS_THRESHOLD; } // Accel cal init. -void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th, - uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb, - uint32_t fz, uint32_t fzb, uint32_t fle) { +void accelCalInit(struct AccelCal *acc, + const struct AccelCalParameters *parameters) { // Init core accel data. - accelCalAlgoInit(&acc->ac1[0], fx, fxb, fy, fyb, fz, fzb, fle); - accelCalAlgoInit(&acc->ac1[1], fx, fxb, fy, fyb, fz, fzb, fle); + accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb, + parameters->fy, parameters->fyb, parameters->fz, + parameters->fzb, parameters->fle); + accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb, + parameters->fy, parameters->fyb, parameters->fz, + parameters->fzb, parameters->fle); // Stillness Reset. - accelStillInit(&acc->asd, t0, n_s, th); + accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th); // Debug data init. #ifdef ACCEL_CAL_DBG_ENABLED @@ -267,31 +282,6 @@ static int accelStillnessDetection(struct AccelStillDet *asd, return complete; } -// Accumulate data for KASA fit. -static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) { - // Run accumulators. - float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y + - asd->mean_z * asd->mean_z; - - akf->acc_x += asd->mean_x; - akf->acc_y += asd->mean_y; - akf->acc_z += asd->mean_z; - akf->acc_w += w; - - akf->acc_xx += asd->mean_x * asd->mean_x; - akf->acc_xy += asd->mean_x * asd->mean_y; - akf->acc_xz += asd->mean_x * asd->mean_z; - akf->acc_xw += asd->mean_x * w; - - akf->acc_yy += asd->mean_y * asd->mean_y; - akf->acc_yz += asd->mean_y * asd->mean_z; - akf->acc_yw += asd->mean_y * w; - - akf->acc_zz += asd->mean_z * asd->mean_z; - akf->acc_zw += asd->mean_z * w; - akf->nsamples += 1; -} - // Good data detection, sorting and accumulate the data for Kasa. static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, float temp) { @@ -304,42 +294,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); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Negative x bucket nxb. if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) { ac1->agd.nxb += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->akf, asd); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Y bucket ny. if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) { ac1->agd.ny += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->akf, asd); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Negative y bucket nyb. if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) { ac1->agd.nyb += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->akf, asd); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Z bucket nz. if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) { ac1->agd.nz += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->akf, asd); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Negative z bucket nzb. if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) { ac1->agd.nzb += 1; ac1->agd.acc_t += temp; ac1->agd.acc_tt += temp * temp; - accelCalUpdate(&ac1->akf, asd); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // The leftover bucket nle. if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y && @@ -348,7 +338,7 @@ 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); + kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z); } // Checking if all buckets are full. if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb && @@ -357,32 +347,16 @@ static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, // Check if akf->nsamples is zero. if (ac1->akf.nsamples == 0) { agdReset(&ac1->agd); - magKasaReset(&ac1->akf); + kasaReset(&ac1->akf); complete = 0; return complete; - } else { - // Normalize the data to the sample numbers. - inv = 1.0f / ac1->akf.nsamples; } - ac1->akf.acc_x *= inv; - ac1->akf.acc_y *= inv; - ac1->akf.acc_z *= inv; - ac1->akf.acc_w *= inv; + // Normalize the data to the sample numbers. + kasaNormalize(&ac1->akf); - ac1->akf.acc_xx *= inv; - ac1->akf.acc_xy *= inv; - ac1->akf.acc_xz *= inv; - ac1->akf.acc_xw *= inv; - - ac1->akf.acc_yy *= inv; - ac1->akf.acc_yz *= inv; - ac1->akf.acc_yw *= inv; - - ac1->akf.acc_zz *= inv; - ac1->akf.acc_zw *= inv; - - // Calculate the temp VAR and MEA.N + // Calculate the temp VAR and MEAN. + inv = 1.0f / ac1->akf.nsamples; ac1->agd.var_t = (ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv; ac1->agd.mean_t = ac1->agd.acc_t * inv; @@ -395,7 +369,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); + kasaReset(&ac1->akf); complete = 0; return complete; } @@ -485,14 +459,15 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, #ifdef IMU_TEMP_DBG_ENABLED if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) { CAL_DEBUG_LOG("IMU Temp Data: ", - ", %s%d.%02d, %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n", - CAL_ENCODE_FLOAT(temp, 2), - (unsigned long long int)sample_time_nanos, - CAL_ENCODE_FLOAT(acc->x_bias_new,5), - CAL_ENCODE_FLOAT(acc->y_bias_new,5), - CAL_ENCODE_FLOAT(acc->z_bias_new,5)); + ", " CAL_FORMAT_3DIGITS ", %" PRIu64 + ", " CAL_FORMAT_6DIGITS_TRIPLET " \n", + CAL_ENCODE_FLOAT(temp, 3), + sample_time_nanos, + CAL_ENCODE_FLOAT(acc->x_bias_new, 6), + CAL_ENCODE_FLOAT(acc->y_bias_new, 6), + CAL_ENCODE_FLOAT(acc->z_bias_new, 6)); acc->temp_time_nanos = sample_time_nanos; - } + } #endif int temp_gate = 0; @@ -523,7 +498,8 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, float radius; // Grabbing the fit from the MAG cal. - magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius); + kasaFit(&acc->ac1[temp_gate].akf, &bias, &radius, G_NORM_MAX, + G_NORM_MIN); // If offset is too large don't take. if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF && @@ -567,238 +543,223 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, // Resetting the structs for a new accel cal run. agdReset(&acc->ac1[temp_gate].agd); - magKasaReset(&acc->ac1[temp_gate].akf); + kasaReset(&acc->ac1[temp_gate].akf); } } } } #ifdef ACCEL_CAL_DBG_ENABLED + +// Local helper macro for printing log messages. +#ifdef CAL_NO_FLOAT_FORMAT_STRINGS +#define CAL_FORMAT_ACCEL_HISTORY \ + "%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d," \ + "%s%d.%06d,%s%d.%06d,%s%d.%06d" +#else +#define CAL_FORMAT_ACCEL_HISTORY \ + "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f" +#endif // CAL_NO_FLOAT_FORMAT_STRINGS + // Debug Print Output void accelCalDebPrint(struct AccelCal *acc, float temp) { static int32_t kk = 0; if (++kk == 1000) { // X offset history last 10 values. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,11,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(x_off history)\n", - CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6), - CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{11," CAL_FORMAT_ACCEL_HISTORY "}(x_off history)\n", + CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6), + CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6)); // Y offset history last 10 values. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,12,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(y_off history)\n", - CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6), - CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{12," CAL_FORMAT_ACCEL_HISTORY "}(y_off history)\n", + CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6), + CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6)); // Z offset history last 10 values. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,13,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(z_off history)\n", - CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6), - CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{13," CAL_FORMAT_ACCEL_HISTORY "}(z_off history)\n", + CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6), + CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6)); // Temp history variation VAR of offset. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,14,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(VAR temp history)\n", - CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6), - CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{14," CAL_FORMAT_ACCEL_HISTORY "}(VAR temp history)\n", + CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6), + CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6)); // Temp mean history of offset. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,15,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(MEAN Temp history)\n", - CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6), - CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{15," CAL_FORMAT_ACCEL_HISTORY "}(MEAN Temp history)\n", + CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6), + CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6)); // KASA radius history. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,16,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(radius)\n", - CAL_ENCODE_FLOAT(acc->adf.rad[0], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[1], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[2], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[3], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[4], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[5], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[6], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[7], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[8], 6), - CAL_ENCODE_FLOAT(acc->adf.rad[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", "{16," CAL_FORMAT_ACCEL_HISTORY "}(radius)\n", + CAL_ENCODE_FLOAT(acc->adf.rad[0], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[1], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[2], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[3], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[4], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[5], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[6], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[7], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[8], 6), + CAL_ENCODE_FLOAT(acc->adf.rad[9], 6)); kk = 0; } if (kk == 750) { // Eigen Vector X. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, " - "7,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen x)\n", - CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6), - CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 7," CAL_FORMAT_ACCEL_HISTORY "}(eigen x)\n", + CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6), + CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6)); // Y. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, " - "8,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen y)\n", - CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6), - CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 8," CAL_FORMAT_ACCEL_HISTORY "}(eigen y)\n", + CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6), + CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6)); // Z. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, " - "9,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" - "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen z)\n", - CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6), - CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 9," CAL_FORMAT_ACCEL_HISTORY "}(eigen z)\n", + CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6), + CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6)); // Accel Time in ns. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL,10,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,}(" - "timestamp ns)\n", - acc->adf.cal_time[0], acc->adf.cal_time[1], acc->adf.cal_time[2], - acc->adf.cal_time[3], acc->adf.cal_time[4], acc->adf.cal_time[5], - acc->adf.cal_time[6], acc->adf.cal_time[7], acc->adf.cal_time[8], - acc->adf.cal_time[9]); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{10,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 + ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 + "}(timestamp ns)\n", + acc->adf.cal_time[0], acc->adf.cal_time[1], + acc->adf.cal_time[2], acc->adf.cal_time[3], + acc->adf.cal_time[4], acc->adf.cal_time[5], + acc->adf.cal_time[6], acc->adf.cal_time[7], + acc->adf.cal_time[8], acc->adf.cal_time[9]); } if (kk == 500) { // Total bucket count. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d,}(Total Bucket #)\n", - (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, (unsigned)acc->adf.nty, - (unsigned)acc->adf.ntyb, (unsigned)acc->adf.ntz, - (unsigned)acc->adf.ntzb, (unsigned)acc->adf.ntle); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d}(Total Bucket #)\n", + (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, + (unsigned)acc->adf.nty, (unsigned)acc->adf.ntyb, + (unsigned)acc->adf.ntz, (unsigned)acc->adf.ntzb, + (unsigned)acc->adf.ntle); // Live bucket count lower. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # " - "lower)\n", - (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb, - (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb, - (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb, - (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # " + "lower)\n", + (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb, + (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb, + (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb, + (unsigned)acc->ac1[0].agd.nle, + (unsigned)acc->ac1[0].akf.nsamples); // Live bucket count hogher. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # " - "higher)\n", - (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb, - (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb, - (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb, - (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # " + "higher)\n", + (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb, + (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb, + (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb, + (unsigned)acc->ac1[1].agd.nle, + (unsigned)acc->ac1[1].akf.nsamples); // Offset used. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 3,%s%d.%06d, %s%d.%06d, %s%d.%06d, %2d,}(updated offset " - "x,y,z, total # of offsets)\n", - CAL_ENCODE_FLOAT(acc->x_bias, 6), CAL_ENCODE_FLOAT(acc->y_bias, 6), - CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 3,"CAL_FORMAT_6DIGITS_TRIPLET", %2d}(updated offset " + "x,y,z, total # of offsets)\n", + CAL_ENCODE_FLOAT(acc->x_bias, 6), + CAL_ENCODE_FLOAT(acc->y_bias, 6), + CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff); // Offset New. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 4,%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d,}(New offset " - "x,y,z, live temp)\n", - CAL_ENCODE_FLOAT(acc->x_bias_new, 6), - CAL_ENCODE_FLOAT(acc->y_bias_new, 6), - CAL_ENCODE_FLOAT(acc->z_bias_new, 6), CAL_ENCODE_FLOAT(temp, 6)); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 4," CAL_FORMAT_6DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS + "}(New offset x,y,z, live temp)\n", + CAL_ENCODE_FLOAT(acc->x_bias_new, 6), + CAL_ENCODE_FLOAT(acc->y_bias_new, 6), + CAL_ENCODE_FLOAT(acc->z_bias_new, 6), + CAL_ENCODE_FLOAT(temp, 6)); // Temp Histogram. - CAL_DEBUG_LOG( - "[BMI160]", - "{MK_ACCEL, 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " - "%7d, %7d,}(temp histo)\n", - (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1], - (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3], - (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5], - (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7], - (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9], - (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11], - (unsigned)acc->adf.t_hist[12]); - CAL_DEBUG_LOG( - "[BMI160]", - "M{K_ACCEL, 6,%7d, %7d, %7d,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " - "%7d,}(temp histo)\n", - (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14], - (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16], - (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18], - (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20], - (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22], - (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " + "%7d, %7d}(temp histo)\n", + (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1], + (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3], + (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5], + (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7], + (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9], + (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11], + (unsigned)acc->adf.t_hist[12]); + CAL_DEBUG_LOG("[ACCEL_CAL]", + "{ 6,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " + "%7d}(temp histo)\n", + (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14], + (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16], + (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18], + (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20], + (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22], + (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]); } } #endif diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h index 1cfef614..3324875d 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.h +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h @@ -27,7 +27,7 @@ #include <stdint.h> #include <sys/types.h> -#include "calibration/magnetometer/mag_cal.h" +#include "common/math/kasa.h" #include "common/math/mat.h" #ifdef __cplusplus @@ -127,6 +127,25 @@ struct AccelCalAlgo { struct KasaFit akf; }; +// AccelCal algorithm parameters (see the AccelCal for details). +struct AccelCalParameters { + // t0 -> Sets the time how long the accel has to be still in ns. + // n_s -> Defines the minimum number of samples for the stillness. + // th -> Sets the threshold for the stillness VAR in (g rms)^2. + // fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the + // sphere cap (Bucket) is needed to reach full. + uint32_t t0; + uint32_t n_s; + uint32_t fx; + uint32_t fxb; + uint32_t fy; + uint32_t fyb; + uint32_t fz; + uint32_t fzb; + uint32_t fle; + float th; +}; + // Complete accel calibration struct. struct AccelCal { struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS]; @@ -163,15 +182,15 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, float y, float z, float temp); /* This function initializes the accCalRun data struct. - * t0 -> Sets the time how long the accel has to be still in ns. - * n_s -> Defines the minimum number of samples for the stillness. - * th -> Sets the threshold for the stillness VAR in (g rms)^2. - * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the - * sphere cap (Bucket) is needed to reach full. + * [parameters]: + * t0 -> Sets the time how long the accel has to be still in ns. + * n_s -> Defines the minimum number of samples for the stillness. + * th -> Sets the threshold for the stillness VAR in (g rms)^2. + * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the + * sphere cap (Bucket) is needed to reach full. */ -void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th, - uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb, - uint32_t fz, uint32_t fzb, uint32_t fle); +void accelCalInit(struct AccelCal *acc, + const struct AccelCalParameters *parameters); void accelCalDestroy(struct AccelCal *acc); @@ -182,6 +201,9 @@ void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z); void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z); +// Returns true when a new accel calibration is available. +bool accelCalNewBiasAvailable(struct AccelCal *acc); + #ifdef ACCEL_CAL_DBG_ENABLED void accelCalDebPrint(struct AccelCal *acc, float temp); #endif diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c index d71ad9af..3fab81f8 100644 --- a/firmware/os/algos/calibration/common/diversity_checker.c +++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c @@ -14,52 +14,46 @@ * limitations under the License. */ -#include "calibration/common/diversity_checker.h" +#include "calibration/diversity_checker/diversity_checker.h" #include <errno.h> #include <stdarg.h> #include <stdio.h> #include <string.h> + #include "common/math/vec.h" // Struct initialization. -void diversityCheckerInit( - struct DiversityChecker* diverse_data, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, - float var_threshold, - float max_min_threshold, - float local_field, - float threshold_tuning_param, - float max_distance_tuning_param) { +void diversityCheckerInit(struct DiversityChecker* diverse_data, + const struct DiversityCheckerParameters* parameters) { ASSERT_NOT_NULL(diverse_data); // Initialize parameters. diverse_data->threshold_tuning_param_sq = - (threshold_tuning_param * threshold_tuning_param); + (parameters->threshold_tuning_param * parameters->threshold_tuning_param); diverse_data->max_distance_tuning_param_sq = - (max_distance_tuning_param * max_distance_tuning_param); + (parameters->max_distance_tuning_param * + parameters->max_distance_tuning_param); // Updating the threshold and max_distance using assumed local field. // Testing for zero and negative local_field. - if (local_field <= 0) { - local_field = 1; - } + const float local_field = + (parameters->local_field <= 0.0f) ? 1.0f : parameters->local_field; diversityCheckerLocalFieldUpdate(diverse_data, local_field); - diverse_data->min_num_diverse_vectors = min_num_diverse_vectors; + diverse_data->min_num_diverse_vectors = parameters->min_num_diverse_vectors; // Checking for min_num_diverse_vectors = 0. - if (min_num_diverse_vectors < 1) { + if (parameters->min_num_diverse_vectors < 1) { diverse_data->min_num_diverse_vectors = 1; } - diverse_data->max_num_max_distance = max_num_max_distance; - diverse_data->var_threshold = var_threshold; - diverse_data->max_min_threshold = max_min_threshold; + diverse_data->max_num_max_distance = parameters->max_num_max_distance; + diverse_data->var_threshold = parameters->var_threshold; + diverse_data->max_min_threshold = parameters->max_min_threshold; // Setting the rest to zero. diversityCheckerReset(diverse_data); - // Debug Messages + // Debug Messages #ifdef DIVERSE_DEBUG_ENABLE memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg)); #endif @@ -69,8 +63,7 @@ void diversityCheckerInit( void diversityCheckerReset(struct DiversityChecker* diverse_data) { ASSERT_NOT_NULL(diverse_data); // Clear data memory. - memset(&diverse_data->diverse_data, 0, - sizeof(diverse_data->diverse_data)); + memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data)); // Resetting counters and data full bit. diverse_data->num_points = 0; @@ -78,74 +71,82 @@ void diversityCheckerReset(struct DiversityChecker* diverse_data) { diverse_data->data_full = false; } -void diversityCheckerUpdate( - struct DiversityChecker* diverse_data, float x, float y, float z) { - ASSERT_NOT_NULL(diverse_data); - +bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data, + float x, float y, float z) { // Converting three single inputs to a vector. - const float vec[3] = {x, y, z}; + const float vec[THREE_AXIS_DATA_DIM] = {x, y, z}; // Result vector for vector difference. - float vec_diff[3]; + float vec_diff[THREE_AXIS_DATA_DIM]; // normSquared result (k) float norm_squared_result; - // 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; - } + size_t i; - // if k > max_distance, count and leave the function. - if (norm_squared_result > diverse_data->max_distance) { - diverse_data->num_max_dist_violations++; - return; - } + // 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 false; + } + + // if k > max_distance, count and leave the function. + if (norm_squared_result > diverse_data->max_distance) { + diverse_data->num_max_dist_violations++; + return false; } + } + return true; +} + +void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, + float y, float z) { + ASSERT_NOT_NULL(diverse_data); - // 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; + // If memory is full, no need to run through the data. + if (!diverse_data->data_full) { + // diversityCheckerDataSet() returns true, if input data is diverse against + // the already stored. + if (diversityCheckerFindNearestPoint(diverse_data, x, y, z)) { + // Converting three single inputs to a vector. + const float vec[THREE_AXIS_DATA_DIM] = {x, y, z}; + + // Notice that the first data vector will be stored no matter what. + memcpy( + &diverse_data + ->diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM], + vec, sizeof(float) * THREE_AXIS_DATA_DIM); + + // Count new data point. + diverse_data->num_points++; + + // Setting data_full to true, 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) { + float x_bias, float y_bias, float z_bias) { ASSERT_NOT_NULL(diverse_data); // If not enough diverse data points or max distance violations return false. if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors || diverse_data->num_max_dist_violations >= - diverse_data->max_num_max_distance) { + diverse_data->max_num_max_distance) { return false; } - float vec_bias[3] = {x_bias, y_bias, z_bias}; - float vec_bias_removed[3]; + float vec_bias[THREE_AXIS_DATA_DIM] = {x_bias, y_bias, z_bias}; + float vec_bias_removed[THREE_AXIS_DATA_DIM]; float norm_results; float acc_norm = 0.0f; float acc_norm_square = 0.0f; @@ -155,8 +156,7 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, for (i = 0; i < diverse_data->num_points; ++i) { // v = v1 - v_bias; vecSub(vec_bias_removed, - &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], - vec_bias, + &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec_bias, THREE_AXIS_DATA_DIM); // norm = ||v|| @@ -164,7 +164,7 @@ bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, // Accumulate for mean and VAR. acc_norm += norm_results; - acc_norm_square += norm_results * norm_results ; + acc_norm_square += norm_results * norm_results; if (i == 0) { min = norm_results; @@ -206,11 +206,11 @@ void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data, float local_field) { if (local_field > 0) { // Updating threshold based on the local field information. - diverse_data->threshold = diverse_data->threshold_tuning_param_sq * - (local_field * local_field); + diverse_data->threshold = + diverse_data->threshold_tuning_param_sq * (local_field * local_field); // Updating max distance based on the local field information. diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq * - (local_field * local_field); + (local_field * local_field); } } diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h index 5a245290..c38549b3 100644 --- a/firmware/os/algos/calibration/common/diversity_checker.h +++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h @@ -41,13 +41,18 @@ * full. This has been done in order to save processing power. */ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ #include <stdbool.h> #include <stddef.h> #include <stdint.h> +#if defined(MAG_CAL_DEBUG_ENABLE) && !defined(DIVERSE_DEBUG_ENABLE) +// Ensures that diversity messaging is set when mag_cal debugging is enabled. +#define DIVERSE_DEBUG_ENABLE +#endif // MAG_CAL_DEBUG_ENABLE && !DIVERSE_DEBUG_ENABLE + #ifdef __cplusplus extern "C" { #endif @@ -68,6 +73,17 @@ struct DiversityDbg { }; #endif +// DiversityChecker parameters container. +struct DiversityCheckerParameters { + float var_threshold; + float max_min_threshold; + float local_field; + float threshold_tuning_param; + float max_distance_tuning_param; + size_t min_num_diverse_vectors; + size_t max_num_max_distance; +}; + // Main data struct. struct DiversityChecker { // Data memory. @@ -82,7 +98,7 @@ struct DiversityChecker { // Threshold value that is used to check k against. float threshold; - // Threshold tuning paramter used to calculate threshold (k_algo): + // Threshold tuning parameter used to calculate threshold (k_algo): // threshold = threshold_tuning_param_sq * (local_field)^2. float threshold_tuning_param_sq; @@ -97,7 +113,6 @@ struct DiversityChecker { bool data_full; // Setup variables for NormQuality check. - size_t min_num_diverse_vectors; size_t max_num_max_distance; float var_threshold; @@ -109,7 +124,7 @@ struct DiversityChecker { #endif }; -// Initialization of the function/struct, input: +// Initialization of the function/struct, input parameters struct consists of: // min_num_diverse_vectors -> sets the gate for a minimum number of data points // in the memory // max_num_max_distance -> sets the value for a max distance violation number @@ -123,16 +138,18 @@ struct DiversityChecker { // max_distance_tuning_param -> Max distance tuning parameter used to calculate // max_distance. void diversityCheckerInit(struct DiversityChecker* diverse_data, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, float var_threshold, - float max_min_threshold, float local_field, - float threshold_tuning_param, - float max_distance_tuning_param); + const struct DiversityCheckerParameters* parameters); // Resetting the memory and the counters, leaves threshold and max_distance // as well as the setup variables for NormQuality check untouched. void diversityCheckerReset(struct DiversityChecker* diverse_data); +// Checks if data point (x, y, z) is diverse against the diverse_data set. +// Returns true when the input point is diverse. +// Returns false when a maximum distance check is violated. +bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data, + float x, float y, float z); + // Main function. Tests the data (x,y,z) against the memory if diverse and // stores it, if so. void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, @@ -149,9 +166,7 @@ void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, // -> norm must be within a MAX/MIN window. // Returned value will only be true if all 4 gates are passed. bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, - float x_bias, - float y_bias, - float z_bias); + float x_bias, float y_bias, float z_bias); // This function updates the threshold value and max distance value based on the // local field. This ensures a local field independent operation of the @@ -165,4 +180,4 @@ void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data, } #endif -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_ +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c index 3179b0eb..90b25446 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c @@ -17,6 +17,7 @@ #include "calibration/gyroscope/gyro_cal.h" #include <float.h> +#include <inttypes.h> #include <math.h> #include <string.h> @@ -40,6 +41,10 @@ // A debug version label to help with tracking results. #define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]" +// Parameters used for sample rate estimation. +#define GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS (100) +#define GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC (1.0f) + // Debug log tag string used to identify debug report output data. #define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]" #endif // GYRO_CAL_DBG_ENABLED @@ -103,26 +108,6 @@ enum DebugPrintData { MAG_STATS_TUNING }; -/* - * Updates the running calculation of the gyro's mean sampling rate. - * - * Behavior: - * 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local - * calculation of the sampling rate is copied, and the function returns. - * 2) Else, if 'reset_stats' is 'true' then the local estimate is reset and - * the function returns. - * 3) Otherwise, the local estimate of the mean sampling rates is updated. - * - * INPUTS: - * sample_rate_estimator: Pointer to the estimator data structure. - * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update. - * timestamp_nanos: Time stamp (nanoseconds). - * reset_stats: Flag that signals a reset of the sampling rate estimate. - */ -static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator, - float* debug_mean_sampling_rate_hz, - uint64_t timestamp_nanos, bool reset_stats); - // Updates the information used for debug printouts. static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); @@ -130,35 +115,13 @@ static void gyroCalUpdateDebug(struct GyroCal* gyro_cal); static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, enum DebugPrintData print_data); - -// This conversion function is necessary for Nanohub firmware compilation (i.e., -// can't cast a uint64_t to a float directly). This conversion function was -// copied from: /third_party/contexthub/firmware/src/floatRt.c -static float floatFromUint64(uint64_t v) -{ - uint32_t hi = v >> 32, lo = v; - - if (!hi) //this is very fast for cases where we fit into a uint32_t - return (float)lo; - else { - return ((float)hi) * 4294967296.0f + (float)lo; - } -} #endif // GYRO_CAL_DBG_ENABLED /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the gyro calibration data structure. -void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, - uint64_t max_still_duration_nanos, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos, - uint64_t window_time_duration_nanos, float gyro_var_threshold, - float gyro_confidence_delta, float accel_var_threshold, - float accel_confidence_delta, float mag_var_threshold, - float mag_confidence_delta, float stillness_threshold, - float stillness_mean_delta_limit, - float temperature_delta_limit_celsius, - bool gyro_calibration_enable) { +void gyroCalInit(struct GyroCal* gyro_cal, + const struct GyroCalParameters* parameters) { // Clear gyro_cal structure memory. memset(gyro_cal, 0, sizeof(struct GyroCal)); @@ -166,35 +129,38 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, // Gyro parameter input units are [rad/sec]. // Accel parameter input units are [m/sec^2]. // Magnetometer parameter input units are [uT]. - gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold, - gyro_confidence_delta); - gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold, - accel_confidence_delta); - gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold, - mag_confidence_delta); + gyroStillDetInit(&gyro_cal->gyro_stillness_detect, + parameters->gyro_var_threshold, + parameters->gyro_confidence_delta); + gyroStillDetInit(&gyro_cal->accel_stillness_detect, + parameters->accel_var_threshold, + parameters->accel_confidence_delta); + gyroStillDetInit(&gyro_cal->mag_stillness_detect, + parameters->mag_var_threshold, + parameters->mag_confidence_delta); // Reset stillness flag and start timestamp. gyro_cal->prev_still = false; gyro_cal->start_still_time_nanos = 0; // Set the min and max window stillness duration. - gyro_cal->min_still_duration_nanos = min_still_duration_nanos; - gyro_cal->max_still_duration_nanos = max_still_duration_nanos; + gyro_cal->min_still_duration_nanos = parameters->min_still_duration_nanos; + gyro_cal->max_still_duration_nanos = parameters->max_still_duration_nanos; // Sets the duration of the stillness processing windows. - gyro_cal->window_time_duration_nanos = window_time_duration_nanos; + gyro_cal->window_time_duration_nanos = parameters->window_time_duration_nanos; // Set the watchdog timeout duration. gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS; // Load the last valid cal from system memory. - gyro_cal->bias_x = bias_x; // [rad/sec] - gyro_cal->bias_y = bias_y; // [rad/sec] - gyro_cal->bias_z = bias_z; // [rad/sec] - gyro_cal->calibration_time_nanos = calibration_time_nanos; + gyro_cal->bias_x = parameters->bias_x; // [rad/sec] + gyro_cal->bias_y = parameters->bias_y; // [rad/sec] + gyro_cal->bias_z = parameters->bias_z; // [rad/sec] + gyro_cal->calibration_time_nanos = parameters->calibration_time_nanos; // Set the stillness threshold required for gyro bias calibration. - gyro_cal->stillness_threshold = stillness_threshold; + gyro_cal->stillness_threshold = parameters->stillness_threshold; // Current window end-time used to assist in keeping sensor data collection in // sync. Setting this to zero signals that sensor data will be dropped until a @@ -202,13 +168,14 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, gyro_cal->stillness_win_endtime_nanos = 0; // Gyro calibrations will be applied (see, gyroCalRemoveBias()). - gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0); + gyro_cal->gyro_calibration_enable = (parameters->gyro_calibration_enable > 0); // Sets the stability limit for the stillness window mean acceptable delta. - gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit; + gyro_cal->stillness_mean_delta_limit = parameters->stillness_mean_delta_limit; // Sets the min/max temperature delta limit for the stillness period. - gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius; + gyro_cal->temperature_delta_limit_celsius = + parameters->temperature_delta_limit_celsius; // Ensures that the data tracking functionality is reset. gyroStillMeanTracker(gyro_cal, DO_RESET); @@ -221,41 +188,47 @@ void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos, CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED."); } - // Ensures that the gyro sampling rate estimate is reset. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0, - /*reset_stats=*/true); + // Initializes the gyro sampling rate estimator. + sampleRateEstimatorInit(&gyro_cal->debug_gyro_cal.sample_rate_estimator, + GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS, + GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC); #endif // GYRO_CAL_DBG_ENABLED } // Void pointer in the gyro calibration data structure (doesn't do anything // except prevent compiler warnings). -void gyroCalDestroy(struct GyroCal* gyro_cal) { - (void)gyro_cal; -} +void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; } // Get the most recent bias calibration value. void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, - float* bias_z, float* temperature_celsius) { + float* bias_z, float* temperature_celsius, + uint64_t* calibration_time_nanos) { *bias_x = gyro_cal->bias_x; *bias_y = gyro_cal->bias_y; *bias_z = gyro_cal->bias_z; + *calibration_time_nanos = gyro_cal->calibration_time_nanos; *temperature_celsius = gyro_cal->bias_temperature_celsius; } // Set an initial bias calibration value. void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos) { + float bias_z, float temperature_celsius, + uint64_t calibration_time_nanos) { gyro_cal->bias_x = bias_x; gyro_cal->bias_y = bias_y; gyro_cal->bias_z = bias_z; gyro_cal->calibration_time_nanos = calibration_time_nanos; + gyro_cal->bias_temperature_celsius = temperature_celsius; #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]", - "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET, - CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3), - CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3), - CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3)); + "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, + CAL_ENCODE_FLOAT(bias_x * RAD_TO_MDEG, 3), + CAL_ENCODE_FLOAT(bias_y * RAD_TO_MDEG, 3), + CAL_ENCODE_FLOAT(bias_z * RAD_TO_MDEG, 3), + CAL_ENCODE_FLOAT(temperature_celsius, 3), + calibration_time_nanos); #endif // GYRO_CAL_DBG_ENABLED } @@ -298,8 +271,8 @@ void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time_nanos, #ifdef GYRO_CAL_DBG_ENABLED // Update the gyro sampling rate estimate. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, - sample_time_nanos, /*reset_stats=*/false); + sampleRateEstimatorUpdate(&gyro_cal->debug_gyro_cal.sample_rate_estimator, + sample_time_nanos); #endif // GYRO_CAL_DBG_ENABLED // Pass gyro data to stillness detector @@ -396,7 +369,7 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, // Determines if the device is currently still. device_is_still = (conf_still > gyro_cal->stillness_threshold) && - !mean_not_stable && !min_max_temp_exceeded; + !mean_not_stable && !min_max_temp_exceeded; if (device_is_still) { // Device is "still" logic: @@ -440,12 +413,6 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, computeGyroCal(gyro_cal, gyro_cal->gyro_stillness_detect.last_sample_time); -#ifdef GYRO_CAL_DBG_ENABLED - // Resets the sampling rate estimate. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, - sample_time_nanos, /*reset_stats=*/true); -#endif // GYRO_CAL_DBG_ENABLED - // Update stillness flag. Force the start of a new stillness period. gyro_cal->prev_still = false; } else { @@ -484,12 +451,6 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); gyroStillMeanTracker(gyro_cal, DO_RESET); -#ifdef GYRO_CAL_DBG_ENABLED - // Resets the sampling rate estimate. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, - sample_time_nanos, /*reset_stats=*/true); -#endif // GYRO_CAL_DBG_ENABLED - // Update stillness flag. gyro_cal->prev_still = false; } @@ -501,17 +462,17 @@ void deviceStillnessCheck(struct GyroCal* gyro_cal, // Calculates a new gyro bias offset calibration value. void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { // Check to see if new calibration values is within acceptable range. - if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS && + if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS && - gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS && - gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS && + gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS && gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) { #ifdef GYRO_CAL_DBG_ENABLED CAL_DEBUG_LOG( "[GYRO_CAL:REJECT]", "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET - ", " CAL_FORMAT_3DIGITS ", %llu", + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, CAL_ENCODE_FLOAT( gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( @@ -519,7 +480,7 @@ void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) { CAL_ENCODE_FLOAT( gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3), - (unsigned long long int)calibration_time_nanos); + calibration_time_nanos); #endif // GYRO_CAL_DBG_ENABLED // Outside of range. Ignore, reset, and continue. @@ -580,21 +541,17 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { #ifdef GYRO_CAL_DBG_ENABLED gyro_cal->debug_watchdog_count++; if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) { - CAL_DEBUG_LOG( - "[GYRO_CAL:WATCHDOG]", - "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu", - (unsigned long int)gyro_cal->debug_watchdog_count, - (unsigned long long int)sample_time_nanos, - (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos - - sample_time_nanos)); + CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", + "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64 + ", -%" PRIu64, + gyro_cal->debug_watchdog_count, sample_time_nanos, + gyro_cal->gyro_watchdog_start_nanos - sample_time_nanos); } else { - CAL_DEBUG_LOG( - "[GYRO_CAL:WATCHDOG]", - "Total#, Timestamp | Delta [nsec]: %lu, %llu, %llu", - (unsigned long int)gyro_cal->debug_watchdog_count, - (unsigned long long int)sample_time_nanos, - (unsigned long long int)(sample_time_nanos - - gyro_cal->gyro_watchdog_start_nanos)); + CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]", + "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64 + ", %" PRIu64, + gyro_cal->debug_watchdog_count, sample_time_nanos, + sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos); } #endif // GYRO_CAL_DBG_ENABLED @@ -607,12 +564,6 @@ void checkWatchdog(struct GyroCal* gyro_cal, uint64_t sample_time_nanos) { gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET); gyroStillMeanTracker(gyro_cal, DO_RESET); -#ifdef GYRO_CAL_DBG_ENABLED - // Resets the sampling rate estimate. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, - sample_time_nanos, /*reset_stats=*/true); -#endif // GYRO_CAL_DBG_ENABLED - // Resets the stillness window end-time. gyro_cal->stillness_win_endtime_nanos = 0; @@ -631,7 +582,6 @@ 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; } } @@ -833,53 +783,6 @@ bool gyroStillMeanTracker(struct GyroCal* gyro_cal, } #ifdef GYRO_CAL_DBG_ENABLED -void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator, - float* debug_mean_sampling_rate_hz, - uint64_t timestamp_nanos, bool reset_stats) { - // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads - // out the estimate of the sampling rate. - if (debug_mean_sampling_rate_hz) { - if (sample_rate_estimator->num_samples > 1 && - sample_rate_estimator->time_delta_accumulator > 0) { - // Computes the final mean calculation. - *debug_mean_sampling_rate_hz = - sample_rate_estimator->num_samples / - (floatFromUint64(sample_rate_estimator->time_delta_accumulator) * - NANOS_TO_SEC); - } else { - // Not enough samples to compute a valid sample rate estimate. Indicate - // this with a -1 value. - *debug_mean_sampling_rate_hz = -1.0f; - } - reset_stats = true; - } - - // Resets the sampling rate mean estimator data. - if (reset_stats) { - sample_rate_estimator->last_timestamp_nanos = 0; - sample_rate_estimator->time_delta_accumulator = 0; - sample_rate_estimator->num_samples = 0; - return; - } - - // Skip adding this data to the accumulator if: - // 1. A bad timestamp was received (i.e., time not monotonic). - // 2. 'last_timestamp_nanos' is zero. - if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos || - sample_rate_estimator->last_timestamp_nanos == 0) { - sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; - return; - } - - // Increments the number of samples. - sample_rate_estimator->num_samples++; - - // Accumulate the time steps. - sample_rate_estimator->time_delta_accumulator += - timestamp_nanos - sample_rate_estimator->last_timestamp_nanos; - sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; -} - void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { // Only update this data if debug printing is not currently in progress // (i.e., don't want to risk overwriting debug information that is actively @@ -912,11 +815,6 @@ void gyroCalUpdateDebug(struct GyroCal* gyro_cal) { gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y; gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z; - // Records the mean gyroscope sampling rate. - gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, - &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0, - /*reset_stats=*/true); - // Records the min/max gyroscope window stillness mean values. memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min, sizeof(gyro_cal->gyro_winmean_min)); @@ -983,8 +881,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, CAL_DEBUG_LOG( debug_tag, "Cal#|Offset|Temp|Time [mDPS|C|nsec]: " - "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu", - (unsigned long int)gyro_cal->debug_calibration_count, + "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS + ", %" PRIu64, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT( gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT( @@ -993,8 +892,7 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3), - (unsigned long long int) - gyro_cal->debug_gyro_cal.end_still_time_nanos); + gyro_cal->debug_gyro_cal.end_still_time_nanos); break; case STILLNESS_DATA: @@ -1003,13 +901,11 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, : -1.0f; // Signals that magnetometer was not used. CAL_DEBUG_LOG( debug_tag, - "Cal#|Stillness|Confidence [nsec]: %lu, " - "%llu, " CAL_FORMAT_3DIGITS_TRIPLET, - (unsigned long int)gyro_cal->debug_calibration_count, - (unsigned long long int)(gyro_cal->debug_gyro_cal - .end_still_time_nanos - - gyro_cal->debug_gyro_cal - .start_still_time_nanos), + "Cal#|Stillness|Confidence [nsec]: %zu, " + "%" PRIu64 ", " CAL_FORMAT_3DIGITS_TRIPLET, + gyro_cal->debug_calibration_count, + gyro_cal->debug_gyro_cal.end_still_time_nanos - + gyro_cal->debug_gyro_cal.start_still_time_nanos, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3), CAL_ENCODE_FLOAT(mag_data, 3)); @@ -1019,9 +915,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, CAL_DEBUG_LOG( debug_tag, "Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: " - "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS + "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", " CAL_FORMAT_3DIGITS, - (unsigned long int)gyro_cal->debug_calibration_count, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3), @@ -1029,15 +925,17 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius - gyro_cal->debug_gyro_cal.temperature_min_celsius, 3), - CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3)); + CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.sample_rate_estimator + .mean_sampling_rate_estimate_hz, + 3)); break; case GYRO_MINMAX_STILLNESS_MEAN: CAL_DEBUG_LOG( debug_tag, "Cal#|Gyro Peak Stillness Variation [mDPS]: " - "%lu, " CAL_FORMAT_3DIGITS_TRIPLET, - (unsigned long int)gyro_cal->debug_calibration_count, + "%zu, " CAL_FORMAT_3DIGITS_TRIPLET, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] - gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) * RAD_TO_MDEG, @@ -1055,9 +953,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, case ACCEL_STATS: CAL_DEBUG_LOG(debug_tag, "Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: " - "%lu, " CAL_FORMAT_3DIGITS_TRIPLET + "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS_TRIPLET, - (unsigned long int)gyro_cal->debug_calibration_count, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3), @@ -1069,9 +967,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, case GYRO_STATS: CAL_DEBUG_LOG( debug_tag, - "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET + "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS_TRIPLET, - (unsigned long int)gyro_cal->debug_calibration_count, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG, 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG, @@ -1093,9 +991,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, if (gyro_cal->debug_gyro_cal.using_mag_sensor) { CAL_DEBUG_LOG( debug_tag, - "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET + "Cal#|Mag Mean|Var [uT|uT^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS_TRIPLET, - (unsigned long int)gyro_cal->debug_calibration_count, + gyro_cal->debug_calibration_count, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3), CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3), @@ -1104,9 +1002,9 @@ void gyroCalDebugPrintData(const struct GyroCal* gyro_cal, char* debug_tag, CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6)); } else { CAL_DEBUG_LOG(debug_tag, - "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, " + "Cal#|Mag Mean|Var [uT|uT^2]: %zu, 0, 0, 0, -1.0, -1.0, " "-1.0", - (unsigned long int)gyro_cal->debug_calibration_count); + gyro_cal->debug_calibration_count); } break; diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h index 5e7d5eec..1f17254c 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h @@ -35,7 +35,6 @@ * - Temperature [Celsius] * * #define GYRO_CAL_DBG_ENABLED to enable debug printout statements. - * data to assist in tuning the GyroCal parameters. */ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_ @@ -43,6 +42,10 @@ #include "calibration/gyroscope/gyro_stillness_detect.h" +#ifdef GYRO_CAL_DBG_ENABLED +#include "calibration/sample_rate_estimator/sample_rate_estimator.h" +#endif // GYRO_CAL_DBG_ENABLED + #ifdef __cplusplus extern "C" { #endif @@ -63,10 +66,10 @@ enum GyroCalDebugState { // Gyro Cal debug information/data tracking structure. struct DebugGyroCal { + struct SampleRateEstimator sample_rate_estimator; uint64_t start_still_time_nanos; uint64_t end_still_time_nanos; uint64_t stillness_duration_nanos; - float mean_sampling_rate_hz; float accel_stillness_conf; float gyro_stillness_conf; float mag_stillness_conf; @@ -84,14 +87,28 @@ struct DebugGyroCal { float temperature_mean_celsius; bool using_mag_sensor; }; +#endif // GYRO_CAL_DBG_ENABLED -// Data structure for sample rate estimation. -struct SampleRateData { - uint64_t last_timestamp_nanos; - uint64_t time_delta_accumulator; - size_t num_samples; +// GyroCal algorithm parameters (see GyroCal and GyroStillDet for details). +struct GyroCalParameters { + uint64_t min_still_duration_nanos; + uint64_t max_still_duration_nanos; + uint64_t calibration_time_nanos; + uint64_t window_time_duration_nanos; + float bias_x; // units: radians per second + float bias_y; + float bias_z; + float stillness_threshold; // units: (radians per second)^2 + float stillness_mean_delta_limit; // units: radians per second + float gyro_var_threshold; // units: (radians per second)^2 + float gyro_confidence_delta; // units: (radians per second)^2 + float accel_var_threshold; // units: (meters per second)^2 + float accel_confidence_delta; // units: (meters per second)^2 + float mag_var_threshold; // units: micro-tesla^2 + float mag_confidence_delta; // units: micro-tesla^2 + float temperature_delta_limit_celsius; + bool gyro_calibration_enable; }; -#endif // GYRO_CAL_DBG_ENABLED // Data structure for tracking min/max window mean during device stillness. struct MinMaxWindowMeanData { @@ -149,7 +166,6 @@ struct GyroCal { // Watchdog timer to reset to a known good state when data capture stalls. uint64_t gyro_watchdog_start_nanos; uint64_t gyro_watchdog_timeout_duration_nanos; - bool gyro_watchdog_timeout; // Flag is "true" when the magnetometer is used. bool using_mag_sensor; @@ -177,7 +193,7 @@ struct GyroCal { float temperature_mean_celsius; float temperature_delta_limit_celsius; -//---------------------------------------------------------------- + //---------------------------------------------------------------- #ifdef GYRO_CAL_DBG_ENABLED // Debug info. @@ -185,9 +201,6 @@ struct GyroCal { enum GyroCalDebugState debug_state; // Debug printout state machine. enum GyroCalDebugState next_state; // Debug state machine next state. uint64_t wait_timer_nanos; // Debug message throttle timer. - - struct SampleRateData sample_rate_estimator; // Debug sample rate estimator. - size_t debug_calibration_count; // Total number of cals performed. size_t debug_watchdog_count; // Total number of watchdog timeouts. bool debug_print_trigger; // Flag used to trigger data printout. @@ -197,27 +210,21 @@ struct GyroCal { /////// FUNCTION PROTOTYPES ////////////////////////////////////////// // Initialize the gyro calibration data structure. -void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration, - uint64_t max_still_duration_nanos, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos, - uint64_t window_time_duration_nanos, float gyro_var_threshold, - float gyro_confidence_delta, float accel_var_threshold, - float accel_confidence_delta, float mag_var_threshold, - float mag_confidence_delta, float stillness_threshold, - float stillness_mean_delta_limit, - float temperature_delta_limit_celsius, - bool gyro_calibration_enable); +void gyroCalInit(struct GyroCal* gyro_cal, + const struct GyroCalParameters* parameters); // Void all pointers in the gyro calibration data structure. void gyroCalDestroy(struct GyroCal* gyro_cal); // Get the most recent bias calibration value. void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y, - float* bias_z, float* temperature_celsius); + float* bias_z, float* temperature_celsius, + uint64_t* calibration_time_nanos); // Set an initial bias calibration value. void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y, - float bias_z, uint64_t calibration_time_nanos); + float bias_z, float temperature_celsius, + uint64_t calibration_time_nanos); // Remove gyro bias from the calibration [rad/sec]. void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi, diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c index 80f2fa21..bb2063b6 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c +++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c @@ -15,6 +15,7 @@ */ #include "calibration/gyroscope/gyro_stillness_detect.h" + #include <string.h> /////// FORWARD DECLARATIONS ///////////////////////////////////////// @@ -25,8 +26,8 @@ static float gyroStillDetLimit(float value); /////// FUNCTION DEFINITIONS ///////////////////////////////////////// // Initialize the GyroStillDet structure. -void gyroStillDetInit(struct GyroStillDet* gyro_still_det, - float var_threshold, float confidence_delta) { +void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold, + float confidence_delta) { // Clear all data structure variables to 0. memset(gyro_still_det, 0, sizeof(struct GyroStillDet)); @@ -192,12 +193,12 @@ float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) { // Each axis score is limited [0,1]. tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh); gyro_still_det->stillness_confidence = - gyroStillDetLimit( - 0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) * - gyroStillDetLimit( - 0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) * - gyroStillDetLimit( - 0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom); + gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) * + tmp_denom) * + gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) * + tmp_denom) * + gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) * + tmp_denom); } } @@ -207,8 +208,7 @@ float gyroStillDetCompute(struct GyroStillDet* gyro_still_det) { // Resets the stillness detector and initiates a new detection window. // 'reset_stats' determines whether the stillness statistics are reset. -void gyroStillDetReset(struct GyroStillDet* gyro_still_det, - bool reset_stats) { +void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) { float tmp_denom = 1.f; // Reset the stillness data ready flag. diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h index 9a1d876c..51c4bee6 100644 --- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h +++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h @@ -60,7 +60,7 @@ struct GyroStillDet { // is used to keep track of the window start time. bool start_new_window; - // Starting time stamp for the the current window. + // Starting time stamp for the current window. uint64_t window_start_time; // Accumulator variables for tracking the sample mean during @@ -93,8 +93,8 @@ struct GyroStillDet { /////// FUNCTION PROTOTYPES ////////////////////////////////////////// // Initialize the gyro_still_det_t structure. -void gyroStillDetInit(struct GyroStillDet* gyro_still_det, - float var_threshold, float confidence_delta); +void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold, + float confidence_delta); // Update the stillness detector with a new sample. void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det, diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c deleted file mode 100644 index 7f8e563f..00000000 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.c +++ /dev/null @@ -1,520 +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/magnetometer/mag_cal.h" - -#include <errno.h> -#include <string.h> - -#include "calibration/util/cal_log.h" - -#ifdef MAG_CAL_ORIGINAL_TUNING -#define MAX_EIGEN_RATIO 25.0f -#define MAX_EIGEN_MAG 80.0f // uT -#define MIN_EIGEN_MAG 10.0f // uT -#define MAX_FIT_MAG 80.0f -#define MIN_FIT_MAG 10.0f -#define MAX_BATCH_WINDOW 15000000UL // 15 sec -#define MIN_BATCH_SIZE 25 // samples -#else -#define MAX_EIGEN_RATIO 15.0f -#define MAX_EIGEN_MAG 70.0f // uT -#define MIN_EIGEN_MAG 20.0f // uT -#define MAX_FIT_MAG 70.0f -#define MIN_FIT_MAG 20.0f -#define MAX_BATCH_WINDOW 15000000UL // 15 sec -#define MIN_BATCH_SIZE 25 // samples -#endif - -#ifdef DIVERSITY_CHECK_ENABLED -#define MAX_DISTANCE_VIOLATIONS 2 -#ifdef SPHERE_FIT_ENABLED -# define MAX_ITERATIONS 30 -# define INITIAL_U_SCALE 1.0e-4f -# define GRADIENT_THRESHOLD 1.0e-16f -# define RELATIVE_STEP_THRESHOLD 1.0e-7f -# define FROM_MICRO_SEC_TO_SEC 1.0e-6f -#endif -#endif - -// eigen value magnitude and ratio test -static int moc_eigen_test(struct KasaFit *kasa) { - // 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; - - struct Vec3 eigenvals; - struct Mat33 eigenvecs; - mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); - - float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; - evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; - - float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; - evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; - - float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; - - // Testing for negative number. - float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; - - int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) && - (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG); - - return eigen_pass; -} - -// Kasa sphere fitting with normal equation -int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) { - // A * out = b - // (4 x 4) (4 x 1) (4 x 1) - struct Mat44 A; - A.elem[0][0] = kasa->acc_xx; - A.elem[0][1] = kasa->acc_xy; - A.elem[0][2] = kasa->acc_xz; - A.elem[0][3] = kasa->acc_x; - A.elem[1][0] = kasa->acc_xy; - A.elem[1][1] = kasa->acc_yy; - A.elem[1][2] = kasa->acc_yz; - A.elem[1][3] = kasa->acc_y; - A.elem[2][0] = kasa->acc_xz; - A.elem[2][1] = kasa->acc_yz; - A.elem[2][2] = kasa->acc_zz; - A.elem[2][3] = kasa->acc_z; - A.elem[3][0] = kasa->acc_x; - A.elem[3][1] = kasa->acc_y; - A.elem[3][2] = kasa->acc_z; - A.elem[3][3] = 1.0f; - - struct Vec4 b; - initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w); - - struct Size4 pivot; - mat44DecomposeLup(&A, &pivot); - - struct Vec4 out; - mat44Solve(&A, &out, &b, &pivot); - - // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 - // - // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2 - // r = sqrt(xc^2 + yc^2 + zc^2 - out[3]) - - struct Vec3 v; - initVec3(&v, out.x, out.y, out.z); - vec3ScalarMul(&v, -0.5f); - - float r_square = vec3Dot(&v, &v) - out.w; - float r = (r_square > 0) ? sqrtf(r_square) : 0; - - initVec3(bias, v.x, v.y, v.z); - *radius = r; - - int success = 0; - if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) { - success = 1; - } - - return success; -} - -void magKasaReset(struct KasaFit *kasa) { - kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f; - kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f; - kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f; - kasa->acc_zz = kasa->acc_zw = 0.0f; - - kasa->nsamples = 0; -} - -void magCalReset(struct MagCal *moc) { - magKasaReset(&moc->kasa); -#ifdef DIVERSITY_CHECK_ENABLED - diversityCheckerReset(&moc->diversity_checker); -#endif - moc->start_time = 0; - moc->kasa_batching = false; -} - -static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { - int complete = 0; - - if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) && - (moc->kasa.nsamples > MIN_BATCH_SIZE)) { - complete = 1; - - } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { - // not enough samples collected in MAX_BATCH_WINDOW or too many - // maximum distance violations detected. - magCalReset(moc); - } - - return complete; -} - -void initKasa(struct KasaFit *kasa) { - magKasaReset(kasa); -} - -void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias, - float c00, float c01, float c02, float c10, float c11, - float c12, float c20, float c21, float c22, - uint32_t min_batch_window_in_micros -#ifdef DIVERSITY_CHECK_ENABLED - ,size_t min_num_diverse_vectors - ,size_t max_num_max_distance - ,float var_threshold - ,float max_min_threshold - ,float local_field - ,float threshold_tuning_param - ,float max_distance_tuning_param -#endif - ) { - magCalReset(moc); - moc->update_time = 0; - moc->min_batch_window_in_micros = min_batch_window_in_micros; - moc->radius = 0.0f; - - moc->x_bias = x_bias; - moc->y_bias = y_bias; - moc->z_bias = z_bias; - - moc->c00 = c00; - moc->c01 = c01; - moc->c02 = c02; - moc->c10 = c10; - moc->c11 = c11; - moc->c12 = c12; - moc->c20 = c20; - moc->c21 = c21; - moc->c22 = c22; - -#ifdef MAG_CAL_DEBUG_ENABLE - moc->mag_dbg.mag_trigger_count = 0; - moc->mag_dbg.kasa_count = 0; -#endif - -#ifdef DIVERSITY_CHECK_ENABLED - // Diversity Checker - diversityCheckerInit(&moc->diversity_checker, - min_num_diverse_vectors, - max_num_max_distance, - var_threshold, - max_min_threshold, - local_field, - threshold_tuning_param, - max_distance_tuning_param); -#endif -} - -void magCalDestroy(struct MagCal *moc) { (void)moc; } - -enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, - float x, float y, float z) { - enum MagUpdate new_bias = NO_UPDATE; - -#ifdef DIVERSITY_CHECK_ENABLED - // Diversity Checker Update. - diversityCheckerUpdate(&moc->diversity_checker, x, y, z); -#endif - - // 1. run accumulators - 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->kasa.acc_xx += x * x; - moc->kasa.acc_xy += x * y; - moc->kasa.acc_xz += x * z; - moc->kasa.acc_xw += x * w; - - moc->kasa.acc_yy += y * y; - moc->kasa.acc_yz += y * z; - moc->kasa.acc_yw += y * w; - - moc->kasa.acc_zz += z * z; - moc->kasa.acc_zw += z * w; - - if (++moc->kasa.nsamples == 1) { - moc->start_time = sample_time_us; - moc->kasa_batching = true; - } - - // 2. batch has enough samples? - if (moc_batch_complete(moc, sample_time_us)) { - float inv = 1.0f / moc->kasa.nsamples; - - moc->kasa.acc_x *= inv; - moc->kasa.acc_y *= inv; - moc->kasa.acc_z *= inv; - moc->kasa.acc_w *= inv; - - moc->kasa.acc_xx *= inv; - moc->kasa.acc_xy *= inv; - moc->kasa.acc_xz *= inv; - moc->kasa.acc_xw *= inv; - - moc->kasa.acc_yy *= inv; - moc->kasa.acc_yz *= inv; - moc->kasa.acc_yw *= inv; - - moc->kasa.acc_zz *= inv; - moc->kasa.acc_zw *= inv; - - // 3. eigen test - if (moc_eigen_test(&moc->kasa)) { - struct Vec3 bias; - float radius; - // 4. Kasa sphere fitting - if (magKasaFit(&moc->kasa, &bias, &radius)) { - -#ifdef MAG_CAL_DEBUG_ENABLE - moc->mag_dbg.kasa_count++; - CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :,", - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %lu, %lu", - CAL_ENCODE_FLOAT(bias.x, 3), - CAL_ENCODE_FLOAT(bias.y, 3), - CAL_ENCODE_FLOAT(bias.z, 3), - CAL_ENCODE_FLOAT(radius, 3), - (unsigned long int)moc->mag_dbg.kasa_count, - (unsigned long int)moc->mag_dbg.mag_trigger_count); -#endif - -#ifdef DIVERSITY_CHECK_ENABLED - // Update the local field. - diversityCheckerLocalFieldUpdate(&moc->diversity_checker, - radius); - - // checking if data is diverse. - if (diversityCheckerNormQuality(&moc->diversity_checker, - bias.x, - bias.y, - bias.z) && - moc->diversity_checker.num_max_dist_violations - <= MAX_DISTANCE_VIOLATIONS) { - - // DEBUG PRINT OUT. -#ifdef MAG_CAL_DEBUG_ENABLE - moc->mag_dbg.mag_trigger_count++; -#ifdef DIVERSE_DEBUG_ENABLE - moc->diversity_checker.diversity_dbg.new_trigger = 1; - CAL_DEBUG_LOG("[MAG_CAL:BIAS UPDATE] :, ", - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d," - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %zu, %s%d.%03d, " - "%s%d.%03d, %lu, %lu, %llu, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %llu", - CAL_ENCODE_FLOAT(bias.x, 3), - CAL_ENCODE_FLOAT(bias.y, 3), - CAL_ENCODE_FLOAT(bias.z, 3), - CAL_ENCODE_FLOAT(radius, 3), - CAL_ENCODE_FLOAT( - moc->diversity_checker.diversity_dbg.var_log, 6), - CAL_ENCODE_FLOAT( - moc->diversity_checker.diversity_dbg.mean_log, 3), - CAL_ENCODE_FLOAT( - moc->diversity_checker.diversity_dbg.max_log, 3), - CAL_ENCODE_FLOAT( - moc->diversity_checker.diversity_dbg.min_log, 3), - moc->diversity_checker.num_points, - CAL_ENCODE_FLOAT( - moc->diversity_checker.threshold, 3), - CAL_ENCODE_FLOAT( - moc->diversity_checker.max_distance, 3), - (unsigned long int)moc->mag_dbg.mag_trigger_count, - (unsigned long int)moc->mag_dbg.kasa_count, - (unsigned long long int)sample_time_us, - CAL_ENCODE_FLOAT(moc->x_bias, 3), - CAL_ENCODE_FLOAT(moc->y_bias, 3), - CAL_ENCODE_FLOAT(moc->z_bias, 3), - (unsigned long long int)moc->update_time); -#endif -#endif -#endif - 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 = UPDATE_BIAS; - -#ifdef DIVERSITY_CHECK_ENABLED - } -#endif - } - } - - // 5. reset for next batch - magCalReset(moc); - } - - return new_bias; -} - -void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) { - *x = moc->x_bias; - *y = moc->y_bias; - *z = moc->z_bias; -} - -void magCalAddBias(struct MagCal *moc, float x, float y, float z) { - moc->x_bias += x; - moc->y_bias += y; - moc->z_bias += z; -} - -void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, - float *xo, float *yo, float *zo) { - *xo = xi - moc->x_bias; - *yo = yi - moc->y_bias; - *zo = zi - moc->z_bias; -} - -void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02, - float c10, float c11, float c12, float c20, float c21, - float c22) { - moc->c00 = c00; - moc->c01 = c01; - moc->c02 = c02; - moc->c10 = c10; - moc->c11 = c11; - moc->c12 = c12; - moc->c20 = c20; - moc->c21 = c21; - moc->c22 = c22; -} - -void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, - float *xo, float *yo, float *zo) { - *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; - *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; - *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; -} - -#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSE_DEBUG_ENABLE -// This function prints every second sample parts of the dbg diverse_data_log, -// which ensures that all the messages get printed into the log file. -void magLogPrint(struct DiversityChecker* diverse_data, float temp) { - // Sample counter. - static size_t sample_counter = 0; - const float* data_log_ptr = - &diverse_data->diversity_dbg.diverse_data_log[0]; - if (diverse_data->diversity_dbg.new_trigger == 1) { - sample_counter++; - if (sample_counter == 2) { - CAL_DEBUG_LOG("[MAG_CAL:MEMORY X] :,", - "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d" - ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d", - (unsigned long int)diverse_data-> - diversity_dbg.diversity_count, - CAL_ENCODE_FLOAT(data_log_ptr[0*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[1*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[2*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[3*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[4*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[5*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[6*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[7*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[8*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[9*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[10*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[11*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[12*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[13*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[14*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[15*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[16*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[17*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[18*3], 3), - CAL_ENCODE_FLOAT(data_log_ptr[19*3], 3), - CAL_ENCODE_FLOAT(temp, 3)); - } - - if (sample_counter == 4) { - CAL_DEBUG_LOG("[MAG_CAL:MEMORY Y] :,", - "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d" - ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ", - (unsigned long int)diverse_data-> - diversity_dbg.diversity_count, - CAL_ENCODE_FLOAT(data_log_ptr[0*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[1*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[2*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[3*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[4*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[5*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[6*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[7*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[8*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[9*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[10*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[11*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[12*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[13*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[14*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[15*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[16*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[17*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[18*3+1], 3), - CAL_ENCODE_FLOAT(data_log_ptr[19*3+1], 3)); - } - if (sample_counter == 6) { - CAL_DEBUG_LOG("[MAG_CAL:MEMORY Z] :,", - "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d" - ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " - "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ", - (unsigned long int)diverse_data-> - diversity_dbg.diversity_count, - CAL_ENCODE_FLOAT(data_log_ptr[0*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[1*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[2*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[3*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[4*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[5*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[6*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[7*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[8*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[9*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[10*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[11*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[12*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[13*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[14*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[15*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[16*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[17*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[18*3+2], 3), - CAL_ENCODE_FLOAT(data_log_ptr[19*3+2], 3)); - sample_counter = 0; - diverse_data->diversity_dbg.new_trigger = 0; - } - } -} -#endif diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c new file mode 100644 index 00000000..c3f12ae6 --- /dev/null +++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c @@ -0,0 +1,358 @@ +/* + * 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/magnetometer/mag_cal/mag_cal.h" + +#include <errno.h> +#include <inttypes.h> +#include <string.h> + +#include "calibration/util/cal_log.h" + +// Local helper macro for printing log messages. +#ifdef MAG_CAL_DEBUG_ENABLE +#ifdef CAL_NO_FLOAT_FORMAT_STRINGS +#define CAL_FORMAT_MAG_MEMORY \ + "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ + "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ + "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ + "%s%d.%03d, %s%d.%03d" +#else +#define CAL_FORMAT_MAG_MEMORY \ + "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, " \ + "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f" +#endif // CAL_NO_FLOAT_FORMAT_STRINGS +#endif // MAG_CAL_DEBUG_ENABLE + +// clang-format off +#define MAX_EIGEN_RATIO 15.0f +#define MAX_EIGEN_MAG 70.0f // uT +#define MIN_EIGEN_MAG 20.0f // uT +#define MAX_FIT_MAG 70.0f +#define MIN_FIT_MAG 20.0f +#define MAX_BATCH_WINDOW 15000000UL // 15 sec +#define MIN_BATCH_SIZE 25 // samples +#define MAX_DISTANCE_VIOLATIONS 2 +// clang-format + +// eigen value magnitude and ratio test. +static int moc_eigen_test(struct KasaFit *kasa) { + // 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; + + struct Vec3 eigenvals; + struct Mat33 eigenvecs; + mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); + + float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; + evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; + + float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; + evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; + + float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; + + // Testing for negative number. + float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; + + int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) && + (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG); + + return eigen_pass; +} + +void magCalReset(struct MagCal *moc) { + kasaReset(&moc->kasa); + diversityCheckerReset(&moc->diversity_checker); + moc->start_time = 0; + moc->kasa_batching = false; +} + +static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { + bool complete = false; + + if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) && + (moc->kasa.nsamples > MIN_BATCH_SIZE)) { + complete = true; + + } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { + // not enough samples collected in MAX_BATCH_WINDOW or too many + // maximum distance violations detected. + magCalReset(moc); + } + + return complete; +} + +void initMagCal(struct MagCal *moc, + const struct MagCalParameters *mag_cal_parameters, + const struct DiversityCheckerParameters *diverse_parameters) { + magCalReset(moc); + moc->update_time = 0; + moc->min_batch_window_in_micros = + mag_cal_parameters->min_batch_window_in_micros; + moc->radius = 0.0f; + + moc->x_bias = mag_cal_parameters->x_bias; + moc->y_bias = mag_cal_parameters->y_bias; + moc->z_bias = mag_cal_parameters->z_bias; + + moc->c00 = mag_cal_parameters->c00; + moc->c01 = mag_cal_parameters->c01; + moc->c02 = mag_cal_parameters->c02; + moc->c10 = mag_cal_parameters->c10; + moc->c11 = mag_cal_parameters->c11; + moc->c12 = mag_cal_parameters->c12; + moc->c20 = mag_cal_parameters->c20; + moc->c21 = mag_cal_parameters->c21; + moc->c22 = mag_cal_parameters->c22; + +#ifdef MAG_CAL_DEBUG_ENABLE + moc->mag_dbg.mag_trigger_count = 0; + moc->mag_dbg.kasa_count = 0; +#endif // MAG_CAL_DEBUG_ENABLE + + // Diversity Checker + diversityCheckerInit(&moc->diversity_checker, diverse_parameters); +} + +void magCalDestroy(struct MagCal *moc) { (void)moc; } + +enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, + float x, float y, float z) { + enum MagUpdate new_bias = NO_UPDATE; + + // Diversity Checker Update. + diversityCheckerUpdate(&moc->diversity_checker, x, y, z); + + // 1. run accumulators + kasaAccumulate(&moc->kasa, x, y, z); + + if (moc->kasa.nsamples == 1) { + moc->start_time = sample_time_us; + moc->kasa_batching = true; + } + + // 2. batch has enough samples? + if (moc_batch_complete(moc, sample_time_us)) { + kasaNormalize(&moc->kasa); + + // 3. eigen test + if (moc_eigen_test(&moc->kasa)) { + struct Vec3 bias; + float radius; + // 4. Kasa sphere fitting + if (kasaFit(&moc->kasa, &bias, &radius, MAX_FIT_MAG, MIN_FIT_MAG)) { +#ifdef MAG_CAL_DEBUG_ENABLE + moc->mag_dbg.kasa_count++; + CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32, + CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3), + CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3), + moc->mag_dbg.kasa_count, moc->mag_dbg.mag_trigger_count); +#endif // MAG_CAL_DEBUG_ENABLE + + // Update the local field. + diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius); + + // checking if data is diverse. + if (diversityCheckerNormQuality(&moc->diversity_checker, bias.x, bias.y, + bias.z) && + moc->diversity_checker.num_max_dist_violations <= + MAX_DISTANCE_VIOLATIONS) { + // DEBUG PRINT OUT. +#ifdef MAG_CAL_DEBUG_ENABLE + moc->mag_dbg.mag_trigger_count++; + moc->diversity_checker.diversity_dbg.new_trigger = 1; + CAL_DEBUG_LOG( + "[MAG_CAL:BIAS UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET ", " + CAL_FORMAT_3DIGITS ", " CAL_FORMAT_6DIGITS ", " + CAL_FORMAT_3DIGITS_TRIPLET ", %zu, " CAL_FORMAT_3DIGITS ", " + CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32 ", %" PRIu64 ", " + CAL_FORMAT_3DIGITS_TRIPLET ", %" PRIu64 "", + CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3), + CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3), + CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.var_log, 6), + CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.mean_log, + 3), + CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.max_log, 3), + CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.min_log, 3), + moc->diversity_checker.num_points, + CAL_ENCODE_FLOAT(moc->diversity_checker.threshold, 3), + CAL_ENCODE_FLOAT(moc->diversity_checker.max_distance, 3), + moc->mag_dbg.mag_trigger_count, + moc->mag_dbg.kasa_count, + sample_time_us, + CAL_ENCODE_FLOAT(moc->x_bias, 3), + CAL_ENCODE_FLOAT(moc->y_bias, 3), + CAL_ENCODE_FLOAT(moc->z_bias, 3), + moc->update_time); +#endif // MAG_CAL_DEBUG_ENABLE + 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 = UPDATE_BIAS; + } + } + } + // 5. reset for next batch + magCalReset(moc); + } + + return new_bias; +} + +void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z) { + *x = moc->x_bias; + *y = moc->y_bias; + *z = moc->z_bias; +} + +void magCalAddBias(struct MagCal *moc, float x, float y, float z) { + moc->x_bias += x; + moc->y_bias += y; + moc->z_bias += z; +} + +void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, + float *xo, float *yo, float *zo) { + *xo = xi - moc->x_bias; + *yo = yi - moc->y_bias; + *zo = zi - moc->z_bias; +} + +void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02, + float c10, float c11, float c12, float c20, float c21, + float c22) { + moc->c00 = c00; + moc->c01 = c01; + moc->c02 = c02; + moc->c10 = c10; + moc->c11 = c11; + moc->c12 = c12; + moc->c20 = c20; + moc->c21 = c21; + moc->c22 = c22; +} + +void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, + float *xo, float *yo, float *zo) { + *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; + *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; + *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; +} + +#if defined MAG_CAL_DEBUG_ENABLE +// This function prints every second sample parts of the dbg diverse_data_log, +// which ensures that all the messages get printed into the log file. +void magLogPrint(struct DiversityChecker *diverse_data, float temp) { + // Sample counter. + static size_t sample_counter = 0; + const float *data_log_ptr = &diverse_data->diversity_dbg.diverse_data_log[0]; + if (diverse_data->diversity_dbg.new_trigger == 1) { + sample_counter++; + if (sample_counter == 2) { + CAL_DEBUG_LOG( + "[MAG_CAL:MEMORY X] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY ", " + CAL_FORMAT_3DIGITS, + diverse_data->diversity_dbg.diversity_count, + CAL_ENCODE_FLOAT(data_log_ptr[0 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[1 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[2 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[3 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[4 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[5 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[6 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[7 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[8 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[9 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[10 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[11 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[12 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[13 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[14 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[15 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[16 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[17 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[18 * 3], 3), + CAL_ENCODE_FLOAT(data_log_ptr[19 * 3], 3), CAL_ENCODE_FLOAT(temp, 3)); + } + + if (sample_counter == 4) { + CAL_DEBUG_LOG( + "[MAG_CAL:MEMORY Y] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY, + diverse_data->diversity_dbg.diversity_count, + CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 1], 3), + CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 1], 3)); + } + if (sample_counter == 6) { + CAL_DEBUG_LOG( + "[MAG_CAL:MEMORY Z] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY, + diverse_data->diversity_dbg.diversity_count, + CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 2], 3), + CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 2], 3)); + sample_counter = 0; + diverse_data->diversity_dbg.new_trigger = 0; + } + } +} +#endif // MAG_CAL_DEBUG_ENABLE diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h index 8c9da6fc..566afa88 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal.h +++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h @@ -13,21 +13,16 @@ * 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_ -#ifdef SPHERE_FIT_ENABLED -#ifndef DIVERSITY_CHECK_ENABLED -#define DIVERSITY_CHECK_ENABLED -#endif -#endif +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_ #include <stdbool.h> #include <stdint.h> #include <sys/types.h> -#ifdef DIVERSITY_CHECK_ENABLED -#include "calibration/common/diversity_checker.h" -#endif + +#include "calibration/diversity_checker/diversity_checker.h" +#include "common/math/kasa.h" #include "common/math/mat.h" #include "common/math/vec.h" @@ -35,17 +30,13 @@ extern "C" { #endif -struct KasaFit { - float acc_x, acc_y, acc_z, acc_w; - float acc_xx, acc_xy, acc_xz, acc_xw; - float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw; - size_t nsamples; -}; - enum MagUpdate { NO_UPDATE = 0x00, UPDATE_BIAS = 0x01, UPDATE_SPHERE_FIT = 0x02, + UPDATE_BIAS_MAGGYRO_MEDIUM = 0x04, + UPDATE_BIAS_MAGGYRO_HIGH = 0x08, + MAGGYRO_TIMEOUT = 0x10, }; #ifdef MAG_CAL_DEBUG_ENABLE @@ -55,17 +46,32 @@ struct MagDbg { }; #endif +// MagCal algorithm parameters (see MagCal for details). +struct MagCalParameters { + uint32_t min_batch_window_in_micros; + float x_bias; // [micro-Tesla] + float y_bias; // [micro-Tesla] + float z_bias; // [micro-Tesla] + float c00; + float c01; + float c02; + float c10; + float c11; + float c12; + float c20; + float c21; + float c22; +}; + struct MagCal { -#ifdef DIVERSITY_CHECK_ENABLED struct DiversityChecker diversity_checker; -#endif struct KasaFit kasa; - uint64_t start_time; - uint64_t update_time; + uint64_t start_time; // [micro-seconds] + uint64_t update_time; // [micro-seconds] uint32_t min_batch_window_in_micros; float x_bias, y_bias, z_bias; - float radius; + float radius; // [micro-Tesla] bool kasa_batching; float c00, c01, c02, c10, c11, c12, c20, c21, c22; @@ -74,29 +80,16 @@ struct MagCal { #endif }; -void initKasa(struct KasaFit *kasa); - -#ifdef DIVERSITY_CHECK_ENABLED -void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias, - float c00, float c01, float c02, float c10, float c11, - float c12, float c20, float c21, float c22, - uint32_t min_batch_window_in_micros, - size_t min_num_diverse_vectors, size_t max_num_max_distance, - float var_threshold, float max_min_threshold, float local_field, - float threshold_tuning_param, float max_distance_tuning_param); -#else -void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias, - float c00, float c01, float c02, float c10, float c11, - float c12, float c20, float c21, float c22, - uint32_t min_batch_window_in_micros); -#endif +void initMagCal(struct MagCal *moc, + const struct MagCalParameters *mag_cal_parameters, + const struct DiversityCheckerParameters *diverse_parameters); void magCalDestroy(struct MagCal *moc); enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, float x, float y, float z); -void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z); +void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z); void magCalAddBias(struct MagCal *moc, float x, float y, float z); @@ -110,13 +103,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); - -#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSITY_CHECK_ENABLED +#if defined MAG_CAL_DEBUG_ENABLE void magLogPrint(struct DiversityChecker *moc, float temp); #endif @@ -124,4 +113,4 @@ void magLogPrint(struct DiversityChecker *moc, float temp); } #endif -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_ +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_ diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c index eb23b876..93c2ac61 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c +++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c @@ -14,13 +14,11 @@ * limitations under the License. */ -#include "calibration/magnetometer/mag_sphere_fit.h" +#include "calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h" #include <errno.h> #include <string.h> -#include "calibration/util/cal_log.h" - #define MAX_ITERATIONS 30 #define INITIAL_U_SCALE 1.0e-4f #define GRADIENT_THRESHOLD 1.0e-16f @@ -33,22 +31,16 @@ void magCalSphereReset(struct MagCalSphere *mocs) { memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data)); } -void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias, - float z_bias, float c00, float c01, float c02, float c10, - float c11, float c12, float c20, float c21, float c22, - uint32_t min_batch_window_in_micros, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, float var_threshold, - float max_min_threshold, float local_field, - float threshold_tuning_param, - float max_distance_tuning_param) { - initMagCal(&mocs->moc, x_bias, y_bias, z_bias, c00, c01, c02, c10, c11, c12, - c20, c21, c22, min_batch_window_in_micros, min_num_diverse_vectors, - max_num_max_distance, var_threshold, max_min_threshold, - local_field, threshold_tuning_param, max_distance_tuning_param); +void initMagCalSphere( + struct MagCalSphere *mocs, + const struct MagCalParameters *mag_cal_parameters, + const struct DiversityCheckerParameters *diverse_parameters, + float default_odr_in_hz) { + initMagCal(&mocs->moc, mag_cal_parameters, diverse_parameters); mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA; mocs->batch_time_in_sec = - (float)(min_batch_window_in_micros) * FROM_MICRO_SEC_TO_SEC; + (float)(mag_cal_parameters->min_batch_window_in_micros) * + FROM_MICRO_SEC_TO_SEC; // Initialize to take every sample, default setting. mocs->sample_drop = 0; magCalSphereReset(mocs); @@ -63,6 +55,9 @@ void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias, sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal, &mocs->sphere_fit.lm_data); calDataReset(&mocs->sphere_fit.sphere_param); + + // Initializes the starting output data rate estimate. + magCalSphereOdrUpdate(mocs, default_odr_in_hz); } void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; } @@ -91,7 +86,7 @@ void magCalSphereDataUpdate(struct MagCalSphere *mocs, float x, float y, if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) { memcpy(&mocs->sphere_data[mocs->number_of_data_samples * THREE_AXIS_DATA_DIM], - vec, sizeof(float) * 3); + vec, sizeof(float) * THREE_AXIS_DATA_DIM); // counting the numbers of samples in the data set. mocs->number_of_data_samples++; } @@ -114,8 +109,7 @@ enum MagUpdate magCalSphereFit(struct MagCalSphere *mocs, // Running the sphere fit and checking if successful. if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) { - // Updating Sphere parameters. Can use "calDataCorrectData" function to - // correct data. + // Updating sphere parameters. sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal, &mocs->sphere_fit.sphere_param); diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h index 85df48fc..3ae4687b 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h +++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h @@ -13,11 +13,12 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_ -#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_ -#include "calibration/common/sphere_fit_calibration.h" -#include "calibration/magnetometer/mag_cal.h" +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_ + +#include "calibration/magnetometer/mag_cal/mag_cal.h" +#include "calibration/sphere_fit/sphere_fit_calibration.h" #define NUM_SPHERE_FIT_DATA 50 @@ -50,24 +51,17 @@ struct MagCalSphere { float sphere_data[THREE_AXIS_DATA_DIM * NUM_SPHERE_FIT_DATA]; }; -void initMagCalSphere(struct MagCalSphere *mocs, - float x_bias, float y_bias, float z_bias, - float c00, float c01, float c02, float c10, float c11, - float c12, float c20, float c21, float c22, - uint32_t min_batch_window_in_micros, - size_t min_num_diverse_vectors, - size_t max_num_max_distance, - float var_threshold, - float max_min_threshold, - float local_field, - float threshold_tuning_param, - float max_distance_tuning_param); +void initMagCalSphere( + struct MagCalSphere *mocs, + const struct MagCalParameters *mag_cal_parameters, + const struct DiversityCheckerParameters *diverse_parameters, + float default_odr_in_hz); void magCalSphereDestroy(struct MagCalSphere *mocs); enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs, - uint64_t sample_time_us, - float x, float y, float z); + uint64_t sample_time_us, float x, float y, + float z); void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz); @@ -75,4 +69,4 @@ void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz); } #endif -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_ +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_ diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h new file mode 100644 index 00000000..12b84701 --- /dev/null +++ b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h @@ -0,0 +1,125 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_ + +#ifdef ACCEL_CAL_ENABLED +#include "calibration/accelerometer/accel_cal.h" +#endif // ACCEL_CAL_ENABLED + +#ifdef GYRO_CAL_ENABLED +#include "calibration/gyroscope/gyro_cal.h" +#include "calibration/over_temp/over_temp_cal.h" +#endif // GYRO_CAL_ENABLED + +#ifdef MAG_CAL_ENABLED +#include "calibration/diversity_checker/diversity_checker.h" +#include "calibration/magnetometer/mag_cal/mag_cal.h" +#endif // MAG_CAL_ENABLED + +#include "common/math/macros.h" + +namespace nano_calibration { + +//////// ACCELEROMETER CALIBRATION //////// +#ifdef ACCEL_CAL_ENABLED +constexpr AccelCalParameters kAccelCalParameters{ + MSEC_TO_NANOS(800), // t0 + 5, // n_s + 15, // fx + 15, // fxb + 15, // fy + 15, // fyb + 15, // fz + 15, // fzb + 15, // fle + 0.00025f // th +}; +#endif // ACCEL_CAL_ENABLED + +//////// GYROSCOPE CALIBRATION //////// +#ifdef GYRO_CAL_ENABLED +constexpr GyroCalParameters kGyroCalParameters{ + SEC_TO_NANOS(1.4), // min_still_duration_nanos + SEC_TO_NANOS(1.4), // max_still_duration_nanos [see, NOTE 1] + 0, // calibration_time_nanos + MSEC_TO_NANOS(500), // window_time_duration_nanos + 0, // bias_x + 0, // bias_y + 0, // bias_z + 0.95f, // stillness_threshold + MDEG_TO_RAD * 60.0f, // stillness_mean_delta_limit [rad/sec] + 3.0e-5f, // gyro_var_threshold [rad/sec]^2 + 3.0e-6f, // gyro_confidence_delta [rad/sec]^2 + 4.5e-3f, // accel_var_threshold [m/sec^2]^2 + 9.0e-4f, // accel_confidence_delta [m/sec^2]^2 + 5.0f, // mag_var_threshold [uTesla]^2 + 1.0f, // mag_confidence_delta [uTesla]^2 + 1.5f, // temperature_delta_limit_celsius + true // gyro_calibration_enable +}; +// [NOTE 1]: 'max_still_duration_nanos' is set to 1.4 seconds to achieve a max +// stillness period of 1.5 seconds and avoid buffer boundary conditions that +// could push the max stillness to the next multiple of the analysis window +// length (i.e., 2.0 seconds). + +constexpr OverTempCalParameters kGyroOtcParameters{ + MSEC_TO_NANOS(100), // min_temp_update_period_nanos + DAYS_TO_NANOS(2), // age_limit_nanos + 0.75f, // delta_temp_per_bin + 40.0f * MDEG_TO_RAD, // jump_tolerance + 100.0f * MDEG_TO_RAD, // outlier_limit + 250.0f * MDEG_TO_RAD, // temp_sensitivity_limit + 8.0e3f * MDEG_TO_RAD, // sensor_intercept_limit + 0.1f * MDEG_TO_RAD, // significant_offset_change + 5, // min_num_model_pts + true // over_temp_enable +}; +#endif // GYRO_CAL_ENABLED + +//////// MAGNETOMETER CALIBRATION //////// +#ifdef MAG_CAL_ENABLED +constexpr MagCalParameters kMagCalParameters{ + 3000000, // min_batch_window_in_micros + 0.0f, // x_bias + 0.0f, // y_bias + 0.0f, // z_bias + 1.0f, // c00 + 0.0f, // c01 + 0.0f, // c02 + 0.0f, // c10 + 1.0f, // c11 + 0.0f, // c12 + 0.0f, // c20 + 0.0f, // c21 + 1.0f // c22 +}; + +constexpr DiversityCheckerParameters kMagDiversityParameters{ + 6.0f, // var_threshold + 10.0f, // max_min_threshold + 48.0f, // local_field + 0.5f, // threshold_tuning_param + 2.552f, // max_distance_tuning_param + 8, // min_num_diverse_vectors + 1 // max_num_max_distance +}; +#endif // MAG_CAL_ENABLED + +} // namespace nano_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_ diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc new file mode 100644 index 00000000..0df2ae6b --- /dev/null +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc @@ -0,0 +1,416 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/nano_calibration/nano_calibration.h" + +#include <cstring> + +#include "chre/util/nanoapp/log.h" + +namespace nano_calibration { +namespace { + +using ::online_calibration::CalibrationDataThreeAxis; +using ::online_calibration::CalibrationTypeFlags; +using ::online_calibration::SensorData; +using ::online_calibration::SensorIndex; +using ::online_calibration::SensorType; + +// NanoSensorCal logging macros. +#ifdef NANO_SENSOR_CAL_DBG_ENABLED +#ifndef LOG_TAG +#define LOG_TAG "[ImuCal]" +#endif +#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__) +#else +#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) +#define NANO_CAL_LOGI(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__) +#endif // NANO_SENSOR_CAL_DBG_ENABLED + +} // namespace + +void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal, + OnlineCalibrationThreeAxis *gyro_cal, + OnlineCalibrationThreeAxis *mag_cal) { + // Loads stored calibration data and initializes the calibration algorithms. + accel_cal_ = accel_cal; + if (accel_cal_ != nullptr) { + if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) { + LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_, + &accel_cal_update_flags_, kAccelTag); + NANO_CAL_LOGI(kAccelTag, + "Accelerometer runtime calibration initialized."); + } else { + accel_cal_ = nullptr; + NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type."); + } + } + + gyro_cal_ = gyro_cal; + if (gyro_cal_ != nullptr) { + if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) { + LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_, + &gyro_cal_update_flags_, kGyroTag); + NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized."); + } else { + gyro_cal_ = nullptr; + NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type."); + } + } + + mag_cal_ = mag_cal; + if (mag_cal != nullptr) { + if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) { + LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_, + &mag_cal_update_flags_, kMagTag); + NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized."); + } else { + mag_cal_ = nullptr; + NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type."); + } + } +} + +void NanoSensorCal::HandleSensorSamples( + uint16_t event_type, const chreSensorThreeAxisData *event_data) { + // Converts CHRE Event -> SensorData::SensorType. + SensorData sample; + switch (event_type) { + case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA: + sample.type = SensorType::kAccelerometerMps2; + break; + case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA: + sample.type = SensorType::kGyroscopeRps; + break; + case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA: + sample.type = SensorType::kMagnetometerUt; + break; + default: + // This sensor type is not used. + NANO_CAL_LOGW("[NanoSensorCal]", + "Unexpected 3-axis sensor type received."); + return; + } + + // Sends the sensor payload to the calibration algorithms and checks for + // calibration updates. + const auto &header = event_data->header; + const auto *data = event_data->readings; + sample.timestamp_nanos = header.baseTimestamp; + for (size_t i = 0; i < header.readingCount; i++) { + sample.timestamp_nanos += data[i].timestampDelta; + memcpy(sample.data, data[i].v, sizeof(sample.data)); + ProcessSample(sample); + } +} + +void NanoSensorCal::HandleTemperatureSamples( + uint16_t event_type, const chreSensorFloatData *event_data) { + // Computes the mean of the batched temperature samples and delivers it to the + // calibration algorithms. Note, the temperature sensor batch size determines + // its minimum update interval. + if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA && + event_data->header.readingCount > 0) { + const auto header = event_data->header; + const auto *data = event_data->readings; + + SensorData sample; + sample.type = SensorType::kTemperatureCelsius; + sample.timestamp_nanos = header.baseTimestamp; + + float accum_temperature_celsius = 0.0f; + for (size_t i = 0; i < header.readingCount; i++) { + sample.timestamp_nanos += data[i].timestampDelta; + accum_temperature_celsius += data[i].value; + } + sample.data[SensorIndex::kSingleAxis] = + accum_temperature_celsius / header.readingCount; + ProcessSample(sample); + } else { + NANO_CAL_LOGW("[NanoSensorCal]", + "Unexpected single-axis sensor type received."); + } +} + +void NanoSensorCal::ProcessSample(const SensorData &sample) { + // Sends a new sensor sample to each active calibration algorithm and sends + // out notifications for new calibration updates. + if (accel_cal_ != nullptr) { + const CalibrationTypeFlags new_cal_flags = + accel_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + accel_cal_update_flags_ |= new_cal_flags; + NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, + accel_cal_->GetSensorCalibration(), + accel_cal_update_flags_, kAccelTag); + PrintCalibration(accel_cal_->GetSensorCalibration(), + accel_cal_update_flags_, kAccelTag); + } + } + + if (gyro_cal_ != nullptr) { + const CalibrationTypeFlags new_cal_flags = + gyro_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + gyro_cal_update_flags_ |= new_cal_flags; + if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, + gyro_cal_->GetSensorCalibration(), + gyro_cal_update_flags_, kGyroTag)) { + // Limits the log messaging update rate for the gyro calibrations since + // these can occur frequently with rapid temperature changes. + if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( + sample.timestamp_nanos, gyro_notification_time_nanos_, + kNanoSensorCalMessageIntervalNanos)) { + gyro_notification_time_nanos_ = sample.timestamp_nanos; + PrintCalibration(gyro_cal_->GetSensorCalibration(), + gyro_cal_update_flags_, kGyroTag); + } + } + } + } + + if (mag_cal_ != nullptr) { + const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample); + if (new_cal_flags != CalibrationTypeFlags::NONE) { + mag_cal_update_flags_ |= new_cal_flags; + NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, + mag_cal_->GetSensorCalibration(), + mag_cal_update_flags_, kMagTag); + PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_, + kMagTag); + } + } +} + +bool NanoSensorCal::NotifyAshCalibration( + uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data, + CalibrationTypeFlags flags, const char *sensor_tag) { + // Updates the sensor offset calibration using the ASH API. + ashCalInfo ash_cal_info; + memset(&ash_cal_info, 0, sizeof(ashCalInfo)); + ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor). + ash_cal_info.compMatrix[4] = 1.0f; + ash_cal_info.compMatrix[8] = 1.0f; + memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias)); + + // Maps CalibrationQualityLevel to ASH calibration accuracy. + switch (cal_data.calibration_quality.level) { + case online_calibration::CalibrationQualityLevel::HIGH_QUALITY: + ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH; + break; + + case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY: + ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM; + break; + + case online_calibration::CalibrationQualityLevel::LOW_QUALITY: + ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW; + break; + + default: + ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE; + break; + } + + if (!ashSetCalibration(chreSensorType, &ash_cal_info)) { + NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update."); + return false; + } + + // Uses the ASH API to store all calibration parameters relevant to a given + // algorithm as indicated by the input calibration type flags. + ashCalParams ash_cal_parameters; + memset(&ash_cal_parameters, 0, sizeof(ashCalParams)); + if (flags & CalibrationTypeFlags::BIAS) { + ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius; + memcpy(ash_cal_parameters.offset, cal_data.offset, + sizeof(ash_cal_parameters.offset)); + ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; + ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; + } + + if (flags & CalibrationTypeFlags::OVER_TEMP) { + memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity, + sizeof(ash_cal_parameters.tempSensitivity)); + memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept, + sizeof(ash_cal_parameters.tempIntercept)); + ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME; + ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME; + } + + if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) { + NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update."); + return false; + } + + return true; +} + +bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, + OnlineCalibrationThreeAxis *online_cal, + CalibrationTypeFlags* flags, + const char *sensor_tag) { + ashCalParams recalled_ash_cal_parameters; + if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH, + &recalled_ash_cal_parameters)) { + // Checks whether a valid set of runtime calibration parameters was received + // and can be used for initialization. + if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags, + &recalled_ash_cal_parameters)) { + CalibrationDataThreeAxis cal_data; + cal_data.type = online_cal->get_sensor_type(); + cal_data.cal_update_time_nanos = chreGetTime(); + + // Analyzes the calibration flags and sets only the runtime calibration + // values that were received. + if (*flags & CalibrationTypeFlags::BIAS) { + cal_data.offset_temp_celsius = + recalled_ash_cal_parameters.offsetTempCelsius; + memcpy(cal_data.offset, recalled_ash_cal_parameters.offset, + sizeof(cal_data.offset)); + } + + if (*flags & CalibrationTypeFlags::OVER_TEMP) { + memcpy(cal_data.temp_sensitivity, + recalled_ash_cal_parameters.tempSensitivity, + sizeof(cal_data.temp_sensitivity)); + memcpy(cal_data.temp_intercept, + recalled_ash_cal_parameters.tempIntercept, + sizeof(cal_data.temp_intercept)); + } + + // Sets the algorithm's initial calibration data and notifies ASH to apply + // the recalled calibration data. + if (online_cal->SetInitialCalibration(cal_data)) { + return NotifyAshCalibration(chreSensorType, + online_cal->GetSensorCalibration(), *flags, + sensor_tag); + } else { + NANO_CAL_LOGE(sensor_tag, + "Calibration data failed to initialize algorithm."); + } + } + } else { + NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data."); + } + + return false; +} + +bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType, + const char *sensor_tag, + CalibrationTypeFlags *flags, + ashCalParams *ash_cal_parameters) { + // Analyzes calibration source flags to determine whether runtime + // calibration values have been loaded and may be used for initialization. A + // valid runtime calibration source will include at least an offset. + *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags. + + // Uses the ASH calibration source flags to set the appropriate + // CalibrationTypeFlags. These will be used to determine which values to copy + // from 'ash_cal_parameters' and provide to the calibration algorithms for + // initialization. + bool runtime_cal_detected = false; + if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME && + ash_cal_parameters->offsetTempCelsiusSource == + ASH_CAL_PARAMS_SOURCE_RUNTIME) { + runtime_cal_detected = true; + *flags = CalibrationTypeFlags::BIAS; + } + + if (ash_cal_parameters->tempSensitivitySource == + ASH_CAL_PARAMS_SOURCE_RUNTIME && + ash_cal_parameters->tempInterceptSource == + ASH_CAL_PARAMS_SOURCE_RUNTIME) { + *flags |= CalibrationTypeFlags::OVER_TEMP; + } + + if (runtime_cal_detected) { + // Prints the retrieved runtime calibration data. + NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected."); + PrintAshCalParams(*ash_cal_parameters, sensor_tag); + } else { + // This is a warning (not an error) since the runtime algorithms will + // function correctly with no recalled calibration values. They will + // eventually trigger and update the system with valid calibration data. + NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found."); + } + + return runtime_cal_detected; +} + +// Helper functions for logging calibration information. +void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params, + const char *sensor_tag) { + if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, + "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f", + cal_params.offset[0], cal_params.offset[1], + cal_params.offset[2], cal_params.offsetTempCelsius); + } + + if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f", + cal_params.tempSensitivity[0], cal_params.tempSensitivity[1], + cal_params.tempSensitivity[2]); + } + + if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f", + cal_params.tempIntercept[0], cal_params.tempIntercept[1], + cal_params.tempIntercept[2]); + } + + if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f", + cal_params.scaleFactor[0], cal_params.scaleFactor[1], + cal_params.scaleFactor[2]); + } + + if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) { + NANO_CAL_LOGI(sensor_tag, + "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f", + cal_params.crossAxis[0], cal_params.crossAxis[1], + cal_params.crossAxis[2]); + } +} + +void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data, + CalibrationTypeFlags flags, + const char *sensor_tag) { + if (flags & CalibrationTypeFlags::BIAS) { + NANO_CAL_LOGI(sensor_tag, + "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f", + cal_data.offset[0], cal_data.offset[1], cal_data.offset[2], + cal_data.offset_temp_celsius); + } + + if (flags & CalibrationTypeFlags::OVER_TEMP) { + NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f", + cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1], + cal_data.temp_sensitivity[2]); + NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f", + cal_data.temp_intercept[0], cal_data.temp_intercept[1], + cal_data.temp_intercept[2]); + } +} + +} // namespace nano_calibration diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h new file mode 100644 index 00000000..d56d0347 --- /dev/null +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h @@ -0,0 +1,165 @@ +/* + * Copyright (C) 2017 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides a containing class (NanoSensorCal) for dynamic runtime + * calibration algorithms that affect the following sensors: + * - Accelerometer (offset) + * - Gyroscope (offset, with over-temperature compensation) + * - Magnetometer (offset) + * + * Sensor Units: + * - Accelerometer [meters/sec^2] + * - Gyroscope [radian/sec] + * - Magnetometer [micro Tesla, uT] + * - Temperature [Celsius]. + * + * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging. + * + * NOTE2: This module uses pointers to runtime calibration algorithm objects. + * These must be constructed and initialized outside of this class. The owner + * bares the burden of managing the lifetime of these objects with respect to + * the NanoSensorCal class which depends on these objects and handles their + * interaction with the Android ASH/CHRE system. This arrangement makes it + * convenient to modify the specific algorithm implementations (i.e., choice of + * calibration algorithm, parameter tuning, etc.) at the nanoapp level without + * the need to specialize the standard functionality implemented here. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ + +#include <stdbool.h> +#include <stdint.h> + +#include <ash.h> +#include <chre.h> + +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" +#include "common/math/macros.h" + +namespace nano_calibration { + +// Common log message sensor-specific identifiers. +constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"}; +constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"}; +constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"}; + +// Limits NanoSensorCal notifications to once every minute. +constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1); + +/* + * NanoSensorCal is a container class for dynamic runtime calibration sensor + * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class + * is to transfer sensor data to the sensor calibration algorithms and provide + * calibration updates to CHRE using the ASH API. + */ +class NanoSensorCal { + public: + // Alias used to reference the three-axis OnlineCalibration baseclass used by + // the runtime calibration sensor wrappers. This is for convenience and to + // help with code readability. + using OnlineCalibrationThreeAxis = online_calibration::OnlineCalibration< + online_calibration::CalibrationDataThreeAxis>; + + NanoSensorCal() = default; + + // Sets the sensor calibration object pointers and initializes the algorithms + // using runtime values recalled using Android Sensor Hub (ASH). A nullptr may + // be passed in to disable a particular sensor calibration. + void Initialize(OnlineCalibrationThreeAxis *accel_cal, + OnlineCalibrationThreeAxis *gyro_cal, + OnlineCalibrationThreeAxis *mag_cal); + + // Sends new sensor samples to the calibration algorithms. + void HandleSensorSamples(uint16_t event_type, + const chreSensorThreeAxisData *event_data); + + // Provides temperature updates to the calibration algorithms. + void HandleTemperatureSamples(uint16_t event_type, + const chreSensorFloatData *event_data); + + private: + // Passes sensor data to the runtime calibration algorithms. + void ProcessSample(const online_calibration::SensorData &sample); + + // Loads runtime calibration data using the Android Sensor Hub API. Returns + // 'true' when runtime calibration values were successfully recalled and used + // for algorithm initialization. 'sensor_tag' is a string that identifies a + // sensor-specific identifier for log messages. Updates 'flags' to indicate + // which runtime calibration parameters were recalled. + bool LoadAshCalibration(uint8_t chreSensorType, + OnlineCalibrationThreeAxis *online_cal, + online_calibration::CalibrationTypeFlags* flags, + const char *sensor_tag); + + // Provides sensor calibration updates using the ASH API for the specified + // sensor type. 'cal_data' contains the new calibration data. 'flags' is used + // to indicate all of the valid calibration values that should be provided + // with the update. Returns 'true' with a successful ASH update. + bool NotifyAshCalibration( + uint8_t chreSensorType, + const online_calibration::CalibrationDataThreeAxis &cal_data, + online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); + + // Checks whether 'ash_cal_parameters' is a valid set of runtime calibration + // data and can be used for algorithm initialization. Updates 'flags' to + // indicate which runtime calibration parameters were detected. + bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag, + online_calibration::CalibrationTypeFlags *flags, + ashCalParams *ash_cal_parameters); + + // Helper functions for logging calibration information. + void PrintAshCalParams(const ashCalParams &cal_params, + const char *sensor_tag); + + void PrintCalibration( + const online_calibration::CalibrationDataThreeAxis &cal_data, + online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); + + // Pointer to the accelerometer runtime calibration object. + OnlineCalibrationThreeAxis *accel_cal_ = nullptr; + + // Pointer to the gyroscope runtime calibration object. + OnlineCalibrationThreeAxis *gyro_cal_ = nullptr; + + // Limits the log messaging update rate for the gyro calibrations since these + // can occur frequently with rapid temperature changes. + uint64_t gyro_notification_time_nanos_ = 0; + + // Pointer to the magnetometer runtime calibration object. + OnlineCalibrationThreeAxis *mag_cal_ = nullptr; + + // Flags that determine which calibration elements are updated with the ASH + // API. These are reset during initialization, and latched when a particular + // calibration update is detected upon a valid recall of parameters and/or + // during runtime. The latching behavior is used to start sending calibration + // values of a given type (e.g., bias, over-temp model, etc.) once they are + // detected and thereafter. + online_calibration::CalibrationTypeFlags accel_cal_update_flags_ = + online_calibration::CalibrationTypeFlags::NONE; + online_calibration::CalibrationTypeFlags gyro_cal_update_flags_ = + online_calibration::CalibrationTypeFlags::NONE; + online_calibration::CalibrationTypeFlags mag_cal_update_flags_ = + online_calibration::CalibrationTypeFlags::NONE; +}; + +} // namespace nano_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc new file mode 100644 index 00000000..e9def4e6 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +void AccelOffsetCal::Initialize( + const AccelCalParameters& accel_cal_parameters) { + accelCalInit(&accel_cal_, &accel_cal_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + switch (sample.type) { + case SensorType::kAccelerometerMps2: + accelCalRun(&accel_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis], // [m/sec^2] + temperature_celsius_); + +#ifdef ACCEL_CAL_DBG_ENABLED + // Prints debug data report. + accelCalDebPrint(&accel_cal_, temperature_celsius_); +#endif + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new offset estimate, and updates the calibration data. + if (accelCalNewBiasAvailable(&accel_cal_)) { + accelCalUpdateBias(&accel_cal_, &cal_data_.offset[0], &cal_data_.offset[1], + &cal_data_.offset[2]); + + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality; + cal_data_.offset_temp_celsius = temperature_celsius_; + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_polling_flags_ = CalibrationTypeFlags::BIAS; + OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS); + } + + return cal_update_polling_flags_; +} + +bool AccelOffsetCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[AccelOffsetCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sets the accelerometer algorithm's calibration data. + accelCalBiasSet(&accel_cal_, input_cal_data.offset[0], + input_cal_data.offset[1], input_cal_data.offset[2]); + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the calibration quality. + cal_data_.calibration_quality.level = CalibrationQualityLevel::LOW_QUALITY; + cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality; + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h new file mode 100644 index 00000000..0256495a --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ + +#include "calibration/accelerometer/accel_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This class is a wrapper for accelerometer offset calibration. + * + * NOTE: Calibration quality reporting (quantitative metric is not used): + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::LOW_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + */ +class AccelOffsetCal final + : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Default constructor. + AccelOffsetCal() = default; + + // Creates an AccelOffsetCal with specified algorithm parameters. + explicit AccelOffsetCal(const AccelCalParameters& accel_cal_parameters) { + Initialize(accel_cal_parameters); + } + + // Initializes with specified algorithm parameters, and resets the calibration + // data. + void Initialize(const AccelCalParameters& accel_cal_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kAccelerometerMps2; + }; + + private: + // Accelerometer offset calibration algorithm data structure. + AccelCal accel_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h new file mode 100644 index 00000000..62bc443d --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides the callback functionality employed by the online sensor + * calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ + +#include "calibration/online_calibration/common_data/calibration_data.h" + +namespace online_calibration { + +/* + * This codebase avoids Standard Template Library (STL) containers to maximize + * its usefullness on embedded hardware with basic C++ compiler support. The + * following uses type erasure to implement callback functionality to a user's + * desired class method. The idea here is to store an object pointer by + * instantiating a class template (Callback) which implements a virtual + * interface function (CallbackInterface::Call). + * + * USAGE: + * See unit testing for a simple example of how to use this for callback + * functionality. + */ + +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class CalibrationType> +class CallbackInterface { + public: + // Interface function that is defined to implement the desired callback. + virtual void Call(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags) = 0; + virtual ~CallbackInterface() {} +}; + +// ClassCallerType: Sets the object's class type for the callback. +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class ClassCallerType, class CalibrationType> +class Callback : public CallbackInterface<CalibrationType> { + public: + // Constructors. + Callback() = delete; + Callback(ClassCallerType* obj, + void (ClassCallerType::*func)(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags)) + : object_ptr_(obj), function_ptr_(func) {} + + // Implements the callback to the desired class-method. + void Call(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags) final { + (object_ptr_->*function_ptr_)(cal_data, cal_update_flags); + } + + private: + // Pointer to the class that owns the callback method. + ClassCallerType* object_ptr_; + + // Templated function pointer with the required function signature. + // Calibration callbacks must accept: + // 1. Constant reference to the calibration. + // 2. Bitmask indicating which calibration components have been updated. + void (ClassCallerType::*function_ptr_)(const CalibrationType& cal_data, + CalibrationTypeFlags cal_update_flags); +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc new file mode 100644 index 00000000..8f1470a8 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/online_calibration/common_data/calibration_data.h" + +namespace online_calibration { + +CalibrationTypeFlags operator|(CalibrationTypeFlags lhs, + CalibrationTypeFlags rhs) { + return static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) | + static_cast<char>(rhs)); +} + +bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs) { + return static_cast<char>(lhs) & static_cast<char>(rhs); +} + +CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs, + CalibrationTypeFlags rhs) { + lhs = static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) | + static_cast<char>(rhs)); + return lhs; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h new file mode 100644 index 00000000..0404936b --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h @@ -0,0 +1,228 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides the component definitions used to represent sensor + * calibration data, labeled flags/enumerators, and the callback functionality + * employed by the online sensor calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ + +#include <stdint.h> +#include <string.h> +#include <sys/types.h> + +#include "calibration/online_calibration/common_data/calibration_quality.h" +#include "calibration/online_calibration/common_data/sensor_data.h" +#include "calibration/over_temp/over_temp_model.h" + +namespace online_calibration { + +/* + * Bitmask used to indicate which calibration values have changed for a given + * calibration update. + * + * [Bit Flag] | [Affected Sensor Calibration Value] + * BIAS - Quasi-static non-zero sensor output (offset), given + * conditions where the sensor should ideally output zero. + * Includes corrections for over-temperature compensation. + * SCALE_FACTOR - Sensor axis scaling (ideally unity). + * CROSS_AXIS - Output sensitivity due to variations of perpendicular + * sensing axes (ideally zero). + * OVER_TEMP - Model parameters that capture variations in sensor + * behavior with temperature (e.g., linear bias sensitivity + * model). + * QUALITY_DEGRADED - Indicates a degradation in calibration quality. + */ +enum class CalibrationTypeFlags : uint8_t { + NONE = 0x00, + BIAS = 0x01, + SCALE_FACTOR = 0x02, + CROSS_AXIS = 0x04, + OVER_TEMP = 0x08, + QUALITY_DEGRADED = 0x10, + ALL = 0xFF +}; + +// Logic operators to assist with common bitmask setting/checking. +CalibrationTypeFlags operator|(CalibrationTypeFlags lhs, + CalibrationTypeFlags rhs); + +bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs); + +CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs, + CalibrationTypeFlags rhs); + +/* + * Defines the calibration data specific to a prototypical three-axis sensing + * device (e.g., accelerometer, gyroscope, magnetometer). + * + * Calibration correction may be applied as: + * corrected_data = scale_skew_matrix * (input_data - offset); + * + * 'offset' is the sensor bias estimate (with temperature compensation applied + * when supported by the underlying calibration algorithm). + * + * The 'scale_skew_matrix' is assumed to be in lower diagonal form where the + * sensor frame reference definition is such that cross-axis sensitivities + * cross_axis_xy, cross_axis_xz, and cross_axis_yz are set to zero. + * + * scale_skew_matrix = [scale_factor_x 0 0 + * cross_axis_yx scale_factor_y 0 + * cross_axis_zx cross_axis_zy scale_factor_z] + * + * NOTE1: If over-temperature compensation is provided, then temperature + * compensation is already applied to the 'offset'. Coefficients representative + * of the sensor's temperature dependency model are provided, and may be used + * for model initialization after a system restart. + * + * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope). + * temp_intercept - Linear model intercept. + * + * The model equation for the over-temperature compensated offset: + * compensated_offset = temp_sensitivity * current_temp + temp_intercept + * + * NOTE2: Unless otherwise stated, 3-dimensional array indices: 0=x, 1=y, 2=z. + */ +struct CalibrationDataThreeAxis { + // Timestamp for the most recent calibration update. + uint64_t cal_update_time_nanos = 0; + + // The sensor's offset (i.e., bias) in the x-, y-, z-axes at + // 'offset_temp_celsius'. + float offset[3]; + + // The temperature associated with the sensor offset. + float offset_temp_celsius; + + // The temperature sensitivity of the offset. + float temp_sensitivity[3]; // [sensor_units/Celsius] + + // The sensor's offset at zero degrees Celsius. + float temp_intercept[3]; // [sensor_units] + + // The sensor's scale factor for each axis. + float scale_factor[3]; + + // The cross-axis factors in order: [0=yx, 1=zx, 2=zy]. + float cross_axis[3]; + + // Metrics used for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + CalibrationQuality calibration_quality; + + // Indicates the type of sensing device being calibrated. + SensorType type = SensorType::kUndefined; + + // Optional pointer to an array of over-temperature model data (null when not + // used). For initialization, populating a model dataset will take precedence + // over the linear model parameters provided in the calibration data. + OverTempModelThreeAxis* otc_model_data = nullptr; + int16_t num_model_pts = 0; + + // Helper function that resets the calibration data to a set of neutral + // reference values where no calibration correction would be applied if used. + void reset() { + otc_model_data = nullptr; + calibration_quality.reset(); + offset_temp_celsius = 0.0f; + scale_factor[0] = 1.0f; + scale_factor[1] = 1.0f; + scale_factor[2] = 1.0f; + memset(offset, 0, sizeof(offset)); + memset(temp_sensitivity, 0, sizeof(temp_sensitivity)); + memset(temp_intercept, 0, sizeof(temp_intercept)); + memset(cross_axis, 0, sizeof(cross_axis)); + } + + CalibrationDataThreeAxis() { reset(); } +}; + +/* + * Defines the calibration data for single dimensional sensing device (e.g., + * thermometer, barometer). + * + * Calibration correction may be applied as: + * corrected_data = scale_factor * (input_data - offset); + * + * 'offset' is the sensor bias estimate (with temperature compensation applied, + * if supported by the underlying calibration algorithm). + * + * NOTE: If over-temperature compensation is provided, then temperature + * compensation is already applied to the 'offset'. Coefficients representative + * of the sensor's temperature dependency model are provided, and may be used + * for model initialization after a system restart. + * + * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope). + * temp_intercept - Linear model intercept. + * + * The model equation for the over-temperature compensated offset: + * compensated_offset = temp_sensitivity * current_temp + temp_intercept + */ +struct CalibrationDataSingleAxis { + // Timestamp for the most recent calibration update. + uint64_t cal_update_time_nanos = 0; + + // The sensor's offset (i.e., bias) at temperature, 'offset_temp_celsius'. + float offset; + + // The temperature associated with the sensor offset. + float offset_temp_celsius; + + // The temperature sensitivity of the offset. + float temp_sensitivity; // [sensor_units/Celsius] + + // The sensor's offset at zero degrees Celsius. + float temp_intercept; // [sensor_units] + + // The sensor's scale factor. + float scale_factor; + + // Metrics used for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + CalibrationQuality calibration_quality; + + // Indicates the type of sensing device being calibrated. + SensorType type = SensorType::kUndefined; + + // Optional pointer to an array of over-temperature model data (null when not + // used). For initialization, populating a model dataset will take precedence + // over the linear model parameters provided in the calibration data. + OverTempModelSingleAxis* otc_model_data = nullptr; + int16_t num_model_pts = 0; + + // Helper function that resets the calibration data to a set of neutral + // reference values where no calibration correction would be applied if used. + void reset() { + otc_model_data = nullptr; + calibration_quality.reset(); + scale_factor = 1.0f; + offset_temp_celsius = 0.0f; + offset = 0.0f; + temp_sensitivity = 0.0f; + temp_intercept = 0.0f; + } + + CalibrationDataSingleAxis() { reset(); } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h new file mode 100644 index 00000000..d6475c78 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides quality definitions that represent the accuracy + * associated with calibration data. This information may be used to affect how + * a system uses available sensor calibration data. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ + +#include <float.h> +#include <stdint.h> + +namespace online_calibration { + +/* + * In general, calibration quality values may be thought of in terms of + * something akin to an error standard deviation. That is, it's a non-negative + * value where lower values imply higher calibration quality, and larger values + * indicate poorer quality. However, the units and numerical interpretation is + * ultimately dictated by the sensor type and calibration algorithm. Consult the + * calibration code implementation for the actual calibration quality metric + * details. + */ + +/* + * Bitmask used to provide a qualitative ranking of the calibration data's + * accuracy. Many systems use this sort of interpretation for calibration + * quality (e.g., Android). + * + * [Bit Flag] | [Qualitative Calibration Quality] + * UNDETERMINED - Calibration quality has not yet been determined (e.g., a + * calibration hasn't occurred, or a metric hasn't been + * established). + * LOW_QUALITY - Sensor calibration is needed. System properties have + * changed that may have affected the applicability of the + * current calibration (e.g., calibration data is old, anomaly + * detected, etc.). + * MEDIUM_QUALITY - The reported sensor calibration has an average level of + * accuracy, updated calibration may improve the readings. + * HIGH_QUALITY - The reported calibration has maximal accuracy. + */ +enum class CalibrationQualityLevel : uint8_t { + UNDETERMINED = 0x00, + LOW_QUALITY = 0x01, + MEDIUM_QUALITY = 0x02, + HIGH_QUALITY = 0x04, +}; + +// Sets the calibration quality value when this metric is either not +// implemented, or has not yet been determined (e.g., a calibration hasn't +// occurred). +constexpr float kUndeterminedCalibrationQuality = -1.0f; + +/* + * Calibration quality structure that contains a quantitative (float) and + * qualitative (enum) measure of a sensor's calibration accuracy. Both entries + * should be co-defined by the algorithm providing the calibration update. The + * enum sets the qualitative interpretation of the float value, this is often + * used in systems (Android, etc.) to label quality and affect the use of the + * calibration data. + */ +struct CalibrationQuality { + // Provides a qualitative measure for sensor calibration accuracy. + CalibrationQualityLevel level = CalibrationQualityLevel::UNDETERMINED; + + // Quantitative metric for the reported calibration accuracy. The behavior and + // definition depends on the sensor calibration type. See the calibration + // algorithm documentation for details. + float value = kUndeterminedCalibrationQuality; + + // Helper function that resets the calibration quality to an initial state. + void reset() { + level = CalibrationQualityLevel::UNDETERMINED; + value = kUndeterminedCalibrationQuality; + } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h new file mode 100644 index 00000000..59e26bae --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ + +#include <string.h> + +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This abstract base class provides a set of general interface functions for + * calibration algorithms. The data structures used are intended to be lean and + * portable to a wide variety of software and hardware systems. Algorithm + * wrappers may use this as a basis for providing the following functionality: + * + * SetMeasurement - Delivers new sensor data. + * SetInitialCalibration - Initializes the algorithm's calibration data. + * GetSensorCalibration - Retrieves the latest calibration data set. + * new_calibration_ready - Used to poll for new calibration updates. + * SetCalibrationCallback - User provides a pointer its own Callback object. + * UpdateDynamicSystemSettings - Provides feedback to adjust system behavior. + * get_sensor_type - Returns the sensor type which is being calibrated. + * + * NOTE 1: This class accomodates two methods of providing calibration updates. + * Either, or both, may be used depending on system requirements. 1) Polling can + * be achieved with new_calibration_ready/GetSensorCalibration functions. 2) + * Callback notification of new calibration updates can managed using the + * SetCalibrationCallback function. + * + * NOTE 2: This code implementation specifically avoids using standard template + * libraries (STL) and other external API’s since they may not be fully + * supported on embedded hardware targets. Only basic C/C++ support will be + * assumed. + */ + +// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis). +template <class CalibrationType> +class OnlineCalibration { + public: + // Virtual destructor. + virtual ~OnlineCalibration() {} + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0; + + // Sets the initial calibration data of the calibration algorithm. Returns + // "true" if set successfully. + virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0; + + // Polling Updates: New calibration updates are generated during + // SetMeasurement and the 'cal_update_polling_flags_' are set according to + // which calibration values have changed. To prevent missing updates in + // systems that use polling, this bitmask remains latched until the + // calibration data is retrieved with this function. + const CalibrationType& GetSensorCalibration() const { + cal_update_polling_flags_ = CalibrationTypeFlags::NONE; + return cal_data_; + } + + // Polling Updates: This function returns 'cal_update_polling_flags_' to + // indicate which calibration components have a pending update. The updated + // calibration data may be retrieved with GetSensorCalibration, and the + // 'cal_update_polling_flags_' will reset. + CalibrationTypeFlags new_calibration_ready() const { + return cal_update_polling_flags_; + } + + // Sets the pointer to the CallbackInterface object used for notification of + // new calibration updates. + void SetCalibrationCallback( + CallbackInterface<CalibrationType>* calibration_callback) { + calibration_callback_ = calibration_callback; + } + + // Returns the sensor-type this calibration algorithm provides updates for. + virtual SensorType get_sensor_type() const = 0; + + protected: + // Helper function that activates the registered callback. + void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const { + if (calibration_callback_ != nullptr) { + calibration_callback_->Call(cal_data_, cal_update_flags); + } + } + + // Helper function used to initialize the calibration data. + void InitializeCalData() { + cal_data_.reset(); + cal_data_.type = get_sensor_type(); + cal_update_polling_flags_ = CalibrationTypeFlags::NONE; + } + + // Stores the sensor calibration data. + CalibrationType cal_data_; + + // Tracks the most recent sensor temperature value. + float temperature_celsius_ = kInvalidTemperatureCelsius; + + // This bitmask indicates which subset of calibration parameters have changed + // and is used specifically for polling; the callback notification passes its + // own set of update flags which do not need this latching behavior. Marked + // mutable so that these flags may be reset when GetSensorCalibration is + // called. + mutable CalibrationTypeFlags cal_update_polling_flags_ = + CalibrationTypeFlags::NONE; + + private: + // Pointer to a callback object. + CallbackInterface<CalibrationType>* calibration_callback_ = nullptr; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_ diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h new file mode 100644 index 00000000..1331153e --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * This module provides the component definitions used to represent sensor + * data employed by the online sensor calibration algorithms. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ + +#include <stdint.h> +#include <string.h> +#include <sys/types.h> + +#include "common/math/macros.h" + +namespace online_calibration { + +// Defines an invalid or uninitialized temperature value (referenced from +// common/math/macros.h). +constexpr float kInvalidTemperatureCelsius = INVALID_TEMPERATURE_CELSIUS; + +// Unit conversion from nanoseconds to microseconds. +constexpr uint64_t NanoToMicroseconds(uint64_t x) { return x / 1000; } + +// Identifies the various sensing devices used by the calibration algorithms. +enum class SensorType : int8_t { + kUndefined = 0, + kAccelerometerMps2 = 1, // 3-axis sensor (units = meter/sec^2). + kGyroscopeRps = 2, // 3-axis sensor (units = radian/sec). + kMagnetometerUt = 3, // 3-axis sensor (units = micro-Tesla). + kTemperatureCelsius = 4, // 1-axis sensor (units = degrees Celsius). + kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal). + kWifiM = 6 // 3-axis sensor (units = meter). +}; + +/* + * SensorData is a generalized data structure used to represent sensor samples + * produced by either a single- or three-axis device. Usage is implied through + * the sensor type (i.e., Gyroscope is a three-axis sensor and would therefore + * use all elements of 'data'; a pressure sensor is single-dimensional and would + * use 'data[SensorIndex::kSingleAxis]'). This arbitration is determined + * at the algorithm wrapper level where knowledge of a sensor's dimensionality + * is clearly understood. + * + * NOTE: The unified dimensional representation makes it convenient to pass + * either type of data into the interface functions defined in the + * OnlineCalibration. + */ + +// Axis index definitions for SensorData::data. +enum SensorIndex : int8_t { + kSingleAxis = 0, + kXAxis = kSingleAxis, + kYAxis = 1, + kZAxis = 2, +}; + +struct SensorData { + // Indicates the type of sensor this data originated from. + SensorType type; + + // Sensor sample timestamp. + uint64_t timestamp_nanos; + + // Generalized sensor sample (represents either single- or three-axis data). + float data[3]; + + SensorData() : type(SensorType::kUndefined), timestamp_nanos(0) { + memset(data, 0, sizeof(data)); + } + + SensorData(SensorType type, uint64_t ts, float x, float y, float z) + : type(type), timestamp_nanos(ts) { + data[SensorIndex::kXAxis] = x; + data[SensorIndex::kYAxis] = y; + data[SensorIndex::kZAxis] = z; + } + + SensorData(SensorType type, uint64_t ts, float single_axis_sample) + : type(type), timestamp_nanos(ts) { + data[SensorIndex::kSingleAxis] = single_axis_sample; + } +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_ diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc new file mode 100644 index 00000000..26eef573 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +// Estimated upper bounds on gyro offset error (i.e., 3-sigma values). +constexpr float GyroOffsetOtcCal::kMediumQualityRps; +constexpr float GyroOffsetOtcCal::kHighQualityRps; + +void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters) { + gyroCalInit(&gyro_cal_, &gyro_cal_parameters); + overTempCalInit(&over_temp_cal_, &otc_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( + const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + switch (sample.type) { + case SensorType::kAccelerometerMps2: + gyroCalUpdateAccel(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); // [m/sec^2] + break; + + case SensorType::kGyroscopeRps: + gyroCalUpdateGyro(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis], // [rad/sec] + temperature_celsius_); + break; + + case SensorType::kMagnetometerUt: + gyroCalUpdateMag(&gyro_cal_, sample.timestamp_nanos, + sample.data[SensorIndex::kXAxis], + sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); // [micro-Tesla] + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + overTempCalSetTemperature(&over_temp_cal_, sample.timestamp_nanos, + temperature_celsius_); + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new calibration, and updates the OTC. + if (gyroCalNewBiasAvailable(&gyro_cal_)) { + float offset[3]; + float temperature_celsius = kInvalidTemperatureCelsius; + uint64_t calibration_time_nanos = 0; + gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2], + &temperature_celsius, &calibration_time_nanos); + overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos, + offset, temperature_celsius); + } + + // Checks the OTC for a new calibration model update. + const bool new_otc_model_update = + overTempCalNewModelUpdateAvailable(&over_temp_cal_); + + // Checks for a change in the temperature compensated offset estimate. + const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_); + + // Sets the new calibration data. + CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE; + if (new_otc_offset) { + overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius, + cal_data_.offset); + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_callback_flags |= CalibrationTypeFlags::BIAS; + } + + if (new_otc_model_update) { + // Sets the pointer to the OTC model dataset and the number of model points. + cal_data_.otc_model_data = over_temp_cal_.model_data; + cal_data_.num_model_pts = over_temp_cal_.num_model_pts; + + cal_update_callback_flags |= CalibrationTypeFlags::OVER_TEMP; + overTempCalGetModel(&over_temp_cal_, cal_data_.offset, + &cal_data_.offset_temp_celsius, + &cal_data_.cal_update_time_nanos, + cal_data_.temp_sensitivity, cal_data_.temp_intercept); + } + + // Sets the new calibration quality, polling flag, and notifies a calibration + // callback listener of the new update. + if (new_otc_model_update || new_otc_offset) { + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kHighQualityRps; + cal_update_polling_flags_ |= cal_update_callback_flags; + OnNotifyCalibrationUpdate(cal_update_callback_flags); + } + + // Print debug data reports. +#ifdef GYRO_CAL_DBG_ENABLED + gyroCalDebugPrint(&gyro_cal_, sample.timestamp_nanos); +#endif // GYRO_CAL_DBG_ENABLED +#ifdef OVERTEMPCAL_DBG_ENABLED + overTempCalDebugPrint(&over_temp_cal_, sample.timestamp_nanos); +#endif // OVERTEMPCAL_DBG_ENABLED + + return cal_update_polling_flags_; +} + +bool GyroOffsetOtcCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[GyroOffsetOtcCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the initial calibration data (offset and OTC model parameters). + gyroCalSetBias(&gyro_cal_, cal_data_.offset[0], cal_data_.offset[1], + cal_data_.offset[2], cal_data_.offset_temp_celsius, + cal_data_.cal_update_time_nanos); + + overTempCalSetModel(&over_temp_cal_, cal_data_.offset, + cal_data_.offset_temp_celsius, + cal_data_.cal_update_time_nanos, + cal_data_.temp_sensitivity, cal_data_.temp_intercept, + /*jump_start_model=*/false); + + // Sets the calibration quality. + cal_data_.calibration_quality.level = CalibrationQualityLevel::MEDIUM_QUALITY; + cal_data_.calibration_quality.value = kMediumQualityRps; + + const bool load_new_model_dataset = + (input_cal_data.otc_model_data != nullptr && + input_cal_data.num_model_pts > 0); + + if (load_new_model_dataset) { + // Loads the new model dataset and uses it to update the linear model + // parameters. + overTempCalSetModelData(&over_temp_cal_, input_cal_data.num_model_pts, + cal_data_.cal_update_time_nanos, + input_cal_data.otc_model_data); + } + + // Sets the pointer to the OTC model dataset and the number of model points. + cal_data_.otc_model_data = over_temp_cal_.model_data; + cal_data_.num_model_pts = over_temp_cal_.num_model_pts; + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h new file mode 100644 index 00000000..e21007b3 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ + +#include "calibration/gyroscope/gyro_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" +#include "calibration/over_temp/over_temp_cal.h" + +namespace online_calibration { + +/* + * This class is a wrapper for the gyroscope offset calibration with + * over-temperature compensation (OTC). + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::MEDIUM_QUALITY + * CalibrationQuality.value = kMediumQualityRps + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = kHighQualityRps + */ +class GyroOffsetOtcCal final + : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Estimated upper bounds on gyro offset error (i.e., 3-sigma values). + static constexpr float kMediumQualityRps = 5.23599e-3f; // 300 mDPS + static constexpr float kHighQualityRps = 1.04720e-3f; // 60 mDPS + + // Default constructor. + GyroOffsetOtcCal() = default; + + // Creates an GyroOffsetOtcCal with specified algorithm parameters. + GyroOffsetOtcCal(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters) { + Initialize(gyro_cal_parameters, otc_parameters); + } + + // Initializes with specified algorithm parameters, and resets the calibration + // data. + void Initialize(const GyroCalParameters& gyro_cal_parameters, + const OverTempCalParameters& otc_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kGyroscopeRps; + }; + + private: + // GyroCal algorithm data structure. + GyroCal gyro_cal_; + + // Over-temperature offset compensation algorithm data structure. + OverTempCal over_temp_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_ diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc new file mode 100644 index 00000000..fe787b1a --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h" + +#include "calibration/util/cal_log.h" + +namespace online_calibration { + +// Empirically estimated upper bounds on offset error. +constexpr float MagDiverseCal::kLowQualityUt; +constexpr float MagDiverseCal::kHighQualityUt; + +void MagDiverseCal::Initialize( + const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters) { + initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters); + InitializeCalData(); +} + +CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) { + // Routes the input sensor sample to the calibration algorithm. + MagUpdate new_calibration_update = MagUpdate::NO_UPDATE; + switch (sample.type) { + case SensorType::kMagnetometerUt: + new_calibration_update = magCalUpdate( + &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos), + sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis], + sample.data[SensorIndex::kZAxis]); + break; + + case SensorType::kTemperatureCelsius: + temperature_celsius_ = sample.data[SensorIndex::kSingleAxis]; + break; + + default: + // This sample is not required. + return cal_update_polling_flags_; + } + + // Checks for a new offset estimate, and updates the calibration data. + if (MagUpdate::UPDATE_BIAS & new_calibration_update) { + magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1], + &cal_data_.offset[2]); + + cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; + cal_data_.calibration_quality.value = kHighQualityUt; + cal_data_.offset_temp_celsius = temperature_celsius_; + cal_data_.cal_update_time_nanos = sample.timestamp_nanos; + cal_update_polling_flags_ = CalibrationTypeFlags::BIAS; + OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS); + } + + return cal_update_polling_flags_; +} + +bool MagDiverseCal::SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) { + // Checks that the input calibration type matches the algorithm type. + if (input_cal_data.type != get_sensor_type()) { + CAL_DEBUG_LOG("[MagDiverseCal]", + "SetInitialCalibration failed due to wrong sensor type."); + return false; + } + + // Sets the magnetometer algorithm's calibration data. + magCalReset(&mag_cal_); // Resets the magnetometer's offset vector. + magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1], + input_cal_data.offset[2]); + + // Sync's all initial calibration data. + cal_data_ = input_cal_data; + + // Sets the calibration quality to undetermined (uncertain magnetic history + // makes the usefulness of the input calibration value unknown). + cal_data_.calibration_quality.reset(); + + return true; +} + +} // namespace online_calibration diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h new file mode 100644 index 00000000..11ede3d4 --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ + +#include "calibration/diversity_checker/diversity_checker.h" +#include "calibration/magnetometer/mag_cal/mag_cal.h" +#include "calibration/online_calibration/common_data/calibration_callback.h" +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/online_calibration.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +/* + * This class is a wrapper for the magnetometer offset calibration with + * diversity checking. + * + * NOTE: Calibration quality reporting: + * Initialize --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * SetInitialCalibration --> CalibrationQualityLevel::UNDETERMINED + * CalibrationQuality.value = + * kUndeterminedCalibrationQuality + * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY + * CalibrationQuality.value = kHighQualityUt + */ +class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> { + public: + // Empirically estimated upper bounds on offset error. + static constexpr float kLowQualityUt = 1000.0f; // Units of micro Tesla + static constexpr float kHighQualityUt = 5.0f; // Units of micro Tesla + + MagDiverseCal() = default; + + // Creates an MagDiverseCal with specified algorithm parameters. + MagDiverseCal(const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters) { + Initialize(mag_cal_parameters, diversity_parameters); + } + + // Initializes with specified algorithm parameters. + void Initialize(const MagCalParameters& mag_cal_parameters, + const DiversityCheckerParameters& diversity_parameters); + + // Sends new sensor data to the calibration algorithm, and returns the state + // of the calibration update flags, 'cal_update_polling_flags_'. + CalibrationTypeFlags SetMeasurement(const SensorData& sample) final; + + // Sets the initial calibration data of the calibration algorithm. Returns + // true if set successfully. + bool SetInitialCalibration( + const CalibrationDataThreeAxis& input_cal_data) final; + + // Returns the calibration sensor type. + SensorType get_sensor_type() const final { + return SensorType::kMagnetometerUt; + }; + + private: + // MagCal algorithm data structure. + MagCal mag_cal_; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_ diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c index d92f1daa..04a9c924 100644 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c +++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c @@ -17,12 +17,11 @@ #include "calibration/over_temp/over_temp_cal.h" #include <float.h> +#include <inttypes.h> #include <math.h> -#include <stdio.h> #include <string.h> #include "calibration/util/cal_log.h" -#include "common/math/macros.h" #include "util/nano_assert.h" /////// DEFINITIONS AND MACROS //////////////////////////////////////////////// @@ -32,40 +31,39 @@ // Defines the default weighting function for the linear model fit routine. // Weighting = 10.0; for offsets newer than 5 minutes. -static const struct OverTempCalWeightPt kOtcDefaultWeight0 = { +static const struct OverTempCalWeight kOtcDefaultWeight0 = { .offset_age_nanos = MIN_TO_NANOS(5), .weight = 10.0f, }; // Weighting = 0.1; for offsets newer than 15 minutes. -static const struct OverTempCalWeightPt kOtcDefaultWeight1 = { +static const struct OverTempCalWeight kOtcDefaultWeight1 = { .offset_age_nanos = MIN_TO_NANOS(15), .weight = 0.1f, }; // The default weighting used for all older offsets. -#define OTC_MIN_WEIGHT_VALUE (0.04f) +#define OTC_MIN_WEIGHT_VALUE (0.04f) #ifdef OVERTEMPCAL_DBG_ENABLED // A debug version label to help with tracking results. -#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]" +#define OTC_DEBUG_VERSION_STRING "[Jan 10, 2018]" // The time interval used to throttle debug messaging (100msec). #define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1)) -// The time interval used to throttle temperture print messaging (1 second). +// The time interval used to throttle temperature print messaging (1 second). #define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1)) // Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z. -static const char kDebugAxisLabel[3] = "XYZ"; +static const char kDebugAxisLabel[3] = "XYZ"; #endif // OVERTEMPCAL_DBG_ENABLED /////// FORWARD DECLARATIONS ////////////////////////////////////////////////// // Updates the latest received model estimate data. static void setLatestEstimate(struct OverTempCal *over_temp_cal, - const float *offset, float offset_temp_celsius, - uint64_t timestamp_nanos); + const float *offset, float offset_temp_celsius); /* * Determines if a new over-temperature model fit should be performed, and then @@ -103,11 +101,10 @@ static bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, * Since it may take a while for an empty model to build up enough data to start * producing new model parameter updates, the model collection can be * jump-started by using the new model parameters to insert "fake" data in place - * of actual sensor offset data. 'timestamp_nanos' sets the timestamp for the - * new model data. + * of actual sensor offset data. The new model data 'offset_age_nanos' is set to + * zero. */ -static bool jumpStartModelData(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos); +static bool jumpStartModelData(struct OverTempCal *over_temp_cal); /* * Computes a new model fit and provides updated model parameters for the @@ -116,7 +113,6 @@ static bool jumpStartModelData(struct OverTempCal *over_temp_cal, * * INPUTS: * over_temp_cal: Over-temp data structure. - * timestamp_nanos: Current timestamp for the model update. * OUTPUTS: * temp_sensitivity: Updated modeled temperature sensitivity (array). * sensor_intercept: Updated model intercept (array). @@ -127,8 +123,7 @@ static bool jumpStartModelData(struct OverTempCal *over_temp_cal, * Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992. */ static void updateModel(const struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, float *temp_sensitivity, - float *sensor_intercept); + float *temp_sensitivity, float *sensor_intercept); /* * Computes a new over-temperature compensated offset estimate based on the @@ -183,7 +178,8 @@ static bool checkAndEnforceTemperatureRange(float *temperature_celsius); // Returns "true" if the candidate linear model parameters are within the valid // range, and not all zeros. static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal, - float temp_sensitivity, float sensor_intercept); + float temp_sensitivity, + float sensor_intercept); // Returns "true" if 'offset' and 'offset_temp_celsius' is valid. static bool isValidOtcOffset(const float *offset, float offset_temp_celsius); @@ -191,8 +187,12 @@ static bool isValidOtcOffset(const float *offset, float offset_temp_celsius); // Returns the least-squares weight based on the age of a particular offset // estimate. static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, - uint64_t offset_timestamp_nanos, - uint64_t current_timestamp_nanos); + uint64_t offset_age_nanos); + +// Computes the age increment, adds it to the age of each OTC model data point, +// and resets the age update counter. +static void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos); // Updates 'compensated_offset' using the linear OTC model. static void compensateWithLinearModel(struct OverTempCal *over_temp_cal, @@ -208,9 +208,10 @@ static void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal, float delta_temp_celsius); // Provides an over-temperature compensated offset based on the 'estimate'. -static void compensateWithEstimate( - struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, - struct OverTempCalDataPt *estimate, float temperature_celsius); +static void compensateWithEstimate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + struct OverTempModelThreeAxis *estimate, + float temperature_celsius); // Evaluates the nearest-temperature compensation (with linear extrapolation // term due to temperature), and compares it with the compensation due to @@ -233,7 +234,7 @@ static void refreshOtcModel(struct OverTempCal *over_temp_cal, #ifdef OVERTEMPCAL_DBG_ENABLED // This helper function stores all of the debug tracking information necessary // for printing log messages. -static void updateDebugData(struct OverTempCal* over_temp_cal); +static void updateDebugData(struct OverTempCal *over_temp_cal); // Helper function that creates tag strings useful for identifying specific // debug output data (embedded system friendly; not all systems have 'sprintf'). @@ -251,12 +252,7 @@ static void createDebugTag(struct OverTempCal *over_temp_cal, /////// FUNCTION DEFINITIONS ////////////////////////////////////////////////// void overTempCalInit(struct OverTempCal *over_temp_cal, - size_t min_num_model_pts, - uint64_t min_temp_update_period_nanos, - float delta_temp_per_bin, float jump_tolerance, - float outlier_limit, uint64_t age_limit_nanos, - float temp_sensitivity_limit, float sensor_intercept_limit, - float significant_offset_change, bool over_temp_enable) { + const struct OverTempCalParameters *parameters) { ASSERT_NOT_NULL(over_temp_cal); // Clears OverTempCal memory. @@ -264,7 +260,7 @@ void overTempCalInit(struct OverTempCal *over_temp_cal, // Initializes the pointers to important sensor offset estimates. over_temp_cal->nearest_offset = &over_temp_cal->model_data[0]; - over_temp_cal->latest_offset = NULL; + over_temp_cal->latest_offset = NULL; // Initializes the OTC linear model parameters. resetOtcLinearModel(over_temp_cal); @@ -272,24 +268,22 @@ void overTempCalInit(struct OverTempCal *over_temp_cal, // Initializes the model identification parameters. over_temp_cal->new_overtemp_model_available = false; over_temp_cal->new_overtemp_offset_available = false; - over_temp_cal->min_num_model_pts = min_num_model_pts; - over_temp_cal->min_temp_update_period_nanos = min_temp_update_period_nanos; - over_temp_cal->delta_temp_per_bin = delta_temp_per_bin; - over_temp_cal->jump_tolerance = jump_tolerance; - over_temp_cal->outlier_limit = outlier_limit; - over_temp_cal->age_limit_nanos = age_limit_nanos; - over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit; - over_temp_cal->sensor_intercept_limit = sensor_intercept_limit; - over_temp_cal->significant_offset_change = significant_offset_change; - over_temp_cal->over_temp_enable = over_temp_enable; + over_temp_cal->min_num_model_pts = parameters->min_num_model_pts; + over_temp_cal->min_temp_update_period_nanos = + parameters->min_temp_update_period_nanos; + over_temp_cal->delta_temp_per_bin = parameters->delta_temp_per_bin; + over_temp_cal->jump_tolerance = parameters->jump_tolerance; + over_temp_cal->outlier_limit = parameters->outlier_limit; + over_temp_cal->age_limit_nanos = parameters->age_limit_nanos; + over_temp_cal->temp_sensitivity_limit = parameters->temp_sensitivity_limit; + over_temp_cal->sensor_intercept_limit = parameters->sensor_intercept_limit; + over_temp_cal->significant_offset_change = + parameters->significant_offset_change; + over_temp_cal->over_temp_enable = parameters->over_temp_enable; // Initializes the over-temperature compensated offset temperature. over_temp_cal->compensated_offset.offset_temp_celsius = - OTC_TEMP_INVALID_CELSIUS; - - // Defines the default weighting function for the linear model fit routine. - overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0); - overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1); + INVALID_TEMPERATURE_CELSIUS; #ifdef OVERTEMPCAL_DBG_ENABLED // Sets the default sensor descriptors for debugging. @@ -305,6 +299,10 @@ void overTempCalInit(struct OverTempCal *over_temp_cal, "Over-temperature compensation DISABLED."); } #endif // OVERTEMPCAL_DBG_ENABLED + + // Defines the default weighting function for the linear model fit routine. + overTempValidateAndSetWeight(over_temp_cal, 0, &kOtcDefaultWeight0); + overTempValidateAndSetWeight(over_temp_cal, 1, &kOtcDefaultWeight1); } void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, @@ -332,8 +330,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, // Model "Jump-Start". const bool model_jump_started = - (jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos) - : false; + jump_start_model ? jumpStartModelData(over_temp_cal) : false; if (!model_jump_started) { // Checks that the new offset data is valid. @@ -342,7 +339,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, memcpy(over_temp_cal->model_data[0].offset, offset, sizeof(over_temp_cal->model_data[0].offset)); over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius; - over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos; + over_temp_cal->model_data[0].offset_age_nanos = 0; over_temp_cal->num_model_pts = 1; } else { // No valid offset data to load. @@ -361,7 +358,7 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, memcpy(over_temp_cal->compensated_offset.offset, offset, sizeof(over_temp_cal->compensated_offset.offset)); over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius; - over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos; + over_temp_cal->compensated_offset.offset_age_nanos = 0; } // Resets the latest offset pointer. There are no new offset estimates to @@ -435,7 +432,7 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, void overTempCalSetModelData(struct OverTempCal *over_temp_cal, size_t data_length, uint64_t timestamp_nanos, - const struct OverTempCalDataPt *model_data) { + const struct OverTempModelThreeAxis *model_data) { ASSERT_NOT_NULL(over_temp_cal); ASSERT_NOT_NULL(model_data); @@ -446,11 +443,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, if (isValidOtcOffset(model_data[i].offset, model_data[i].offset_temp_celsius)) { memcpy(&over_temp_cal->model_data[i], &model_data[i], - sizeof(struct OverTempCalDataPt)); - - // Updates the model time stamps to the current load time. - over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos; - + sizeof(struct OverTempModelThreeAxis)); valid_data_count++; } } @@ -491,11 +484,11 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, void overTempCalGetModelData(struct OverTempCal *over_temp_cal, size_t *data_length, - struct OverTempCalDataPt *model_data) { + struct OverTempModelThreeAxis *model_data) { ASSERT_NOT_NULL(over_temp_cal); *data_length = over_temp_cal->num_model_pts; memcpy(model_data, over_temp_cal->model_data, - over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt)); + over_temp_cal->num_model_pts * sizeof(struct OverTempModelThreeAxis)); } void overTempCalGetOffset(struct OverTempCal *over_temp_cal, @@ -559,6 +552,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, ASSERT_NOT_NULL(offset); ASSERT(over_temp_cal->delta_temp_per_bin > 0); + // Updates the age of each OTC model data point. + modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos); + // Checks that the new offset data is valid, returns if bad. if (!isValidOtcOffset(offset, temperature_celsius)) { return; @@ -586,14 +582,13 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, createDebugTag(over_temp_cal, ":OUTLIER]"); CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, - "Offset|Temperature|Time [%s|C|nsec]: " - CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu", + "Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, over_temp_cal->otc_unit_tag, CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3), CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3), CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3), - CAL_ENCODE_FLOAT(temperature_celsius, 3), - (unsigned long long int)timestamp_nanos); + CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos); #endif // OVERTEMPCAL_DBG_ENABLED return; // Outlier detected: skips adding this offset to the model. @@ -648,8 +643,8 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, // oldest data with the incoming one. over_temp_cal->latest_offset = &over_temp_cal->model_data[0]; for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) { - if (over_temp_cal->latest_offset->timestamp_nanos < - over_temp_cal->model_data[i].timestamp_nanos) { + if (over_temp_cal->latest_offset->offset_age_nanos < + over_temp_cal->model_data[i].offset_age_nanos) { over_temp_cal->latest_offset = &over_temp_cal->model_data[i]; } } @@ -657,8 +652,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, } // Updates the latest model estimate data. - setLatestEstimate(over_temp_cal, offset, temperature_celsius, - timestamp_nanos); + setLatestEstimate(over_temp_cal, offset, temperature_celsius); // The latest offset estimate is the nearest temperature offset. over_temp_cal->nearest_offset = over_temp_cal->latest_offset; @@ -705,13 +699,20 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, // Prints out temperature and the current timestamp. createDebugTag(over_temp_cal, ":TEMP]"); CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, - "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu", - CAL_ENCODE_FLOAT(temperature_celsius, 3), - (unsigned long long int)timestamp_nanos); + "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS + ", %" PRIu64, + CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos); } #endif // OVERTEMPCAL_DBG_LOG_TEMP #endif // OVERTEMPCAL_DBG_ENABLED + // Updates the age of each OTC model data point. + if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( + timestamp_nanos, over_temp_cal->last_age_update_nanos, + OTC_MODEL_AGE_UPDATE_NANOS)) { + modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos); + } + // This check throttles new OTC offset compensation updates so that high data // rate temperature samples do not cause excessive computational burden. Note, // temperature sensor updates are expected to potentially increase the data @@ -720,7 +721,7 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( timestamp_nanos, over_temp_cal->last_offset_update_nanos, over_temp_cal->min_temp_update_period_nanos)) { - return; // Time interval too short, skip further data processing. + return; // Time interval too short, skip further data processing. } // Checks that the offset temperature is within a valid range, saturates if @@ -746,8 +747,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, } void overTempGetModelError(const struct OverTempCal *over_temp_cal, - const float *temp_sensitivity, const float *sensor_intercept, - float *max_error) { + const float *temp_sensitivity, + const float *sensor_intercept, float *max_error) { ASSERT_NOT_NULL(over_temp_cal); ASSERT_NOT_NULL(temp_sensitivity); ASSERT_NOT_NULL(sensor_intercept); @@ -770,14 +771,34 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal, } } -// TODO(davejacobs): Refactor to implement a compliance check on the storage of -// 'offset_age_nanos' to ensure a monotonically increasing order with index. -void overTempSetWeightingFunction( +bool overTempValidateAndSetWeight( struct OverTempCal *over_temp_cal, size_t index, - const struct OverTempCalWeightPt *new_otc_weight) { - if (index < OTC_NUM_WEIGHT_LEVELS) { + const struct OverTempCalWeight *new_otc_weight) { + ASSERT_NOT_NULL(over_temp_cal); + ASSERT_NOT_NULL(new_otc_weight); + + // The input weighting coefficient must be positive. + if (new_otc_weight->weight <= 0.0f) { +#ifdef OVERTEMPCAL_DBG_ENABLED + createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]"); + CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Invalid weight: Must > 0."); +#endif // OVERTEMPCAL_DBG_ENABLED + return false; + } + + // Ensures that the 'index-1' weight's age is younger. + if (index == 0 || + over_temp_cal->weighting_function[index - 1].offset_age_nanos < + new_otc_weight->offset_age_nanos) { over_temp_cal->weighting_function[index] = *new_otc_weight; + return true; } + +#ifdef OVERTEMPCAL_DBG_ENABLED + createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]"); + CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Non monotonic weight age."); +#endif // OVERTEMPCAL_DBG_ENABLED + return false; } /////// LOCAL HELPER FUNCTION DEFINITIONS ///////////////////////////////////// @@ -827,9 +848,10 @@ void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal, } } -void compensateWithEstimate( - struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, - struct OverTempCalDataPt *estimate, float temperature_celsius) { +void compensateWithEstimate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos, + struct OverTempModelThreeAxis *estimate, + float temperature_celsius) { ASSERT_NOT_NULL(over_temp_cal); ASSERT_NOT_NULL(estimate); @@ -838,7 +860,7 @@ void compensateWithEstimate( memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset)); // Checks that the offset temperature is valid. - if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) { + if (estimate->offset_temp_celsius > INVALID_TEMPERATURE_CELSIUS) { const float delta_temp_celsius = temperature_celsius - estimate->offset_temp_celsius; @@ -886,7 +908,7 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal, // Adds a delta term to the compensated offset using the temperature // difference defined by 'delta_temp_celsius'. if (over_temp_cal->compensated_offset.offset_temp_celsius <= - OTC_TEMP_INVALID_CELSIUS) { + INVALID_TEMPERATURE_CELSIUS) { // If temperature is invalid, then skip further processing. break; } @@ -918,7 +940,7 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // If 'temperature_celsius' is invalid, then no changes to the compensated // offset are computed. - if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) { + if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) { return; } @@ -927,9 +949,9 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // than one estimate in the model (i.e., don't want to remove all data, even // if it is very old [something is likely better than nothing]). if ((timestamp_nanos >= - OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) && + OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer_nanos) && over_temp_cal->num_model_pts > 1) { - over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer. + over_temp_cal->stale_data_timer_nanos = timestamp_nanos; // Resets timer. removeStaleModelData(over_temp_cal, timestamp_nanos); } @@ -944,12 +966,32 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // not empty. const bool model_points_available = (over_temp_cal->num_model_pts > 0); + // To properly evaluate the logic paths that use the latest and nearest offset + // data below, the current age of the nearest and latest offset estimates are + // computed. + uint64_t latest_offset_age_nanos = 0; + if (over_temp_cal->latest_offset != NULL) { + latest_offset_age_nanos = + (over_temp_cal->last_age_update_nanos < timestamp_nanos) + ? over_temp_cal->latest_offset->offset_age_nanos + + timestamp_nanos - over_temp_cal->last_age_update_nanos + : over_temp_cal->latest_offset->offset_age_nanos; + } + + uint64_t nearest_offset_age_nanos = 0; + if (over_temp_cal->nearest_offset != NULL) { + nearest_offset_age_nanos = + (over_temp_cal->last_age_update_nanos < timestamp_nanos) + ? over_temp_cal->nearest_offset->offset_age_nanos + + timestamp_nanos - over_temp_cal->last_age_update_nanos + : over_temp_cal->nearest_offset->offset_age_nanos; + } + // True when the latest offset estimate will be used to compute a sensor // offset calibration estimate. const bool use_latest_offset_compensation = - over_temp_cal->latest_offset && model_points_available && - timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos + - OTC_USE_RECENT_OFFSET_TIME_NANOS; + over_temp_cal->latest_offset != NULL && model_points_available && + latest_offset_age_nanos <= OTC_USE_RECENT_OFFSET_TIME_NANOS; // True when the conditions are met to use the nearest-temperature offset to // compute a sensor offset calibration estimate. @@ -958,7 +1000,7 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // ii. Offset temperature must be within a small neighborhood of the // current measured temperature (+/- 'delta_temp_per_bin'). const bool can_compensate_with_nearest = - model_points_available && over_temp_cal->nearest_offset && + model_points_available && over_temp_cal->nearest_offset != NULL && NANO_ABS(temperature_celsius - over_temp_cal->nearest_offset->offset_temp_celsius) < over_temp_cal->delta_temp_per_bin; @@ -966,18 +1008,14 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, // True if the last received sensor offset estimate is old or non-existent. const bool latest_model_point_not_relevant = (over_temp_cal->latest_offset == NULL) || - (over_temp_cal->latest_offset && - NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - timestamp_nanos, over_temp_cal->latest_offset->timestamp_nanos, - OTC_OFFSET_IS_STALE_NANOS)); + (over_temp_cal->latest_offset != NULL && + latest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS); // True if the nearest-temperature offset estimate is old or non-existent. const bool nearest_model_point_not_relevant = (over_temp_cal->nearest_offset == NULL) || - (over_temp_cal->nearest_offset && - NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - timestamp_nanos, over_temp_cal->nearest_offset->timestamp_nanos, - OTC_OFFSET_IS_STALE_NANOS)); + (over_temp_cal->nearest_offset != NULL && + nearest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS); // --------------------------------------------------------------------------- // The following conditional expressions govern new OTC offset updates. @@ -1051,26 +1089,25 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal, over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available; // If the offset has changed significantly, then the offset compensation - // vector and timestamp are updated. + // vector is updated. if (new_overtemp_offset_available) { memcpy(over_temp_cal->compensated_offset.offset, compensated_offset, sizeof(over_temp_cal->compensated_offset.offset)); - over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos; over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius; } } void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, - float offset_temp_celsius, uint64_t timestamp_nanos) { + float offset_temp_celsius) { ASSERT_NOT_NULL(over_temp_cal); ASSERT_NOT_NULL(offset); - if (over_temp_cal->latest_offset) { + if (over_temp_cal->latest_offset != NULL) { // Sets the latest over-temp calibration estimate. memcpy(over_temp_cal->latest_offset->offset, offset, sizeof(over_temp_cal->latest_offset->offset)); over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius; - over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos; + over_temp_cal->latest_offset->offset_age_nanos = 0; } } @@ -1098,19 +1135,17 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, // Ensures that the minimum number of points required for a model fit has been // satisfied. - if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) - return; + if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) return; // Updates the linear model fit. float temp_sensitivity[3]; float sensor_intercept[3]; - updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity, - sensor_intercept); + updateModel(over_temp_cal, temp_sensitivity, sensor_intercept); // 2) A new set of model parameters are accepted if: // i. The model fit parameters must be within certain absolute bounds: - // a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit - // b. NANO_ABS(sensor_intercept) < sensor_intercept_limit + // a. |temp_sensitivity| < temp_sensitivity_limit + // b. |sensor_intercept| < sensor_intercept_limit // NOTE: Model parameter updates are not qualified against model fit error // here to protect against the case where there is large change in the // temperature characteristic either during runtime (e.g., temperature @@ -1131,14 +1166,14 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, "%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS - ", " CAL_FORMAT_3DIGITS ", %llu", + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, kDebugAxisLabel[i], over_temp_cal->otc_unit_tag, over_temp_cal->otc_unit_tag, CAL_ENCODE_FLOAT( temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3), CAL_ENCODE_FLOAT( sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3), - (unsigned long long int)timestamp_nanos); + timestamp_nanos); #endif // OVERTEMPCAL_DBG_ENABLED } } @@ -1161,7 +1196,7 @@ void findNearestEstimate(struct OverTempCal *over_temp_cal, ASSERT_NOT_NULL(over_temp_cal); // If 'temperature_celsius' is invalid, then do not search. - if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) { + if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) { return; } @@ -1186,9 +1221,8 @@ void removeStaleModelData(struct OverTempCal *over_temp_cal, bool removed_one = false; for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { - if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos && - timestamp_nanos > over_temp_cal->age_limit_nanos + - over_temp_cal->model_data[i].timestamp_nanos) { + if (over_temp_cal->model_data[i].offset_age_nanos >= + over_temp_cal->age_limit_nanos) { // If the latest offset was removed, then indicate this by setting it to // NULL. if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) { @@ -1223,8 +1257,8 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, createDebugTag(over_temp_cal, ":REMOVE]"); CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, - "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET - ", " CAL_FORMAT_3DIGITS ", %llu", + "Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, over_temp_cal->otc_unit_tag, CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] * over_temp_cal->otc_unit_conversion, @@ -1237,22 +1271,20 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, 3), CAL_ENCODE_FLOAT( over_temp_cal->model_data[model_index].offset_temp_celsius, 3), - (unsigned long long int)over_temp_cal->model_data[model_index] - .timestamp_nanos); + over_temp_cal->model_data[model_index].offset_age_nanos); #endif // OVERTEMPCAL_DBG_ENABLED // Remove the model data at 'model_index'. for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) { memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1], - sizeof(struct OverTempCalDataPt)); + sizeof(struct OverTempModelThreeAxis)); } over_temp_cal->num_model_pts--; return true; } -bool jumpStartModelData(struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos) { +bool jumpStartModelData(struct OverTempCal *over_temp_cal) { ASSERT_NOT_NULL(over_temp_cal); ASSERT(over_temp_cal->delta_temp_per_bin > 0); @@ -1291,7 +1323,7 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal, over_temp_cal->sensor_intercept[j]; } over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius; - over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos; + over_temp_cal->model_data[i].offset_age_nanos = 0; offset_temp_celsius += over_temp_cal->delta_temp_per_bin; over_temp_cal->num_model_pts++; @@ -1301,8 +1333,8 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal, createDebugTag(over_temp_cal, ":INIT]"); if (over_temp_cal->num_model_pts > 0) { CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, - "Model Jump-Start: #Points = %lu.", - (unsigned long int)over_temp_cal->num_model_pts); + "Model Jump-Start: #Points = %zu.", + over_temp_cal->num_model_pts); } #endif // OVERTEMPCAL_DBG_ENABLED @@ -1310,8 +1342,7 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal, } void updateModel(const struct OverTempCal *over_temp_cal, - uint64_t timestamp_nanos, float *temp_sensitivity, - float *sensor_intercept) { + float *temp_sensitivity, float *sensor_intercept) { ASSERT_NOT_NULL(over_temp_cal); ASSERT_NOT_NULL(temp_sensitivity); ASSERT_NOT_NULL(sensor_intercept); @@ -1328,8 +1359,7 @@ void updateModel(const struct OverTempCal *over_temp_cal, const size_t n = over_temp_cal->num_model_pts; for (size_t i = 0; i < n; ++i) { weight = evaluateWeightingFunction( - over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos, - timestamp_nanos); + over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos); sw += weight; st += over_temp_cal->model_data[i].offset_temp_celsius * weight; @@ -1343,13 +1373,11 @@ void updateModel(const struct OverTempCal *over_temp_cal, const float inv_sw = 1.0f / sw; for (size_t i = 0; i < n; ++i) { weight = evaluateWeightingFunction( - over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos, - timestamp_nanos); + over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos); const float t = - over_temp_cal->model_data[i].offset_temp_celsius - - st * inv_sw; - stt += weight * t * t; + over_temp_cal->model_data[i].offset_temp_celsius - st * inv_sw; + stt += weight * t * t; stsx += t * over_temp_cal->model_data[i].offset[0] * weight; stsy += t * over_temp_cal->model_data[i].offset[1] * weight; stsz += t * over_temp_cal->model_data[i].offset[2] * weight; @@ -1440,13 +1468,11 @@ bool isValidOtcOffset(const float *offset, float offset_temp_celsius) { } float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, - uint64_t offset_timestamp_nanos, - uint64_t current_timestamp_nanos) { + uint64_t offset_age_nanos) { ASSERT_NOT_NULL(over_temp_cal); for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) { - if (current_timestamp_nanos <= - offset_timestamp_nanos + - over_temp_cal->weighting_function[i].offset_age_nanos) { + if (offset_age_nanos <= + over_temp_cal->weighting_function[i].offset_age_nanos) { return over_temp_cal->weighting_function[i].weight; } } @@ -1455,6 +1481,26 @@ float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, return OTC_MIN_WEIGHT_VALUE; } +void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal, + uint64_t timestamp_nanos) { + ASSERT_NOT_NULL(over_temp_cal); + if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) { + // Age updates must be monotonic. + return; + } + + uint64_t age_increment_nanos = + timestamp_nanos - over_temp_cal->last_age_update_nanos; + + // Resets the age update counter. + over_temp_cal->last_age_update_nanos = timestamp_nanos; + + // Updates the model dataset ages. + for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { + over_temp_cal->model_data[i].offset_age_nanos += age_increment_nanos; + } +} + /////// DEBUG FUNCTION DEFINITIONS //////////////////////////////////////////// #ifdef OVERTEMPCAL_DBG_ENABLED @@ -1468,7 +1514,7 @@ void createDebugTag(struct OverTempCal *over_temp_cal, new_debug_tag, strlen(new_debug_tag) + 1); } -void updateDebugData(struct OverTempCal* over_temp_cal) { +void updateDebugData(struct OverTempCal *over_temp_cal) { ASSERT_NOT_NULL(over_temp_cal); // Only update this data if debug printing is not currently in progress @@ -1502,13 +1548,13 @@ void updateDebugData(struct OverTempCal* over_temp_cal) { // If 'latest_offset' is defined the copy the data for debug printing. // Otherwise, the current compensated offset will be printed. - if (over_temp_cal->latest_offset) { + if (over_temp_cal->latest_offset != NULL) { memcpy(&over_temp_cal->debug_overtempcal.latest_offset, - over_temp_cal->latest_offset, sizeof(struct OverTempCalDataPt)); + over_temp_cal->latest_offset, sizeof(struct OverTempModelThreeAxis)); } else { memcpy(&over_temp_cal->debug_overtempcal.latest_offset, &over_temp_cal->compensated_offset, - sizeof(struct OverTempCalDataPt)); + sizeof(struct OverTempModelThreeAxis)); } // Total number of OTC model data points. @@ -1516,9 +1562,9 @@ void updateDebugData(struct OverTempCal* over_temp_cal) { // Computes the maximum error over all of the model data. overTempGetModelError(over_temp_cal, - over_temp_cal->debug_overtempcal.temp_sensitivity, - over_temp_cal->debug_overtempcal.sensor_intercept, - over_temp_cal->debug_overtempcal.max_error); + over_temp_cal->debug_overtempcal.temp_sensitivity, + over_temp_cal->debug_overtempcal.sensor_intercept, + over_temp_cal->debug_overtempcal.max_error); } void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, @@ -1554,10 +1600,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, // Prints out the latest offset estimate (input data). CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, - "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET - ", " CAL_FORMAT_3DIGITS ", %llu", - over_temp_cal->otc_unit_tag, - (unsigned long int)over_temp_cal->debug_num_estimates, + "Cal#|Offset|Temp|Age [%s|C|nsec]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, + over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates, CAL_ENCODE_FLOAT( over_temp_cal->debug_overtempcal.latest_offset.offset[0] * over_temp_cal->otc_unit_conversion, @@ -1573,40 +1618,40 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset .offset_temp_celsius, 3), - (unsigned long long int) - over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos); + over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos); + // clang-format off over_temp_cal->wait_timer_nanos = timestamp_nanos; // Starts the wait timer. over_temp_cal->next_state = OTC_PRINT_MODEL_PARAMETERS; // Sets the next state. over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. + // clang-format on break; case OTC_PRINT_MODEL_PARAMETERS: // Prints out the model parameters. - CAL_DEBUG_LOG( - over_temp_cal->otc_debug_tag, - "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET, - over_temp_cal->otc_unit_tag, - (unsigned long int)over_temp_cal->debug_num_estimates, - CAL_ENCODE_FLOAT( - over_temp_cal->debug_overtempcal.temp_sensitivity[0] * - over_temp_cal->otc_unit_conversion, - 3), - CAL_ENCODE_FLOAT( - over_temp_cal->debug_overtempcal.temp_sensitivity[1] * - over_temp_cal->otc_unit_conversion, - 3), - CAL_ENCODE_FLOAT( - over_temp_cal->debug_overtempcal.temp_sensitivity[2] * - over_temp_cal->otc_unit_conversion, - 3)); + CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, + "Cal#|Sensitivity [%s/C]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET, + over_temp_cal->otc_unit_tag, + over_temp_cal->debug_num_estimates, + CAL_ENCODE_FLOAT( + over_temp_cal->debug_overtempcal.temp_sensitivity[0] * + over_temp_cal->otc_unit_conversion, + 3), + CAL_ENCODE_FLOAT( + over_temp_cal->debug_overtempcal.temp_sensitivity[1] * + over_temp_cal->otc_unit_conversion, + 3), + CAL_ENCODE_FLOAT( + over_temp_cal->debug_overtempcal.temp_sensitivity[2] * + over_temp_cal->otc_unit_conversion, + 3)); CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, - "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET, + "Cal#|Intercept [%s]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET, over_temp_cal->otc_unit_tag, - (unsigned long int)over_temp_cal->debug_num_estimates, + over_temp_cal->debug_num_estimates, CAL_ENCODE_FLOAT( over_temp_cal->debug_overtempcal.sensor_intercept[0] * over_temp_cal->otc_unit_conversion, @@ -1621,8 +1666,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, 3)); over_temp_cal->wait_timer_nanos = - timestamp_nanos; // Starts the wait timer. - over_temp_cal->next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state. + timestamp_nanos; // Starts the wait timer. + over_temp_cal->next_state = + OTC_PRINT_MODEL_ERROR; // Sets the next state. over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. break; @@ -1630,12 +1676,11 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, // Computes the maximum error over all of the model data. CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, - "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, " - "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET, - over_temp_cal->otc_unit_tag, - (unsigned long int)over_temp_cal->debug_num_estimates, - (unsigned long int)over_temp_cal->debug_num_model_updates, - (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts, + "Cal#|#Updates|#ModelPts|Model Error [%s]: %zu, " + "%zu, %zu, " CAL_FORMAT_3DIGITS_TRIPLET, + over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates, + over_temp_cal->debug_num_model_updates, + over_temp_cal->debug_overtempcal.num_model_pts, CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] * over_temp_cal->otc_unit_conversion, 3), @@ -1648,7 +1693,7 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, over_temp_cal->model_counter = 0; // Resets the model data print counter. over_temp_cal->wait_timer_nanos = - timestamp_nanos; // Starts the wait timer. + timestamp_nanos; // Starts the wait timer. over_temp_cal->next_state = OTC_PRINT_MODEL_DATA; // Sets the next state. over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state. break; @@ -1658,10 +1703,9 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) { CAL_DEBUG_LOG( over_temp_cal->otc_debug_tag, - " Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET - ", " CAL_FORMAT_3DIGITS ", %llu", - (unsigned long int)over_temp_cal->model_counter, - over_temp_cal->otc_unit_tag, + " Model[%zu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET + ", " CAL_FORMAT_3DIGITS ", %" PRIu64, + over_temp_cal->model_counter, over_temp_cal->otc_unit_tag, CAL_ENCODE_FLOAT( over_temp_cal->model_data[over_temp_cal->model_counter] .offset[0] * @@ -1681,24 +1725,23 @@ void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, over_temp_cal->model_data[over_temp_cal->model_counter] .offset_temp_celsius, 3), - (unsigned long long int)over_temp_cal - ->model_data[over_temp_cal->model_counter] - .timestamp_nanos); + over_temp_cal->model_data[over_temp_cal->model_counter] + .offset_age_nanos); over_temp_cal->model_counter++; over_temp_cal->wait_timer_nanos = - timestamp_nanos; // Starts the wait timer. + timestamp_nanos; // Starts the wait timer. over_temp_cal->next_state = - OTC_PRINT_MODEL_DATA; // Sets the next state. + OTC_PRINT_MODEL_DATA; // Sets the next state. over_temp_cal->debug_state = - OTC_WAIT_STATE; // First, go to wait state. + OTC_WAIT_STATE; // First, go to wait state. } else { // Sends this state machine to its idle state. over_temp_cal->wait_timer_nanos = - timestamp_nanos; // Starts the wait timer. - over_temp_cal->next_state = OTC_IDLE; // Sets the next state. + timestamp_nanos; // Starts the wait timer. + over_temp_cal->next_state = OTC_IDLE; // Sets the next state. over_temp_cal->debug_state = - OTC_WAIT_STATE; // First, go to wait state. + OTC_WAIT_STATE; // First, go to wait state. } break; diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h index 8a404d32..8f6f0a4d 100644 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h +++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h @@ -100,13 +100,13 @@ #include <stddef.h> #include <stdint.h> +#include "calibration/over_temp/over_temp_model.h" +#include "common/math/macros.h" + #ifdef __cplusplus extern "C" { #endif -// Defines the maximum size of the 'model_data' array. -#define OTC_MODEL_SIZE (40) - // A common sensor operating temperature at which to begin the model jump-start // data. #define JUMPSTART_START_TEMP_CELSIUS (30.0f) @@ -122,13 +122,13 @@ extern "C" { #define OTC_TEMP_MIN_CELSIUS (-40.0f) #define OTC_TEMP_MAX_CELSIUS (85.0f) -// Invalid sensor temperature. -#define OTC_TEMP_INVALID_CELSIUS (-274.0f) - // Number of time-interval levels used to define the least-squares weighting // function. #define OTC_NUM_WEIGHT_LEVELS (2) +// The time interval used to update the model data age. +#define OTC_MODEL_AGE_UPDATE_NANOS (MIN_TO_NANOS(1)) + // Rate-limits the check of old data to every 2 hours. #define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2)) @@ -143,7 +143,7 @@ extern "C" { #define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30)) // Defines a weighting function value for the linear model fit routine. -struct OverTempCalWeightPt { +struct OverTempCalWeight { // The age limit below which an offset will use this weight value. uint64_t offset_age_nanos; @@ -151,14 +151,6 @@ struct OverTempCalWeightPt { float weight; }; -// Over-temperature sensor offset estimate structure. -struct OverTempCalDataPt { - // Sensor offset estimate, temperature, and timestamp. - uint64_t timestamp_nanos; // [nanoseconds] - float offset_temp_celsius; // [Celsius] - float offset[3]; -}; - #ifdef OVERTEMPCAL_DBG_ENABLED // Debug printout state enumeration. enum OverTempCalDebugState { @@ -173,7 +165,7 @@ enum OverTempCalDebugState { // OverTempCal debug information/data tracking structure. struct DebugOverTempCal { // The latest received offset estimate data. - struct OverTempCalDataPt latest_offset; + struct OverTempModelThreeAxis latest_offset; // The maximum model error over all model_data points. float max_error[3]; @@ -184,38 +176,55 @@ struct DebugOverTempCal { }; #endif // OVERTEMPCAL_DBG_ENABLED +// OverTempCal algorithm parameters (see OverTempCal for details). +struct OverTempCalParameters { + uint64_t min_temp_update_period_nanos; + uint64_t age_limit_nanos; + float delta_temp_per_bin; // [Celsius/bin] + float jump_tolerance; // [sensor units] + float outlier_limit; // [sensor units/Celsius] + float temp_sensitivity_limit; // [sensor units/Celsius] + float sensor_intercept_limit; // [sensor units] + float significant_offset_change; // [sensor units] + size_t min_num_model_pts; + bool over_temp_enable; +}; + // The following data structure contains all of the necessary components for // modeling a sensor's temperature dependency and providing over-temperature // offset corrections. struct OverTempCal { // Storage for over-temperature model data. - struct OverTempCalDataPt model_data[OTC_MODEL_SIZE]; + struct OverTempModelThreeAxis model_data[OTC_MODEL_SIZE]; // Implements a weighting function to emphasize fitting a linear model to // younger offset estimates. - struct OverTempCalWeightPt weighting_function[OTC_NUM_WEIGHT_LEVELS]; + struct OverTempCalWeight weighting_function[OTC_NUM_WEIGHT_LEVELS]; // The active over-temperature compensated offset estimate data. Contains the // current sensor temperature at which offset compensation is performed. - struct OverTempCalDataPt compensated_offset; + struct OverTempModelThreeAxis compensated_offset; // Timer used to limit the rate at which old estimates are removed from // the 'model_data' collection. - uint64_t stale_data_timer; // [nanoseconds] + uint64_t stale_data_timer_nanos; // Duration beyond which data will be removed to avoid corrupting the model // with drift-compromised data. - uint64_t age_limit_nanos; // [nanoseconds] + uint64_t age_limit_nanos; // Timestamp of the last OTC offset compensation update. - uint64_t last_offset_update_nanos; // [nanoseconds] + uint64_t last_offset_update_nanos; // Timestamp of the last OTC model update. - uint64_t last_model_update_nanos; // [nanoseconds] + uint64_t last_model_update_nanos; + + // Timestamp of the last OTC model dataset age update. + uint64_t last_age_update_nanos; // Limit on the minimum interval for offset update calculations resulting from // an arbitrarily high temperature sampling rate. - uint64_t min_temp_update_period_nanos; // [nanoseconds] + uint64_t min_temp_update_period_nanos; ///// Online Model Identification Parameters //////////////////////////////// // @@ -229,17 +238,17 @@ struct OverTempCal { // model_temp_span >= 'num_model_pts' * delta_temp_per_bin // 2) A new set of model parameters are accepted if: // i. The model fit parameters must be within certain absolute bounds: - // a. ABS(temp_sensitivity) < temp_sensitivity_limit - // b. ABS(sensor_intercept) < sensor_intercept_limit - float temp_sensitivity_limit; // [sensor units/Celsius] - float sensor_intercept_limit; // [sensor units] + // a. |temp_sensitivity| < temp_sensitivity_limit + // b. |sensor_intercept| < sensor_intercept_limit + float temp_sensitivity_limit; // [sensor units/Celsius] + float sensor_intercept_limit; // [sensor units] size_t min_num_model_pts; // Pointer to the offset estimate closest to the current sensor temperature. - struct OverTempCalDataPt *nearest_offset; + struct OverTempModelThreeAxis *nearest_offset; // Pointer to the most recent offset estimate. - struct OverTempCalDataPt *latest_offset; + struct OverTempModelThreeAxis *latest_offset; // Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius]. float temp_sensitivity[3]; @@ -251,15 +260,15 @@ struct OverTempCal { // above which the model fit is preferred for providing offset compensation // (also applies to checks between the nearest-temperature and the current // compensated estimate). - float jump_tolerance; // [sensor units] + float jump_tolerance; // [sensor units] // A limit used to reject new offset estimates that deviate from the current // model fit. - float outlier_limit; // [sensor units] + float outlier_limit; // [sensor units] // This parameter is used to detect offset changes that require updates to // system calibration and persistent memory storage. - float significant_offset_change; // [sensor units] + float significant_offset_change; // [sensor units] // Used to track the previous significant change in temperature. float last_temp_check_celsius; @@ -282,7 +291,7 @@ struct OverTempCal { // recent estimates in cases where they arrive frequently near a given // temperature, and prevents model oversampling (i.e., dominance of estimates // concentrated at a given set of temperatures). - float delta_temp_per_bin; // [Celsius/bin] + float delta_temp_per_bin; // [Celsius/bin] // Total number of model data points collected. size_t num_model_pts; @@ -299,8 +308,7 @@ struct OverTempCal { // overTempCalNewModelUpdateAvailable() is called. This variable indicates // that the following should be stored in persistent system memory: // 1) 'temp_sensitivity' and 'sensor_intercept'. - // 2) The 'compensated_offset' offset data - // (saving timestamp information is not required). + // 2) The 'compensated_offset' offset data. bool new_overtemp_model_available; // True when a new offset estimate has been computed and registers as a @@ -320,15 +328,15 @@ struct OverTempCal { uint64_t temperature_print_timer; #endif // OVERTEMPCAL_DBG_LOG_TEMP - size_t model_counter; // Model output print counter. - float otc_unit_conversion; // Unit conversion for debug display. - char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS"). - char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO"). - char otc_debug_tag[32]; // Temporary string descriptor. - size_t debug_num_model_updates; // Total number of model updates. - size_t debug_num_estimates; // Total number of offset estimates. - bool debug_print_trigger; // Flag used to trigger data printout. -#endif // OVERTEMPCAL_DBG_ENABLED + size_t model_counter; // Model output print counter. + float otc_unit_conversion; // Unit conversion for debug display. + char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS"). + char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO"). + char otc_debug_tag[32]; // Temporary string descriptor. + size_t debug_num_model_updates; // Total number of model updates. + size_t debug_num_estimates; // Total number of offset estimates. + bool debug_print_trigger; // Flag used to trigger data printout. +#endif // OVERTEMPCAL_DBG_ENABLED }; /////// FUNCTION PROTOTYPES /////////////////////////////////////////////////// @@ -338,6 +346,9 @@ struct OverTempCal { * * INPUTS: * over_temp_cal: Over-temp main data structure. + * parameters: An algorithm parameters that contains the + * following initialization variables. + * [parameters]: * min_num_model_pts: Minimum number of model points per model * calculation update. * min_temp_update_period_nanos: Limits the rate of offset updates due to an @@ -351,19 +362,14 @@ struct OverTempCal { * temp_sensitivity_limit: Values that define the upper limits for the * sensor_intercept_limit: model parameters. The acceptance of new model * parameters must satisfy: - * i. ABS(temp_sensitivity) < temp_sensitivity_limit - * ii. ABS(sensor_intercept) < sensor_intercept_limit + * i. |temp_sensitivity| < temp_sensitivity_limit + * ii. |sensor_intercept| < sensor_intercept_limit * significant_offset_change Minimum limit that triggers offset updates. * over_temp_enable: Flag that determines whether over-temp sensor * offset compensation is applied. */ void overTempCalInit(struct OverTempCal *over_temp_cal, - size_t min_num_model_pts, - uint64_t min_temp_update_period_nanos, - float delta_temp_per_bin, float jump_tolerance, - float outlier_limit, uint64_t age_limit_nanos, - float temp_sensitivity_limit, float sensor_intercept_limit, - float significant_offset_change, bool over_temp_enable); + const struct OverTempCalParameters *parameters); /* * Sets the over-temp calibration model parameters. @@ -417,7 +423,7 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, */ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, size_t data_length, uint64_t timestamp_nanos, - const struct OverTempCalDataPt *model_data); + const struct OverTempModelThreeAxis *model_data); /* * Gets the over-temp compensation model data set. @@ -432,7 +438,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, */ void overTempCalGetModelData(struct OverTempCal *over_temp_cal, size_t *data_length, - struct OverTempCalDataPt *model_data); + struct OverTempModelThreeAxis *model_data); /* * Gets the current over-temp compensated offset estimate data. @@ -472,8 +478,8 @@ bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal); bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal); /* - * Updates the sensor's offset estimate and conditionally assimilates it into - * the over-temp model data set, 'model_data'. + * Updates the sensor's offset estimate and conditionally incorporates it into + * the over-temp model data set, 'model_data'. Updates the model dataset age. * * INPUTS: * over_temp_cal: Over-temp data structure. @@ -491,7 +497,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, // Updates the temperature at which the offset compensation is performed (i.e., // the current measured temperature value). This function is provided mainly for // flexibility since temperature updates may come in from a source other than -// the sensor itself, and at a different rate. +// the sensor itself, and at a different rate. Since this function is +// periodically called, it is also used to update the age of the model +// estimates. void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float temperature_celsius); @@ -527,7 +535,9 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal, * age is less than 'offset_age_nanos'. NOTE: The ordering of the * 'offset_age_nanos' values in the weight function array should be * monotonically increasing from lowest index to highest so that weighting - * selection can be conveniently evaluated. + * selection can be conveniently evaluated. A simple compliance check is + * applied, and 'true' is returned when the input weight is positive and older + * than the 'index-1' weight's age. * * INPUTS: * over_temp_cal: Over-temp data structure. @@ -536,9 +546,9 @@ void overTempGetModelError(const struct OverTempCal *over_temp_cal, * value and corresponding age limit below which an offset * will use the weight. */ -void overTempSetWeightingFunction( +bool overTempValidateAndSetWeight( struct OverTempCal *over_temp_cal, size_t index, - const struct OverTempCalWeightPt *new_otc_weight); + const struct OverTempCalWeight *new_otc_weight); #ifdef OVERTEMPCAL_DBG_ENABLED // This debug printout function assumes the input sensor data is a gyroscope diff --git a/firmware/os/algos/calibration/over_temp/over_temp_model.h b/firmware/os/algos/calibration/over_temp/over_temp_model.h new file mode 100644 index 00000000..f359c3b3 --- /dev/null +++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_ + +#include <stdint.h> + +#ifdef __cplusplus +extern "C" { +#endif + +// Defines the maximum size of the OverTempCal 'model_data' array. +#define OTC_MODEL_SIZE (40) + +/* + * Over-temperature data structures that contain a modeled sensor offset + * estimate, an associated temperature, and the age of the data point since it + * first entered the model. + */ + +struct OverTempModelThreeAxis { + // Sensor offset estimate, temperature, and age timestamp. + uint64_t offset_age_nanos; // [nanoseconds] + float offset_temp_celsius; // [Celsius] + + // Three-axis offset estimate (indices: 0=x, 1=y, 2=z). + float offset[3]; +}; + +struct OverTempModelSingleAxis { + // Sensor offset estimate, temperature, and age timestamp. + uint64_t offset_age_nanos; // [nanoseconds] + float offset_temp_celsius; // [Celsius] + + // Single-axis offset estimate. + float offset; +}; + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_ diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c new file mode 100644 index 00000000..d1b66195 --- /dev/null +++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/sample_rate_estimator/sample_rate_estimator.h" + +#include <string.h> + +#include "common/math/macros.h" +#include "util/nano_assert.h" + +// Helper function used to reset the sampling rate estimator accumulator. +static void sampleRateEstimatorResetAccumulator( + struct SampleRateEstimator* sample_rate_estimator) { + sample_rate_estimator->last_timestamp_nanos = 0.0f; + sample_rate_estimator->interval_accumulator_nanos = 0.0f; + sample_rate_estimator->num_intervals_collected = 0; +} + +void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator, + size_t num_intervals_to_collect, + float max_interval_sec) { + ASSERT_NOT_NULL(sample_rate_estimator); + memset(sample_rate_estimator, 0, sizeof(struct SampleRateEstimator)); + sample_rate_estimator->mean_sampling_rate_estimate_hz = + SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ; + sample_rate_estimator->num_intervals_to_collect = num_intervals_to_collect; + sample_rate_estimator->max_interval_nanos = + max_interval_sec * SEC_TO_NANOS(1); +} + +float sampleRateEstimatorGetHz( + struct SampleRateEstimator* sample_rate_estimator) { + sample_rate_estimator->new_sampling_rate_estimate_ready = false; + return sample_rate_estimator->mean_sampling_rate_estimate_hz; +} + +void sampleRateEstimatorUpdate( + struct SampleRateEstimator* sample_rate_estimator, + uint64_t timestamp_nanos) { + // Resets the current interval capture and returns if: + // 1. A bad timestamp was received (i.e., time not monotonic). + // 2. 'last_timestamp_nanos' is zero. NOTE: 'last_timestamp_nanos' = 0 + // indicates that the first complete time interval has not been captured + // yet (so, set it and return). + if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos || + sample_rate_estimator->last_timestamp_nanos == 0) { + sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; + return; + } + + // Computes the current sampling interval. This conversion will be very fast + // for intervals less than ~4.3 seconds (i.e., 2^32 nano-seconds). + const float next_interval_nanos = floatFromUint64( + timestamp_nanos - sample_rate_estimator->last_timestamp_nanos); + + // Helps prevent corruption of the estimator when there are gaps in the input + // sampling intervals greater than 'max_interval_nanos' (i.e., intermittant + // periods where there are no input timestamps). + if (next_interval_nanos >= sample_rate_estimator->max_interval_nanos) { + // Resets the estimator and returns. + sampleRateEstimatorResetAccumulator(sample_rate_estimator); + return; + } + + // Accumulates the next sampling interval. + sample_rate_estimator->interval_accumulator_nanos += next_interval_nanos; + sample_rate_estimator->last_timestamp_nanos = timestamp_nanos; + sample_rate_estimator->num_intervals_collected++; + + // If the number of collected time intervals exceed the target number, then + // this computes a new sample rate estimate. + if (sample_rate_estimator->num_intervals_collected > + sample_rate_estimator->num_intervals_to_collect) { + sample_rate_estimator->mean_sampling_rate_estimate_hz = + sample_rate_estimator->num_intervals_collected * + (SEC_TO_NANOS(1) / sample_rate_estimator->interval_accumulator_nanos); + + // Sets the polling flag to indicate that a new estimate is ready. + sample_rate_estimator->new_sampling_rate_estimate_ready = true; + + // Resets the estimator variables. + sampleRateEstimatorResetAccumulator(sample_rate_estimator); + } +} diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h new file mode 100644 index 00000000..f506eea5 --- /dev/null +++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2018 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * [Sample Rate Estimator] + * This module periodically estimates the mean sampling rate of a uniformly + * sampled signal. Note, this estimator uses a floating point accumulator for + * the timing intervals. This trades-off some accumulation of finite precision + * error to enhance the range of estimated sampling rates and prevent overflow + * when an equivalent 32bit integer accumulator is used. In typical use cases + * (sample rates: 5Hz - 2kHz, num_intervals_to_collect 10 - 100), the sample + * rate accuracy is typically much better than 0.1Hz. + * + * FUNCTIONALITY: + * sampleRateEstimatorInit -- Initializes the estimator. Sets the number of time + * intervals require to compute the sample rate mean, and the upper limit for + * the acceptable time interval. + * + * new_sampling_rate_estimate_ready -- Check this boolean flag if polling for + * new estimates is desired. + * + * sampleRateEstimatorGetHz -- Returns the latest sample rate estimate and + * resets the polling flag. + * + * sampleRateEstimatorUpdate -- Processes new timestamp data and computes new + * sample rate estimates. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_ + +#include <stdbool.h> +#include <stdint.h> +#include <sys/types.h> + +#ifdef __cplusplus +extern "C" { +#endif + +// Designates the value used to indicate an invalid sample rate estimate. +#define SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ (-1.0f) + +// Sample rate estimator data structure. +struct SampleRateEstimator { + // Used to compute sample intervals to estimate the sampling rate. + uint64_t last_timestamp_nanos; + + // Accumulator used for computing the mean sampling interval. + float interval_accumulator_nanos; + + // The upper limit on the acceptance of valid time intervals (in nanoseconds). + // Larger time deltas result in a reset of the interval accumulator. + float max_interval_nanos; + + // The most recent mean sampling rate estimate. + float mean_sampling_rate_estimate_hz; + + /* + * The targeted number of time intervals required for a new sample rate mean + * calculation. + * + * NOTE: Assuming that the time interval noise is independent and identically + * distributed and drawn from a zero-mean normal distribution with variance + * 'Sigma^2'. The expected noise reduction is related to the choice of + * 'num_intervals_to_collect' as: + * + * Output RMS Noise = Sigma / sqrt(num_intervals_to_collect). + * + * Example, a value of 100 for a 100Hz signal would provide updates once every + * second and provide a 90% reduction (i.e., [1 - 1/sqrt(100)] * 100%) in time + * interval noise. + */ + size_t num_intervals_to_collect; + + // The number of time intervals currently collected. + size_t num_intervals_collected; + + // Polling flag used to signal that a new estimate is ready. This flag is + // reset when 'sampleRateEstimatorGetHz' is called. + bool new_sampling_rate_estimate_ready; +}; + +// Initializes the sample rate estimator, sets the number of time intervals +// for the mean sample rate calculation, and defines the tolerable gap in timing +// data (in seconds). +void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator, + size_t num_intervals_to_collect, + float max_interval_sec); + +// Rather than using a function to poll for new sample rate estimates, which +// would incur an additional function call, simply check +// 'new_sampling_rate_estimate_ready' directly. + +// Returns the most recent sampling rate estimate, and resets the +// 'new_sampling_rate_estimate_ready' polling flag. Note, if polling is not +// used, one may access the sample rate estimate directly from the struct and +// avoid this function call. +float sampleRateEstimatorGetHz( + struct SampleRateEstimator* sample_rate_estimator); + +// Takes in a new timestamp and updates the sampling rate estimator. +void sampleRateEstimatorUpdate( + struct SampleRateEstimator* sample_rate_estimator, + uint64_t timestamp_nanos); + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_ diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/sphere_fit/calibration_data.c index 9ae72d27..2de4a8c0 100644 --- a/firmware/os/algos/calibration/common/calibration_data.c +++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.c @@ -1,4 +1,20 @@ -#include "calibration/common/calibration_data.h" +/* + * Copyright (C) 2016 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/sphere_fit/calibration_data.h" #include <string.h> @@ -9,7 +25,7 @@ // Set calibration data to identity scale factors, zero skew and // zero bias. -void calDataReset(struct ThreeAxisCalData *calstruct) { +void calDataReset(struct ThreeAxisCalData* calstruct) { memset(calstruct, 0, sizeof(struct ThreeAxisCalData)); calstruct->scale_factor_x = 1.0f; calstruct->scale_factor_y = 1.0f; @@ -28,9 +44,9 @@ void calDataCorrectData(const struct ThreeAxisCalData* calstruct, // 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[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]; + 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/sphere_fit/calibration_data.h index b0e6809b..405d0acc 100644 --- a/firmware/os/algos/calibration/common/calibration_data.h +++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.h @@ -13,7 +13,7 @@ * 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, @@ -23,8 +23,9 @@ * * 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_ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_ #include <stdint.h> @@ -57,7 +58,7 @@ struct ThreeAxisCalData { // Set calibration data to identity scale factors, zero skew and // zero bias. -void calDataReset(struct ThreeAxisCalData *calstruct); +void calDataReset(struct ThreeAxisCalData* calstruct); // Apply a stored calibration to correct a single sample of impaired sensor // data. @@ -69,4 +70,4 @@ void calDataCorrectData(const struct ThreeAxisCalData* calstruct, } #endif -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_ +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_ diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c index 2c26af55..853a73d5 100644 --- a/firmware/os/algos/calibration/common/sphere_fit_calibration.c +++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c @@ -1,4 +1,20 @@ -#include "calibration/common/sphere_fit_calibration.h" +/* + * Copyright (C) 2016 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "calibration/sphere_fit/sphere_fit_calibration.h" #include <errno.h> #include <stdarg.h> @@ -19,7 +35,7 @@ static bool runCalibration(struct SphereFitCal *sphere_cal, const struct SphereFitData *data, uint64_t timestamp_nanos); -#define MIN_VALID_DATA_NORM (1e-4) +#define MIN_VALID_DATA_NORM (1e-4f) // FUNCTION IMPLEMENTATIONS ////////////////////////////////////////////////////////////////////////////// @@ -103,7 +119,7 @@ void sphereFitResidAndJacobianFunc(const float *state, const void *f_data, ASSERT_NOT_NULL(f_data); ASSERT_NOT_NULL(residual); - const struct SphereFitData *data = (const struct SphereFitData*)f_data; + 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; @@ -204,10 +220,9 @@ bool runCalibration(struct SphereFitCal *sphere_cal, 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); + 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 || @@ -215,32 +230,34 @@ bool runCalibration(struct SphereFitCal *sphere_cal, // 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 + CAL_DEBUG_LOG("[SPHERE CAL]", + "Solution found in %d iterations with status %d.\n", + sphere_cal->lm_solver.num_iter, status); + CAL_DEBUG_LOG("[SPHERE CAL]", "Solution:\n {" + CAL_FORMAT_6DIGITS " [M(1,1)], " + CAL_FORMAT_6DIGITS " [M(2,1)], " + CAL_FORMAT_6DIGITS " [M(2,2)], \n" + CAL_FORMAT_6DIGITS " [M(3,1)], " + CAL_FORMAT_6DIGITS " [M(3,2)], " + CAL_FORMAT_6DIGITS " [M(3,3)], \n" + CAL_FORMAT_6DIGITS " [b(1)], " + CAL_FORMAT_6DIGITS " [b(2)], " + CAL_FORMAT_6DIGITS " [b(3)]}.", + CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6), + CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6), + CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6), + CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6), + CAL_ENCODE_FLOAT(x_sol[8], 6)); +#endif // SPHERE_FIT_DBG_ENABLED memcpy(sphere_cal->x, x_sol, sizeof(x_sol)); sphere_cal->estimate_time_nanos = timestamp_nanos; return true; } else { #ifdef SPHERE_FIT_DBG_ENABLED - CAL_DEBUG_LOG( - "[SPHERE CAL]", - "Solution failed in %d iterations with status %d.\n", - sphere_cal->lm_solver.num_iter, status); -#endif + CAL_DEBUG_LOG("[SPHERE CAL]", + "Solution failed in %d iterations with status %d.\n", + sphere_cal->lm_solver.num_iter, status); +#endif // SPHERE_FIT_DBG_ENABLED } return false; diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h index e49f225a..d3bbf7f1 100644 --- a/firmware/os/algos/calibration/common/sphere_fit_calibration.h +++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h @@ -13,7 +13,7 @@ * 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 @@ -34,13 +34,14 @@ * 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_ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_ #include <stdbool.h> #include <stdint.h> -#include "calibration/common/calibration_data.h" +#include "calibration/sphere_fit/calibration_data.h" #include "common/math/levenberg_marquardt.h" #ifdef __cplusplus @@ -140,4 +141,4 @@ void sphereFitResidAndJacobianFunc(const float *state, const void *f_data, } #endif -#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_ +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_ diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h index 1bd80fd4..46297dbb 100644 --- a/firmware/os/algos/calibration/util/cal_log.h +++ b/firmware/os/algos/calibration/util/cal_log.h @@ -16,12 +16,13 @@ /////////////////////////////////////////////////////////////// /* - * Logging macros for printing formatted strings to Nanohub. + * Logging macros for printing user-debug messages. */ /////////////////////////////////////////////////////////////// #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_ +// clang-format off #ifdef GCC_DEBUG_LOG # include <stdio.h> # define CAL_DEBUG_LOG(tag, fmt, ...) \ @@ -35,31 +36,61 @@ LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__) # define CAL_DEBUG_LOG(tag, fmt, ...) \ osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__); -#else // _OS_BUILD_ +#elif NANOHUB_DEBUG_LOG # include <chre.h> # define CAL_DEBUG_LOG(tag, fmt, ...) \ chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__) +#else +// CHRE/SLPI Nanoapp Logging. +# include "chre/util/nanoapp/log.h" +# ifndef LOG_TAG +# define LOG_TAG "" +# endif // LOG_TAG +# define CAL_DEBUG_LOG(tag, format, ...) \ + LOGI("%s " format, tag, ##__VA_ARGS__) #endif // GCC_DEBUG_LOG +// clang-format on #ifdef __cplusplus extern "C" { #endif -// Using this macro because floorf() is not currently implemented by the Nanohub -// firmware. +// Floor macro implementation for platforms that do not supply the standard +// floorf() math function. #define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT +/* + * On some embedded software platforms numerical string formatting is not fully + * supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that + * prints floating-point values having a specified number of digits using the + * CAL_ENCODE_FLOAT macro. + * Examples: + * - Nanohub does not support floating-point format strings. + * - CHRE/SLPI allows %.Xf for printing float values. + */ +#ifdef CAL_NO_FLOAT_FORMAT_STRINGS // Macro used to print floating point numbers with a specified number of digits. -#define CAL_ENCODE_FLOAT(x, num_digits) \ - ((x < 0) ? "-" : ""), \ - (int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits)) // NOLINT +# define CAL_ENCODE_FLOAT(x, num_digits) \ + ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)), \ + (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * \ + powf(10, num_digits)) // NOLINT // Helper definitions for CAL_ENCODE_FLOAT to specify the print format with // desired significant digits. -#define CAL_FORMAT_3DIGITS "%s%d.%03d" -#define CAL_FORMAT_6DIGITS "%s%d.%06d" -#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d" -#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d" +# define CAL_FORMAT_3DIGITS "%s%d.%03d" +# define CAL_FORMAT_6DIGITS "%s%d.%06d" +# define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d" +# define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d" +#else +// Pass-through when float string formatting (e.g., %.6f) is available. +# define CAL_ENCODE_FLOAT(x, num_digits) (x) + +// Float string formatting helpers. +# define CAL_FORMAT_3DIGITS "%.3f" +# define CAL_FORMAT_6DIGITS "%.6f" +# define CAL_FORMAT_3DIGITS_TRIPLET "%.3f, %.3f, %.3f" +# define CAL_FORMAT_6DIGITS_TRIPLET "%.6f, %.6f, %.6f" +#endif // CAL_NO_FLOAT_FORMAT_STRINGS #ifdef __cplusplus } diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c new file mode 100644 index 00000000..a24a31bf --- /dev/null +++ b/firmware/os/algos/common/math/kasa.c @@ -0,0 +1,120 @@ +#include "common/math/kasa.h" + +#include <stdint.h> +#include <sys/types.h> + +#include "common/math/mat.h" + +void kasaReset(struct KasaFit *kasa) { + kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f; + kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f; + kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f; + kasa->acc_zz = kasa->acc_zw = 0.0f; + kasa->nsamples = 0; +} + +void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); } + +void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) { + float w = x * x + y * y + z * z; + + kasa->acc_x += x; + kasa->acc_y += y; + kasa->acc_z += z; + kasa->acc_w += w; + + kasa->acc_xx += x * x; + kasa->acc_xy += x * y; + kasa->acc_xz += x * z; + kasa->acc_xw += x * w; + + kasa->acc_yy += y * y; + kasa->acc_yz += y * z; + kasa->acc_yw += y * w; + + kasa->acc_zz += z * z; + kasa->acc_zw += z * w; + + kasa->nsamples += 1; +} + +bool kasaNormalize(struct KasaFit *kasa) { + if (kasa->nsamples == 0) { + return false; + } + + float inv = 1.0f / kasa->nsamples; + + kasa->acc_x *= inv; + kasa->acc_y *= inv; + kasa->acc_z *= inv; + kasa->acc_w *= inv; + + kasa->acc_xx *= inv; + kasa->acc_xy *= inv; + kasa->acc_xz *= inv; + kasa->acc_xw *= inv; + + kasa->acc_yy *= inv; + kasa->acc_yz *= inv; + kasa->acc_yw *= inv; + + kasa->acc_zz *= inv; + kasa->acc_zw *= inv; + + return true; +} + +int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius, + float max_fit, float min_fit) { + // A * out = b + // (4 x 4) (4 x 1) (4 x 1) + struct Mat44 A; + A.elem[0][0] = kasa->acc_xx; + A.elem[0][1] = kasa->acc_xy; + A.elem[0][2] = kasa->acc_xz; + A.elem[0][3] = kasa->acc_x; + A.elem[1][0] = kasa->acc_xy; + A.elem[1][1] = kasa->acc_yy; + A.elem[1][2] = kasa->acc_yz; + A.elem[1][3] = kasa->acc_y; + A.elem[2][0] = kasa->acc_xz; + A.elem[2][1] = kasa->acc_yz; + A.elem[2][2] = kasa->acc_zz; + A.elem[2][3] = kasa->acc_z; + A.elem[3][0] = kasa->acc_x; + A.elem[3][1] = kasa->acc_y; + A.elem[3][2] = kasa->acc_z; + A.elem[3][3] = 1.0f; + + struct Vec4 b; + initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w); + + struct Size4 pivot; + mat44DecomposeLup(&A, &pivot); + + struct Vec4 out; + mat44Solve(&A, &out, &b, &pivot); + + // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2 + // + // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2 + // r = sqrt(xc^2 + yc^2 + zc^2 - out[3]) + + struct Vec3 v; + initVec3(&v, out.x, out.y, out.z); + vec3ScalarMul(&v, -0.5f); + + float r_square = vec3Dot(&v, &v) - out.w; + float r = (r_square > 0) ? sqrtf(r_square) : 0; + + initVec3(bias, v.x, v.y, v.z); + *radius = r; + + int success = 0; + if (r > min_fit && r < max_fit) { + success = 1; + } + + return success; +} diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h new file mode 100644 index 00000000..e9652d60 --- /dev/null +++ b/firmware/os/algos/common/math/kasa.h @@ -0,0 +1,54 @@ +/* + * This module provides a data structure, initialization, and fit + * routine for algorithms that use the Kasa method for determining the + * 3-dimensional offset vector from a set of points on a sphere. + * + * Reference: I. Kåsa, "A circle fitting procedure and its error analysis," in + * IEEE Transactions on Instrumentation and Measurement, vol. IM-25, no. 1, pp. + * 8-14, March 1976. + */ + +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_ + +#include <stdbool.h> + +#include "common/math/vec.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct KasaFit { + float acc_x, acc_y, acc_z, acc_w; + float acc_xx, acc_xy, acc_xz, acc_xw; + float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw; + size_t nsamples; +}; + +// Resets the KasaFit data structure (sets all variables to zero). +void kasaReset(struct KasaFit *kasa); + +// Initializes the KasaFit data structure. +void kasaInit(struct KasaFit *kasa); + +// Accumulates the Kasa acc_** variables with the input vector [x, y, z], and +// updates the number of samples. +void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z); + +// Normalizes the Kasa acc_** variables. Returns 'false' if the number of +// samples is zero, otherwise 'true'. +bool kasaNormalize(struct KasaFit *kasa); + +// Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for +// the best-fit sphere using the normal equations, and the sphere's 'radius'. +// Returns '1' if the radius of the fit sphere is within the bounds +// (min_fit, max_fit), otherwise '0'. +int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius, + float max_fit, float min_fit); + +#ifdef __cplusplus +} +#endif + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_ diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h index c01d2eb1..d62308da 100644 --- a/firmware/os/algos/common/math/levenberg_marquardt.h +++ b/firmware/os/algos/common/math/levenberg_marquardt.h @@ -109,7 +109,7 @@ struct LmSolver { // Initializes LM solver with provided parameters and error function. void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, - ResidualAndJacobianFunction error_func); + ResidualAndJacobianFunction func); void lmSolverDestroy(struct LmSolver *solver); @@ -133,7 +133,7 @@ void lmSolverSetData(struct LmSolver *solver, struct LmData *data); */ enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state, void *f_data, size_t state_dim, size_t meas_dim, - float *est_state); + float *state); ////////////////////////// TEST UTILITIES //////////////////////////////////// // This function is exposed here for testing purposes only. diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h index 5c06e247..cb75595b 100644 --- a/firmware/os/algos/common/math/macros.h +++ b/firmware/os/algos/common/math/macros.h @@ -14,30 +14,43 @@ * limitations under the License. */ -// This file contains helper macros and definitions. +// This file contains frequently used constants and helper macros. + +#include <stdint.h> #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_ -// Mathematical constants. -#define NANO_PI (3.14159265359f) +// Constants. +#define NANO_PI (3.14159265359f) +#define INVALID_TEMPERATURE_CELSIUS (-274.0f) // Common math operations. #define NANO_ABS(x) ((x) > 0 ? (x) : -(x)) #define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b) #define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b) +#define SIGMOID(x) (1 / (1 + expf(-x))) // Timestamp conversion macros. #ifdef __cplusplus -#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000) +#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000))) #else -#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000) // NOLINT +#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000))) // NOLINT #endif -#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * 1000) -#define MIN_TO_NANOS(x) SEC_TO_NANOS(x * 60) -#define HRS_TO_NANOS(x) MIN_TO_NANOS(x * 60) -#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24) +#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * UINT64_C(1000)) +#define MIN_TO_NANOS(x) SEC_TO_NANOS (x * UINT64_C(60)) +#define HRS_TO_NANOS(x) MIN_TO_NANOS (x * UINT64_C(60)) +#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24)) + +// Sample rate to period conversion. +#ifdef __cplusplus +#define HZ_TO_PERIOD_NANOS(hz) \ + (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024))) +#else +#define HZ_TO_PERIOD_NANOS(hz) \ + (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024))) // NOLINT +#endif // Unit conversion: nanoseconds to seconds. #define NANOS_TO_SEC (1.0e-9f) @@ -57,4 +70,26 @@ #define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \ (((t1) >= (t2) + (t_delta)) || ((t1) < (t2))) +#ifdef __cplusplus +extern "C" { +#endif + +// This conversion function may be necessary for embedded hardware that can't +// cast a uint64_t to a float directly. This conversion function was taken from: +// [android]//device/google/contexthub/firmware/os/core/floatRt.c +static inline float floatFromUint64(uint64_t v) { + uint32_t hi = v >> 32; + uint32_t lo = (uint32_t) v; + + if (!hi) { // This is very fast for cases where 'v' fits into a uint32_t. + return (float)lo; + } else { + return ((float)hi) * 4294967296.0f + (float)lo; + } +} + +#ifdef __cplusplus +} +#endif + #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_ diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c index 34aaa512..8b62cceb 100644 --- a/firmware/os/algos/common/math/mat.c +++ b/firmware/os/algos/common/math/mat.c @@ -32,8 +32,8 @@ #include <stddef.h> #include <string.h> -#define EPSILON 1E-5 -#define CHOLESKY_TOLERANCE 1E-6 +#define EPSILON 1E-5f +#define CHOLESKY_TOLERANCE 1E-6f // Forward declarations. static void mat33SwapRows(struct Mat33 *A, uint32_t i, uint32_t j); @@ -248,8 +248,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { } } // divide by zero guard. - ASSERT(fabs(tmp.elem[i][i]) > 0); - if(!(fabs(tmp.elem[i][i]) > 0)) { + ASSERT(fabsf(tmp.elem[i][i]) > 0); + if(!(fabsf(tmp.elem[i][i]) > 0)) { return; } t = 1.0f / tmp.elem[i][i]; @@ -407,6 +407,16 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, initVec3(eigenvals, _eigenvals[0], _eigenvals[1], _eigenvals[2]); } +float mat33Determinant(const struct Mat33 *A) { + ASSERT_NOT_NULL(A); + return A->elem[0][0] * + (A->elem[1][1] * A->elem[2][2] - A->elem[1][2] * A->elem[2][1]) + - A->elem[0][1] * + (A->elem[1][0] * A->elem[2][2] - A->elem[1][2] * A->elem[2][0]) + + A->elem[0][2] * + (A->elem[1][0] * A->elem[2][1] - A->elem[1][1] * A->elem[2][0]); +} + // index of largest off-diagonal element in row k UNROLLED uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) { diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h index 13494f55..9d69405e 100644 --- a/firmware/os/algos/common/math/mat.h +++ b/firmware/os/algos/common/math/mat.h @@ -125,6 +125,8 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A); void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, struct Mat33 *eigenvecs); +// Computes the determinant of a 3 by 3 matrix. +float mat33Determinant(const struct Mat33 *A); // 4x4 MATRIX MATH ///////////////////////////////////////////////////////////// // Updates out with the multiplication of A and v, i.e.: diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h index 0a4c8b39..e839ad53 100644 --- a/firmware/os/algos/common/math/vec.h +++ b/firmware/os/algos/common/math/vec.h @@ -70,6 +70,17 @@ static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) { v->z += w->z; } +// Sets u as the sum of v and w. +static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v, + const struct Vec3 *w) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + u->x = v->x + w->x; + u->y = v->y + w->y; + u->z = v->z + w->z; +} + // Updates v as the subtraction of w from v. static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { ASSERT_NOT_NULL(v); @@ -79,6 +90,17 @@ static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { v->z -= w->z; } +// Sets u as the difference of v and w. +static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v, + const struct Vec3 *w) { + ASSERT_NOT_NULL(u); + ASSERT_NOT_NULL(v); + ASSERT_NOT_NULL(w); + u->x = v->x - w->x; + u->y = v->y - w->y; + u->z = v->z - w->z; +} + // Scales v by the scalar c, i.e. v = c * v. static inline void vec3ScalarMul(struct Vec3 *v, float c) { ASSERT_NOT_NULL(v); diff --git a/firmware/os/algos/util/array.h b/firmware/os/algos/util/array.h new file mode 100644 index 00000000..9658be44 --- /dev/null +++ b/firmware/os/algos/util/array.h @@ -0,0 +1,6 @@ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_ + +#define ARRAY_SIZE(a) (sizeof((a)) / sizeof((a)[0])) + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_ diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h index da777491..cb3286e2 100644 --- a/firmware/os/algos/util/nano_assert.h +++ b/firmware/os/algos/util/nano_assert.h @@ -1,7 +1,7 @@ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_ -#if defined(__NANOHUB__) || defined(_OS_BUILD_) +#ifndef GOOGLE3 // For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided // by the build system, which has the directory stripped. But allow the @@ -32,7 +32,7 @@ #include <assert.h> #define ASSERT_IMPL(x) assert(x) -#endif +#endif // GOOGLE3 #ifndef ASSERT #ifdef NANO_ASSERT_ENABLED diff --git a/firmware/os/algos/util/nano_log.h b/firmware/os/algos/util/nano_log.h new file mode 100644 index 00000000..8ec80430 --- /dev/null +++ b/firmware/os/algos/util/nano_log.h @@ -0,0 +1,13 @@ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_ + +#ifdef GOOGLE3 +#include <stdio.h> + +// ignore log level argument +#define LOG_FUNC(level, ...) printf(__VA_ARGS__) +#endif + +#include "third_party/contexthub/nanoapps/util/log/log.h" + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_ diff --git a/firmware/os/core/heap.c b/firmware/os/core/heap.c index 2f38a0da..f6e50672 100644 --- a/firmware/os/core/heap.c +++ b/firmware/os/core/heap.c @@ -229,3 +229,50 @@ int heapFreeAll(uint32_t tid) return count; } + +int heapGetFreeSize(int *numChunks, int *largestChunk) +{ + struct HeapNode *node; + bool haveLock; + int bytes = 0; + *numChunks = *largestChunk = 0; + + // this can only fail if called from interrupt + haveLock = trylockTryTake(&gHeapLock); + if (!haveLock) + return -1; + + for (node = gHeapHead; node; node = heapPrvGetNext(node)) { + if (!node->used) { + if (node->size > *largestChunk) + *largestChunk = node->size; + bytes += node->size + sizeof(struct HeapNode); + (*numChunks)++; + } + } + trylockRelease(&gHeapLock); + + return bytes; +} + +int heapGetTaskSize(uint32_t tid) +{ + struct HeapNode *node; + bool haveLock; + int bytes = 0; + + // this can only fail if called from interrupt + haveLock = trylockTryTake(&gHeapLock); + if (!haveLock) + return -1; + + tid &= TIDX_MASK; + for (node = gHeapHead; node; node = heapPrvGetNext(node)) { + if (node->used && node->tidx == tid) { + bytes += node->size + sizeof(struct HeapNode); + } + } + trylockRelease(&gHeapLock); + + return bytes; +} diff --git a/firmware/os/core/hostIntf.c b/firmware/os/core/hostIntf.c index 084e508c..ccd571d8 100644 --- a/firmware/os/core/hostIntf.c +++ b/firmware/os/core/hostIntf.c @@ -35,6 +35,7 @@ #include <nanohubCommand.h> #include <nanohubPacket.h> #include <seos.h> +#include <seos_priv.h> #include <util.h> #include <atomicBitset.h> #include <atomic.h> @@ -1097,18 +1098,36 @@ static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable, static void hostIntfNotifyReboot(uint32_t reason) { - struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp)); __le32 raw_reason = htole32(reason); + struct NanohubHalSysMgmtTx *resp; + resp = heapAlloc(sizeof(*resp)); if (resp) { - resp->hdr = (struct NanohubHalHdr){ + resp->hdr = (struct NanohubHalHdr) { .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), - .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg), - .msg = NANOHUB_HAL_REBOOT, + .len = sizeof(*resp) - sizeof(resp->hdr), }; - memcpy(&resp->reason, &raw_reason, sizeof(resp->reason)); - osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_SYS_MGMT, + .status = raw_reason, + }; + resp->cmd = NANOHUB_HAL_SYS_MGMT_REBOOT; + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + } + +#ifdef LEGACY_HAL_ENABLED + struct NanohubHalLegacyRebootTx *respLegacy; + respLegacy = heapAlloc(sizeof(*respLegacy)); + if (respLegacy) { + respLegacy->hdr = (struct NanohubHalLegacyHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*respLegacy) - sizeof(respLegacy->hdr) + sizeof(respLegacy->hdr.msg), + .msg = NANOHUB_HAL_LEGACY_REBOOT, + }; + memcpy(&respLegacy->reason, &raw_reason, sizeof(respLegacy->reason)); + osEnqueueEvtOrFree(EVT_APP_TO_HOST, respLegacy, heapFree); } +#endif } static void queueFlush(struct ActiveSensor *sensor) @@ -1151,9 +1170,10 @@ static void onEvtAppStart(const void *evtData) struct HostIntfDataBuffer *data; osEventUnsubscribe(mHostIntfTid, EVT_APP_START); - osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT); - osEventSubscribe(mHostIntfTid, EVT_APP_TO_SENSOR_HAL_DATA); - osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST); + osEventsSubscribe(4, EVT_NO_SENSOR_CONFIG_EVENT, + EVT_APP_TO_SENSOR_HAL_DATA, + EVT_APP_TO_HOST, + EVT_APP_TO_HOST_CHRE); #ifdef DEBUG_LOG_EVT osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG); platEarlyLogFlush(); @@ -1186,12 +1206,59 @@ static void onEvtAppToHost(const void *evtData) } } +static void onEvtAppToHostChre(const void *evtData) +{ + const struct HostHubChrePacket *hostMsg = evtData; + + if (hostMsg->messageSize <= HOST_HUB_CHRE_PACKET_MAX_LEN) { + struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->messageSize); + + data->sensType = SENS_TYPE_INVALID; + data->length = sizeof(*hostMsg) + hostMsg->messageSize; + data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST; + data->interrupt = NANOHUB_INT_WAKEUP; + memcpy(data->buffer, evtData, data->length); + hostIntfAddBlock(data, false, true); + } +} + +#ifdef LEGACY_HAL_ENABLED +static void handleLegacyHalCmd(const uint8_t *halData, uint8_t size) +{ + const struct NanohubHalLegacyCommand *halCmd = nanohubHalLegacyFindCommand(halData[0]); + if (halCmd) + halCmd->handler((void *)&halData[1], size - 1); +} + static void onEvtAppFromHost(const void *evtData) { const uint8_t *halMsg = evtData; - const struct NanohubHalCommand *halCmd = nanohubHalFindCommand(halMsg[1]); - if (halCmd) - halCmd->handler((void *)&halMsg[2], halMsg[0] - 1); + handleLegacyHalCmd(&halMsg[1], halMsg[0]); +} +#endif + +static void onEvtAppFromHostChre(const void *evtData) +{ + const struct NanohubMsgChreHdr *halMsg = (const struct NanohubMsgChreHdr *)evtData; + const struct NanohubHalCommand *halCmd; + const uint8_t *halData = (const uint8_t *)(halMsg+1); + uint8_t len; + uint32_t transactionId; + + memcpy(&transactionId, &halMsg->appEvent, sizeof(halMsg->appEvent)); + + if (halMsg->size >= 1) { + len = halMsg->size - 1; + halCmd = nanohubHalFindCommand(halData[0]); + if (halCmd) { + if (len >= halCmd->minDataLen && len <= halCmd->maxDataLen) + halCmd->handler((void *)&halData[1], len, transactionId); + return; + } + } +#ifdef LEGACY_HAL_ENABLED + handleLegacyHalCmd(halData, halMsg->size); +#endif } #ifdef DEBUG_LOG_EVT @@ -1485,16 +1552,24 @@ static void onEvtSensorData(uint32_t evtType, const void* evtData) static void hostIntfHandleEvent(uint32_t evtType, const void* evtData) { - switch (evtType) { + switch (EVENT_GET_EVENT(evtType)) { case EVT_APP_START: onEvtAppStart(evtData); break; case EVT_APP_TO_HOST: onEvtAppToHost(evtData); break; + case EVT_APP_TO_HOST_CHRE: + onEvtAppToHostChre(evtData); + break; +#ifdef LEGACY_HAL_ENABLED case EVT_APP_FROM_HOST: onEvtAppFromHost(evtData); break; +#endif + case EVT_APP_FROM_HOST_CHRE: + onEvtAppFromHostChre(evtData); + break; #ifdef DEBUG_LOG_EVT case EVT_DEBUG_LOG: onEvtDebugLog(evtData); @@ -1510,7 +1585,7 @@ static void hostIntfHandleEvent(uint32_t evtType, const void* evtData) onEvtAppToSensorHalData(evtData); break; default: - onEvtSensorData(evtType, evtData); + onEvtSensorData(EVENT_GET_EVENT(evtType), evtData); break; } } @@ -1613,4 +1688,4 @@ void hostIntfClearInterruptMask(uint32_t bit) cpuIntsRestore(state); } -INTERNAL_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent); +INTERNAL_CHRE_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent); diff --git a/firmware/os/core/nanohubCommand.c b/firmware/os/core/nanohubCommand.c index 67261188..d679e73d 100644 --- a/firmware/os/core/nanohubCommand.c +++ b/firmware/os/core/nanohubCommand.c @@ -39,6 +39,7 @@ #include <nanohubPacket.h> #include <eeData.h> #include <seos.h> +#include <seos_priv.h> #include <util.h> #include <mpu.h> #include <heap.h> @@ -49,6 +50,7 @@ #include <cpu.h> #include <cpu/cpuMath.h> #include <algos/ap_hub_sync.h> +#include <sensors_priv.h> #include <chre.h> @@ -56,9 +58,13 @@ { .reason = _reason, .fastHandler = _fastHandler, .handler = _handler, \ .minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) } -#define NANOHUB_HAL_COMMAND(_msg, _handler) \ +#define NANOHUB_HAL_LEGACY_COMMAND(_msg, _handler) \ { .msg = _msg, .handler = _handler } +#define NANOHUB_HAL_COMMAND(_msg, _handler, _minReqType, _maxReqType) \ + { .msg = _msg, .handler = _handler, \ + .minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) } + // maximum number of bytes to feed into appSecRxData at once // The bigger the number, the more time we block other event processing // appSecRxData only feeds 16 bytes at a time into writeCbk, so large @@ -91,6 +97,7 @@ struct DownloadState static struct DownloadState *mDownloadState; static AppSecErr mAppSecStatus; +static struct AppHdr *mApp; static struct SlabAllocator *mEventSlab; static struct HostIntfDataBuffer mTxCurr, mTxNext; static uint8_t mTxCurrLength, mTxNextLength; @@ -263,7 +270,7 @@ static void freeDownloadState() mDownloadState = NULL; } -static void resetDownloadState(bool initial) +static bool resetDownloadState(bool initial, bool erase) { bool doCreate = true; @@ -280,14 +287,19 @@ static void resetDownloadState(bool initial) else doCreate = false; } + mDownloadState->dstOffset = 0; if (doCreate) mDownloadState->start = osAppSegmentCreate(mDownloadState->size); - if (!mDownloadState->start) - mDownloadState->erase = true; - mDownloadState->dstOffset = 0; + if (!mDownloadState->start) { + if (erase) + mDownloadState->erase = true; + else + return false; + } + return true; } -static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req) +static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req, bool erase) { if (!mDownloadState) { mDownloadState = heapAlloc(sizeof(struct DownloadState)); @@ -301,9 +313,7 @@ static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req) mDownloadState->size = le32toh(req->size); mDownloadState->crc = le32toh(req->crc); mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED; - resetDownloadState(true); - - return true; + return resetDownloadState(true, erase); } static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp) @@ -311,7 +321,7 @@ static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t struct NanohubStartFirmwareUploadRequest *req = rx; struct NanohubStartFirmwareUploadResponse *resp = tx; - resp->accepted = doStartFirmwareUpload(req); + resp->accepted = doStartFirmwareUpload(req, true); return sizeof(*resp); } @@ -444,15 +454,18 @@ static uint32_t firmwareFinish(bool valid) if (!osAppSegmentClose(app, mDownloadState->dstOffset, segState)) { osLog(LOG_INFO, "%s: Failed to close segment\n", __func__); valid = false; + mApp = NULL; } else { segState = osAppSegmentGetState(app); + mApp = app; valid = (segState == SEG_ST_VALID); } osLog(LOG_INFO, "Loaded %s image type %" PRIu8 ": %" PRIu32 - " bytes @ %p; state=%02" PRIX32 "\n", + " bytes @ %p; state=%02" PRIX32 "; crc=%08" PRIX32 "\n", valid ? "valid" : "invalid", app->hdr.payInfoType, mDownloadState->size, - mDownloadState->start, segState); + mDownloadState->start, segState, + mApp ? osAppSegmentGetCrc(mApp) : 0xFFFFFFFF); freeDownloadState(); // no more access to mDownloadState @@ -502,11 +515,30 @@ static void firmwareErase(void *cookie) mDownloadState->eraseScheduled = false; } +SET_PACKED_STRUCT_MODE_ON +struct FirmwareWriteCookie +{ + uint32_t evtType; + union { +#ifdef LEGACY_HAL_ENABLED + struct NanohubHalLegacyContUploadTx respLegacy; +#endif + struct NanohubHalContUploadTx resp; + }; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +static void writeCookieFree(void *ptr) +{ + struct FirmwareWriteCookie *buf = container_of(ptr, struct FirmwareWriteCookie, resp); + heapFree(buf); +} + static void firmwareWrite(void *cookie) { bool valid; bool finished = false; - struct NanohubHalContUploadTx *resp = cookie; + struct FirmwareWriteCookie *resp = cookie; // only check crc when cookie is NULL (write came from kernel, not HAL) bool checkCrc = !cookie; @@ -545,8 +577,15 @@ static void firmwareWrite(void *cookie) valid = false; } if (resp) { - resp->success = valid; - osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); + if (resp->evtType == EVT_APP_TO_HOST) { +#ifdef LEGACY_HAL_ENABLED + resp->respLegacy.success = valid; + osEnqueueEvtOrFree(EVT_APP_TO_HOST, &resp->respLegacy, writeCookieFree); +#endif + } else { + resp->resp.ret.status = !valid; + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &resp->resp, writeCookieFree); + } } } @@ -575,10 +614,10 @@ static uint32_t doFirmwareChunk(uint8_t *data, uint32_t offset, uint32_t len, vo firmwareFinish(false); } else if (offset != mDownloadState->srcOffset) { reply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART; - resetDownloadState(false); + resetDownloadState(false, true); } else { if (!cookie) - mDownloadState->srcCrc = crc32(data, len, mDownloadState->srcCrc); + mDownloadState->srcCrc = soft_crc32(data, len, mDownloadState->srcCrc); mDownloadState->srcOffset += len; memcpy(mDownloadState->data, data, len); mDownloadState->lenLeft = mDownloadState->len = len; @@ -602,12 +641,24 @@ static uint32_t firmwareChunk(void *rx, uint8_t rx_len, void *tx, uint64_t times return sizeof(*resp); } -static uint32_t doFinishFirmwareUpload() +static uint32_t doFinishFirmwareUpload(uint32_t *addr, uint32_t *crc) { uint32_t reply; if (!mDownloadState) { reply = appSecErrToNanohubReply(mAppSecStatus); + if (addr) { + if (mApp) + *addr = (uint32_t)mApp; + else + *addr = 0xFFFFFFFF; + } + if (crc) { + if (mApp) + *crc = osAppSegmentGetCrc(mApp); + else + *crc = 0xFFFFFFFF; + } } else if (mDownloadState->srcOffset == mDownloadState->size) { reply = NANOHUB_FIRMWARE_UPLOAD_PROCESSING; } else { @@ -620,7 +671,7 @@ static uint32_t doFinishFirmwareUpload() static uint32_t finishFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp) { struct NanohubFinishFirmwareUploadResponse *resp = tx; - resp->uploadReply = doFinishFirmwareUpload(); + resp->uploadReply = doFinishFirmwareUpload(NULL, NULL); if (resp->uploadReply != NANOHUB_FIRMWARE_UPLOAD_PROCESSING) osLog(LOG_INFO, "%s: reply=%" PRIu8 "\n", __func__, resp->uploadReply); return sizeof(*resp); @@ -666,26 +717,12 @@ static uint32_t unmaskInterrupt(void *rx, uint8_t rx_len, void *tx, uint64_t tim return sizeof(*resp); } -static void nanohubDelayStartApps(void *cookie) -{ - uint32_t status = 0; - status = osExtAppStartAppsDelayed(); - osLog(LOG_DEBUG, "Started delayed apps; EXT status: %08" PRIX32 "\n", status); -} - static void addDelta(struct ApHubSync *sync, uint64_t apTime, uint64_t hubTime) { - static bool delayStart = false; - #if DEBUG_APHUB_TIME_SYNC syncDebugAdd(apTime, hubTime); #endif apHubSyncAddDelta(sync, apTime, hubTime); - - if (!delayStart) { - delayStart = true; - osDefer(nanohubDelayStartApps, NULL, false); - } } static int64_t getAvgDelta(struct ApHubSync *sync) @@ -951,7 +988,7 @@ static uint32_t writeEvent(void *rx, uint8_t rx_len, void *tx, uint64_t timestam if (rx_len >= sizeof(struct HostMsgHdrChre) && rx_len == sizeof(struct HostMsgHdrChre) + hostPacket->len && osTidById(&hostPacket->appId, &tid)) { - if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) { + if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) { struct NanohubMsgChreHdr hdr = { .size = hostPacket->len, .endpoint = hostPacket->endpoint, @@ -976,7 +1013,7 @@ static uint32_t writeEvent(void *rx, uint8_t rx_len, void *tx, uint64_t timestam } else if (rx_len >= sizeof(struct HostMsgHdrChreV10) && rx_len == sizeof(struct HostMsgHdrChreV10) + hostPacketV10->len && osTidById(&hostPacketV10->appId, &tid)) { - if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) { + if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) { struct NanohubMsgChreHdr hdr = { .size = hostPacketV10->len, .endpoint = CHRE_HOST_ENDPOINT_UNSPECIFIED, @@ -1044,7 +1081,7 @@ const static struct NanohubCommand mBuiltinCommands[] = { NANOHUB_COMMAND(NANOHUB_REASON_GET_INTERRUPT, getInterrupt, getInterrupt, - 0, + struct { }, struct NanohubGetInterruptRequest), NANOHUB_COMMAND(NANOHUB_REASON_MASK_INTERRUPT, maskInterrupt, @@ -1080,13 +1117,15 @@ const struct NanohubCommand *nanohubFindCommand(uint32_t packetReason) return NULL; } -static void halSendMgmtResponse(uint32_t cmd, uint32_t status) +#ifdef LEGACY_HAL_ENABLED + +static void halSendLegacyMgmtResponse(uint32_t cmd, uint32_t status) { - struct NanohubHalMgmtTx *resp; + struct NanohubHalLegacyMgmtTx *resp; resp = heapAlloc(sizeof(*resp)); if (resp) { - resp->hdr = (struct NanohubHalHdr) { + resp->hdr = (struct NanohubHalLegacyHdr) { .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg), .msg = cmd, @@ -1096,36 +1135,36 @@ static void halSendMgmtResponse(uint32_t cmd, uint32_t status) } } -static void halExtAppsOn(void *rx, uint8_t rx_len) +static void halLegacyExtAppsOn(void *rx, uint8_t rx_len) { - struct NanohubHalMgmtRx *req = rx; + struct NanohubHalLegacyMgmtRx *req = rx; - halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId)))); + halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId)))); } -static void halExtAppsOff(void *rx, uint8_t rx_len) +static void halLegacyExtAppsOff(void *rx, uint8_t rx_len) { - struct NanohubHalMgmtRx *req = rx; + struct NanohubHalLegacyMgmtRx *req = rx; - halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)))); + halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)))); } -static void halExtAppDelete(void *rx, uint8_t rx_len) +static void halLegacyExtAppDelete(void *rx, uint8_t rx_len) { - struct NanohubHalMgmtRx *req = rx; + struct NanohubHalLegacyMgmtRx *req = rx; - halSendMgmtResponse(NANOHUB_HAL_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId)))); + halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId)))); } -static void halQueryMemInfo(void *rx, uint8_t rx_len) +static void halLegacyQueryMemInfo(void *rx, uint8_t rx_len) { } -static void halQueryApps(void *rx, uint8_t rx_len) +static void halLegacyQueryApps(void *rx, uint8_t rx_len) { - struct NanohubHalQueryAppsRx *req = rx; - struct NanohubHalQueryAppsTx *resp; - struct NanohubHalHdr *hdr; + struct NanohubHalLegacyQueryAppsRx *req = rx; + struct NanohubHalLegacyQueryAppsTx *resp; + struct NanohubHalLegacyHdr *hdr; uint64_t appId; uint32_t appVer, appSize; @@ -1133,8 +1172,8 @@ static void halQueryApps(void *rx, uint8_t rx_len) resp = heapAlloc(sizeof(*resp)); if (resp) { resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); - resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1; - resp->hdr.msg = NANOHUB_HAL_QUERY_APPS; + resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1; + resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_APPS; resp->appId = appId; resp->version = appVer; resp->flashUse = appSize; @@ -1146,16 +1185,16 @@ static void halQueryApps(void *rx, uint8_t rx_len) if (hdr) { hdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); hdr->len = 1; - hdr->msg = NANOHUB_HAL_QUERY_APPS; + hdr->msg = NANOHUB_HAL_LEGACY_QUERY_APPS; osEnqueueEvtOrFree(EVT_APP_TO_HOST, hdr, heapFree); } } } -static void halQueryRsaKeys(void *rx, uint8_t rx_len) +static void halLegacyQueryRsaKeys(void *rx, uint8_t rx_len) { - struct NanohubHalQueryRsaKeysRx *req = rx; - struct NanohubHalQueryRsaKeysTx *resp; + struct NanohubHalLegacyQueryRsaKeysRx *req = rx; + struct NanohubHalLegacyQueryRsaKeysTx *resp; int len = 0; const uint32_t *ptr; uint32_t numKeys; @@ -1172,75 +1211,77 @@ static void halQueryRsaKeys(void *rx, uint8_t rx_len) } resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); - resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1 + len; - resp->hdr.msg = NANOHUB_HAL_QUERY_RSA_KEYS; + resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1 + len; + resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS; osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); } -static void halStartUpload(void *rx, uint8_t rx_len) +static void halLegacyStartUpload(void *rx, uint8_t rx_len) { - struct NanohubHalStartUploadRx *req = rx; + struct NanohubHalLegacyStartUploadRx *req = rx; struct NanohubStartFirmwareUploadRequest hwReq = { - .size= req->length + .size = req->length }; - struct NanohubHalStartUploadTx *resp; + struct NanohubHalLegacyStartUploadTx *resp; if (!(resp = heapAlloc(sizeof(*resp)))) return; resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); - resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1; - resp->hdr.msg = NANOHUB_HAL_START_UPLOAD; - resp->success = doStartFirmwareUpload(&hwReq); + resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1; + resp->hdr.msg = NANOHUB_HAL_LEGACY_START_UPLOAD; + resp->success = doStartFirmwareUpload(&hwReq, true); osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); } -static void halContUpload(void *rx, uint8_t rx_len) +static void halLegacyContUpload(void *rx, uint8_t rx_len) { uint32_t offset; uint32_t reply; uint8_t len; - struct NanohubHalContUploadRx *req = rx; - struct NanohubHalContUploadTx *resp; + struct NanohubHalLegacyContUploadRx *req = rx; + struct FirmwareWriteCookie *cookie; - if (!(resp = heapAlloc(sizeof(*resp)))) + if (!(cookie = heapAlloc(sizeof(*cookie)))) return; - resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); - resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1; - resp->hdr.msg = NANOHUB_HAL_CONT_UPLOAD; + cookie->evtType = EVT_APP_TO_HOST; + cookie->respLegacy.hdr = (struct NanohubHalLegacyHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(cookie->respLegacy) - sizeof(struct NanohubHalLegacyHdr) + 1, + .msg = NANOHUB_HAL_LEGACY_CONT_UPLOAD, + }; + cookie->respLegacy.success = false; if (!mDownloadState) { reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY; } else { offset = le32toh(req->offset); len = rx_len - sizeof(req->offset); - reply = doFirmwareChunk(req->data, offset, len, resp); + reply = doFirmwareChunk(req->data, offset, len, cookie); } if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) { osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply); - resp->success = false; - - osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); + osEnqueueEvtOrFree(EVT_APP_TO_HOST, &cookie->respLegacy, writeCookieFree); } } -static void halFinishUpload(void *rx, uint8_t rx_len) +static void halLegacyFinishUpload(void *rx, uint8_t rx_len) { - struct NanohubHalFinishUploadTx *resp; + struct NanohubHalLegacyFinishUploadTx *resp; uint32_t reply; if (!(resp = heapAlloc(sizeof(*resp)))) return; resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0); - resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1; - resp->hdr.msg = NANOHUB_HAL_FINISH_UPLOAD; + resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1; + resp->hdr.msg = NANOHUB_HAL_LEGACY_FINISH_UPLOAD; - reply = doFinishFirmwareUpload(); + reply = doFinishFirmwareUpload(NULL, NULL); osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply); @@ -1249,32 +1290,598 @@ static void halFinishUpload(void *rx, uint8_t rx_len) osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree); } -static void halReboot(void *rx, uint8_t rx_len) +static void halLegacyReboot(void *rx, uint8_t rx_len) { BL.blReboot(); } +const static struct NanohubHalLegacyCommand mBuiltinHalLegacyCommands[] = { + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_ON, + halLegacyExtAppsOn), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_OFF, + halLegacyExtAppsOff), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APP_DELETE, + halLegacyExtAppDelete), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_MEMINFO, + halLegacyQueryMemInfo), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_APPS, + halLegacyQueryApps), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS, + halLegacyQueryRsaKeys), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_START_UPLOAD, + halLegacyStartUpload), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_CONT_UPLOAD, + halLegacyContUpload), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_FINISH_UPLOAD, + halLegacyFinishUpload), + NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_REBOOT, + halLegacyReboot), +}; + +const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg) +{ + uint32_t i; + + for (i = 0; i < ARRAY_SIZE(mBuiltinHalLegacyCommands); i++) { + const struct NanohubHalLegacyCommand *cmd = &mBuiltinHalLegacyCommands[i]; + if (cmd->msg == msg) + return cmd; + } + return NULL; +} + +#endif /* LEGACY_HAL_ENABLED */ + +static void halSendAppMgmtResponse(struct NanohubHalAppMgmtRx *req, uint32_t status, struct MgmtStatus stat, uint32_t transactionId) +{ + struct NanohubHalAppMgmtTx *resp; + + resp = heapAlloc(sizeof(*resp)); + if (resp) { + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr), + .transactionId = transactionId, + }; + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_APP_MGMT, + .status = htole32(status), + }; + resp->cmd = req->cmd; + resp->stat = stat; + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + } +} + +static void halAppMgmt(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalAppMgmtRx *req = rx; + struct MgmtStatus stat; + uint32_t ret; + + switch (req->cmd) { + case NANOHUB_HAL_APP_MGMT_START: + stat.value= osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId))); + ret = stat.op > 0 ? 0 : -1; + break; + case NANOHUB_HAL_APP_MGMT_STOP: + stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))); + ret = stat.op > 0 ? 0 : -1; + break; + case NANOHUB_HAL_APP_MGMT_UNLOAD: + stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))); + ret = stat.op > 0 ? 0 : -1; + break; + case NANOHUB_HAL_APP_MGMT_DELETE: + stat.value = osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId))); + ret = stat.erase > 0 ? 0 : -1; + break; + default: + return; + } + + halSendAppMgmtResponse(req, ret, stat, transactionId); +} + +static void deferHalSysMgmtErase(void *cookie) +{ + struct NanohubHalSysMgmtTx *resp = cookie; + + bool success = osEraseShared(); + + if (success) + resp->ret.status = htole32(0); + else + resp->ret.status = htole32(-1); + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + +static void halSysMgmt(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalSysMgmtRx *req = rx; + struct NanohubHalSysMgmtTx *resp; + uint32_t ret = 0; + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr), + .transactionId = transactionId, + }; + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_SYS_MGMT, + }; + resp->cmd = req->cmd; + + switch (req->cmd) { + case NANOHUB_HAL_SYS_MGMT_ERASE: + ret = osExtAppStopAppsByAppId(APP_ID_ANY); + osLog(LOG_INFO, "%s: unloaded apps, ret=%08lx\n", __func__, ret); + // delay to make sure all apps are unloaded before erasing + if (osDefer(deferHalSysMgmtErase, resp, false) == false) { + resp->ret.status = htole32(-1); + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + } + break; + case NANOHUB_HAL_SYS_MGMT_REBOOT: + BL.blReboot(); + break; + default: + resp->ret.status = htole32(-1); + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + } +} + +static bool copyTLV64(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint64_t val) +{ + if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint64_t) > max_len) + return false; + buf[(*offset)++] = tag; + buf[(*offset)++] = sizeof(uint64_t); + memcpy(&buf[*offset], &val, sizeof(uint64_t)); + *offset += sizeof(uint64_t); + return true; +} + +static bool copyTLV32(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint32_t val) +{ + if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) > max_len) + return false; + buf[(*offset)++] = tag; + buf[(*offset)++] = sizeof(uint32_t); + memcpy(&buf[*offset], &val, sizeof(uint32_t)); + *offset += sizeof(uint32_t); + return true; +} + +static bool copyTLV8(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint8_t val) +{ + if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t) > max_len) + return false; + buf[(*offset)++] = tag; + buf[(*offset)++] = sizeof(uint8_t); + memcpy(&buf[*offset], &val, sizeof(uint8_t)); + *offset += sizeof(uint8_t); + return true; +} + +static bool copyTLVEmpty(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag) +{ + if (*offset + sizeof(uint8_t) + sizeof(uint8_t) > max_len) + return false; + buf[(*offset)++] = tag; + buf[(*offset)++] = 0; + return true; +} + +static int processAppTags(const struct AppHdr *app, uint32_t crc, uint32_t size, uint8_t *data, uint8_t *tags, int cnt, bool req_tid) +{ + int i; + size_t offset = 0; + const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet); + bool success = true; + uint32_t tid; + bool tid_valid = false; + struct Task *task; + + if (app->hdr.magic != APP_HDR_MAGIC || + app->hdr.fwVer != APP_HDR_VER_CUR || + (app->hdr.fwFlags & FL_APP_HDR_APPLICATION) == 0 || + app->hdr.payInfoType != LAYOUT_APP) { + return 0; + } + + if (osTidById(&app->hdr.appId, &tid)) { + tid_valid = true; + task = osTaskFindByTid(tid); + if (task) { + if (task->app != app) + tid_valid = false; + } else + tid_valid = false; + } + + if (!tid_valid && req_tid) + return 0; + + for (i=0; i<cnt && success; i++) { + switch(tags[i]) { + case NANOHUB_HAL_APP_INFO_APPID: + success = copyTLV64(data, &offset, max_len, tags[i], app->hdr.appId); + break; + case NANOHUB_HAL_APP_INFO_CRC: + if (size) + success = copyTLV32(data, &offset, max_len, tags[i], crc); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_TID: + if (tid_valid) + success = copyTLV32(data, &offset, max_len, tags[i], tid); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_VERSION: + success = copyTLV32(data, &offset, max_len, tags[i], app->hdr.appVer); + break; + case NANOHUB_HAL_APP_INFO_ADDR: + success = copyTLV32(data, &offset, max_len, tags[i], (uint32_t)app); + break; + case NANOHUB_HAL_APP_INFO_SIZE: + if (size) + success = copyTLV32(data, &offset, max_len, tags[i], size); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_HEAP: + if (tid_valid) + success = copyTLV32(data, &offset, max_len, tags[i], heapGetTaskSize(tid)); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_DATA: + success = copyTLV32(data, &offset, max_len, tags[i], app->sect.got_end - app->sect.data_start); + break; + case NANOHUB_HAL_APP_INFO_BSS: + success = copyTLV32(data, &offset, max_len, tags[i], app->sect.bss_end - app->sect.bss_start); + break; + case NANOHUB_HAL_APP_INFO_CHRE_MAJOR: + if (app->hdr.fwFlags & FL_APP_HDR_CHRE) + success = copyTLV8(data, &offset, max_len, tags[i], + (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x01 : + app->hdr.chreApiMajor); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_CHRE_MINOR: + if (app->hdr.fwFlags & FL_APP_HDR_CHRE) + success = copyTLV8(data, &offset, max_len, tags[i], + (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x00 : + app->hdr.chreApiMinor); + else + success = copyTLVEmpty(data, &offset, max_len, tags[i]); + break; + case NANOHUB_HAL_APP_INFO_END: + default: + success = false; + copyTLVEmpty(data, &offset, max_len, NANOHUB_HAL_APP_INFO_END); + break; + } + } + + return offset; +} + +static void halAppInfo(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalAppInfoRx *req = rx; + struct NanohubHalAppInfoTx *resp; + struct SegmentIterator it; + uint32_t state; + int ret, i; + uint32_t sharedSize, numApps; + const struct AppHdr *internal; + const uint8_t *shared; + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data), + .transactionId = transactionId, + }; + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_APP_INFO, + }; + + shared = platGetSharedAreaInfo(&sharedSize); + internal = platGetInternalAppList(&numApps); + + if ((le32toh(req->addr) >= (uint32_t)shared && le32toh(req->addr) < (uint32_t)shared + sharedSize) || + (le32toh(req->addr) < (uint32_t)shared && + ((uint32_t)shared < (uint32_t)internal || + (numApps > 0 && le32toh(req->addr) > (uint32_t)(internal+numApps-1))))) { + osSegmentIteratorInit(&it); + while (osSegmentIteratorNext(&it)) { + state = osSegmentGetState(it.seg); + switch (state) { + case SEG_ST_EMPTY: + case SEG_ST_RESERVED: + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + return; + case SEG_ST_ERASED: + case SEG_ST_VALID: + if (le32toh(req->addr) <= (uint32_t)osSegmentGetData(it.seg)) { + ret = processAppTags(osSegmentGetData(it.seg), osSegmentGetCrc(it.seg), osSegmentGetSize(it.seg), resp->data, req->tags, rx_len - 4, state == SEG_ST_ERASED); + if (ret > 0) { + resp->hdr.len += ret; + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + return; + } + } + break; + } + } + } else { + for (i = 0; i < numApps; i++, internal++) { + if (le32toh(req->addr) <= (uint32_t)internal) { + ret = processAppTags(internal, 0, 0, resp->data, req->tags, rx_len - 4, false); + if (ret > 0) { + resp->hdr.len += ret; + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); + return; + } + } + } + } + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + +static void halSysInfo(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + extern uint8_t __code_start[]; + extern uint8_t __code_end[]; + extern uint8_t __text_end[]; + extern uint8_t __ram_start[]; + extern uint8_t __ram_end[]; + + struct NanohubHalSysInfoRx *req = rx; + struct NanohubHalSysInfoTx *resp; + int i; + size_t offset = 0; + const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet); + bool success = true; + int free, chunks, largest; + uint32_t shared_size; + + free = heapGetFreeSize(&chunks, &largest); + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data), + .transactionId = transactionId, + }; + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_SYS_INFO, + }; + + for (i=0; i<rx_len && success; i++) { + switch(req->tags[i]) { + case NANOHUB_HAL_SYS_INFO_HEAP_FREE: + if (free >= 0) + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], free); + else + success = copyTLVEmpty(resp->data, &offset, max_len, req->tags[i]); + break; + case NANOHUB_HAL_SYS_INFO_RAM_SIZE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __ram_end - __ram_start); + break; + case NANOHUB_HAL_SYS_INFO_EEDATA_SIZE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetSize()); + break; + case NANOHUB_HAL_SYS_INFO_EEDATA_FREE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetFree()); + break; + case NANOHUB_HAL_SYS_INFO_CODE_SIZE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __code_start); + break; + case NANOHUB_HAL_SYS_INFO_CODE_FREE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __text_end); + break; + case NANOHUB_HAL_SYS_INFO_SHARED_SIZE: + platGetSharedAreaInfo(&shared_size); + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], shared_size); + break; + case NANOHUB_HAL_SYS_INFO_SHARED_FREE: + success = copyTLV32(resp->data, &offset, max_len, req->tags[i], osSegmentGetFree()); + break; + case NANOHUB_HAL_SYS_INFO_END: + default: + success = false; + copyTLVEmpty(resp->data, &offset, max_len, NANOHUB_HAL_APP_INFO_END); + break; + } + } + + resp->hdr.len += offset; + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + +static void halKeyInfo(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalKeyInfoRx *req = rx; + struct NanohubHalKeyInfoTx *resp; + const uint32_t *ptr; + uint32_t numKeys; + uint32_t dataLength; + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + ptr = BL.blGetPubKeysInfo(&numKeys); + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data), + .transactionId = transactionId, + }; + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_KEY_INFO, + }; + + resp->keyLength = 0; + + if (ptr && req->keyNum < numKeys) { + if (req->dataOffset < RSA_BYTES) { + resp->keyLength = RSA_BYTES; + if (RSA_BYTES - req->dataOffset > NANOHUB_RSA_KEY_CHUNK_LEN) + dataLength = NANOHUB_RSA_KEY_CHUNK_LEN; + else + dataLength = RSA_BYTES - req->dataOffset; + memcpy(resp->data, (const uint8_t *)ptr + (req->keyNum * RSA_BYTES) + req->dataOffset, dataLength); + resp->hdr.len += dataLength; + } + } + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + +static void halStartUpload(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalStartUploadRx *req = rx; + struct NanohubStartFirmwareUploadRequest hwReq = { + .size = req->length + }; + struct NanohubHalStartUploadTx *resp; + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr), + .transactionId = transactionId, + }; + + resp->ret.msg = NANOHUB_HAL_START_UPLOAD; + if (doStartFirmwareUpload(&hwReq, false)) + resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED; + else + resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE; + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + +static void halContUpload(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + uint32_t offset; + uint32_t reply; + uint8_t len; + struct NanohubHalContUploadRx *req = rx; + struct FirmwareWriteCookie *cookie; + + if (!(cookie = heapAlloc(sizeof(*cookie)))) + return; + + cookie->evtType = EVT_APP_TO_HOST_CHRE; + cookie->resp.hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(cookie->resp) - sizeof(cookie->resp.hdr), + .transactionId = transactionId, + }; + cookie->resp.ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_CONT_UPLOAD, + }; + + if (!mDownloadState) { + reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY; + } else { + offset = le32toh(req->offset); + len = rx_len - sizeof(req->offset); + reply = doFirmwareChunk(req->data, offset, len, cookie); + } + if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) { + osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply); + + cookie->resp.ret.status = reply; + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &cookie->resp, writeCookieFree); + } +} + +static void halFinishUpload(void *rx, uint8_t rx_len, uint32_t transactionId) +{ + struct NanohubHalFinishUploadTx *resp; + uint32_t reply; + uint32_t addr = 0xFFFFFFFF; + uint32_t crc = 0xFFFFFFFF; + + if (!(resp = heapAlloc(sizeof(*resp)))) + return; + + resp->hdr = (struct NanohubHalHdr) { + .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), + .len = sizeof(*resp) - sizeof(resp->hdr), + .transactionId = transactionId, + }; + + reply = doFinishFirmwareUpload(&addr, &crc); + + osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply); + + resp->ret = (struct NanohubHalRet) { + .msg = NANOHUB_HAL_FINISH_UPLOAD, + .status = reply, + }; + + resp->addr = addr; + resp->crc = crc; + + osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree); +} + const static struct NanohubHalCommand mBuiltinHalCommands[] = { - NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_ON, - halExtAppsOn), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_OFF, - halExtAppsOff), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APP_DELETE, - halExtAppDelete), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_MEMINFO, - halQueryMemInfo), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_APPS, - halQueryApps), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_RSA_KEYS, - halQueryRsaKeys), + NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_MGMT, + halAppMgmt, + struct NanohubHalAppMgmtRx, + struct NanohubHalAppMgmtRx), + NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_MGMT, + halSysMgmt, + struct NanohubHalSysMgmtRx, + struct NanohubHalSysMgmtRx), + NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_INFO, + halAppInfo, + __le32, + struct NanohubHalAppInfoRx), + NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_INFO, + halSysInfo, + struct { }, + struct NanohubHalSysInfoRx), + NANOHUB_HAL_COMMAND(NANOHUB_HAL_KEY_INFO, + halKeyInfo, + struct NanohubHalKeyInfoRx, + struct NanohubHalKeyInfoRx), NANOHUB_HAL_COMMAND(NANOHUB_HAL_START_UPLOAD, - halStartUpload), + halStartUpload, + struct NanohubHalStartUploadRx, + struct NanohubHalStartUploadRx), NANOHUB_HAL_COMMAND(NANOHUB_HAL_CONT_UPLOAD, - halContUpload), + halContUpload, + __le32, + struct NanohubHalContUploadRx), NANOHUB_HAL_COMMAND(NANOHUB_HAL_FINISH_UPLOAD, - halFinishUpload), - NANOHUB_HAL_COMMAND(NANOHUB_HAL_REBOOT, - halReboot), + halFinishUpload, + struct { }, + struct { }), }; const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg) @@ -1289,6 +1896,7 @@ const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg) return NULL; } + int64_t hostGetTimeDelta(void) { int64_t delta = getAvgDelta(&mTimeSync); diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c index 5ed419b6..e6fc9518 100644 --- a/firmware/os/core/nanohub_chre.c +++ b/firmware/os/core/nanohub_chre.c @@ -207,7 +207,7 @@ static bool osChreSendMessageToHost(void *message, uint32_t messageSize, chreMessageFreeFunction *freeCallback) { bool result = false; - struct HostHubRawPacket *hostMsg = NULL; + struct HostHubChrePacket *hostMsg = NULL; if (messageSize > CHRE_MESSAGE_TO_HOST_MAX_SIZE || (messageSize && !message)) goto out; @@ -220,8 +220,10 @@ static bool osChreSendMessageToHost(void *message, uint32_t messageSize, memcpy(hostMsg+1, message, messageSize); hostMsg->appId = osChreGetAppId(); - hostMsg->dataLen = messageSize; - result = osEnqueueEvtOrFree(EVT_APP_TO_HOST, hostMsg, heapFree); + hostMsg->messageSize = messageSize; + hostMsg->messageType = messageType; + hostMsg->hostEndpoint = hostEndpoint; + result = osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, hostMsg, heapFree); out: if (freeCallback) @@ -538,6 +540,16 @@ static void osChreEventCfgInfo(uintptr_t *retValP, va_list args) osEventsUnsubscribe(2, EVT_APP_STARTED, EVT_APP_STOPPED); } +static void osChreEventHostSleep(uintptr_t *retValP, va_list args) +{ + // bool enable = va_arg(args, int(); +} + +static void osChreEventIsHostAwake(uintptr_t *retValP, va_list args) +{ + *retValP = true; +} + static void osChreDrvGnssGetCap(uintptr_t *retValP, va_list args) { *retValP = CHRE_GNSS_CAPABILITIES_NONE; @@ -570,6 +582,12 @@ static void osChreDrvGnssMeasStopAsync(uintptr_t *retValP, va_list args) *retValP = false; } +static void osChreDrvGnssConfLocMon(uintptr_t *retValP, va_list args) +{ + // bool enable = va_args(args, bool); + *retValP = false; +} + static void osChreDrvWifiGetCap(uintptr_t *retValP, va_list args) { *retValP = CHRE_WIFI_CAPABILITIES_NONE; @@ -600,6 +618,33 @@ static void osChreDrvWwanGetCallInfoAsync(uintptr_t *retValP, va_list args) *retValP = false; } +static void osChreDrvAudioGetSrc(uintptr_t *retValP, va_list args) +{ + // uint32_t handle = va_args(args, uint32_t); + // struct chreAudioSource *audioSource = va_args(args, struct chreAudioSource *); + *retValP = false; +} + +static void osChreDrvAudioConfSrc(uintptr_t *retValP, va_list args) +{ + // uint32_t handle = va_args(args, uint32_t); + // bool enable = va_args(args, int); + // uint32_t duration_lo = va_arg(args, uint32_t); + // uint32_t duration_hi = va_arg(args, uint32_t); + // uint64_t bufferDuration = (((uint64_t)dur_hi) << 32) | dur_lo; + // uint32_t interval_lo = va_args(args, uint32_t); + // uint32_t interval_hi = va_args(args, uint32_t); + // uint64_t deliveryInterval = (((uint64_t)del_hi) << 32) | del_lo; + *retValP = false; +} + +static void osChreDrvAudioGetStatus(uintptr_t *retValP, va_list args) +{ + // uint32_t handle = va_args(args, uint32_t); + // struct chreAudioSourceStatus *status = va_args(args, struct chreAudioSourceStatus *); + *retValP = false; +} + static const struct SyscallTable chreMainApiTable = { .numEntries = SYSCALL_CHRE_MAIN_API_LAST, .entry = { @@ -618,7 +663,7 @@ static const struct SyscallTable chreMainApiTable = { [SYSCALL_CHRE_MAIN_API_SEND_MSG] = { .func = osChreApiSendMessageToHost }, [SYSCALL_CHRE_MAIN_API_SENSOR_FIND_DEFAULT] = { .func = osChreApiSensorFindDefault }, [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO_OLD] = { .func = osChreApiSensorGetInfoOld }, - [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO] = { .func = osChreApiSensorGetInfo }, + [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO] = { .func = osChreApiSensorGetInfo }, [SYSCALL_CHRE_MAIN_API_SENSOR_GET_STATUS] = { .func = osChreApiSensorGetStatus }, [SYSCALL_CHRE_MAIN_API_SENSOR_CONFIG] = { .func = osChreApiSensorConfig }, [SYSCALL_CHRE_MAIN_API_GET_OS_API_VERSION] = { .func = osChreApiChreApiVersion }, @@ -635,6 +680,8 @@ static const struct SyscallTable chreMainEventTable = { [SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID] = { .func = osChreEventInfoByAppId }, [SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID] = { .func = osChreEeventInfoByInstId }, [SYSCALL_CHRE_MAIN_EVENT_CFG_INFO] = { .func = osChreEventCfgInfo }, + [SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP] = { .func = osChreEventHostSleep }, + [SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE] = { .func = osChreEventIsHostAwake }, }, }; @@ -654,6 +701,7 @@ static const struct SyscallTable chreDrvGnssTable = { [SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC] = { .func = osChreDrvGnssLocStopAsync }, [SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC] = { .func = osChreDrvGnssMeasStartAsync }, [SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC] = { .func = osChreDrvGnssMeasStopAsync }, + [SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS] = { .func = osChreDrvGnssConfLocMon }, }, }; @@ -674,12 +722,22 @@ static const struct SyscallTable chreDrvWwanTable = { }, }; +static const struct SyscallTable chreDrvAudioTable = { + .numEntries = SYSCALL_CHRE_DRV_AUDIO_LAST, + .entry = { + [SYSCALL_CHRE_DRV_AUDIO_GET_SRC] = { .func = osChreDrvAudioGetSrc }, + [SYSCALL_CHRE_DRV_AUDIO_CONF_SRC] = { .func = osChreDrvAudioConfSrc }, + [SYSCALL_CHRE_DRV_AUDIO_GET_STATUS] = { .func = osChreDrvAudioGetStatus }, + }, +}; + static const struct SyscallTable chreDriversTable = { .numEntries = SYSCALL_CHRE_DRV_LAST, .entry = { [SYSCALL_CHRE_DRV_GNSS] = { .subtable = (struct SyscallTable*)&chreDrvGnssTable, }, [SYSCALL_CHRE_DRV_WIFI] = { .subtable = (struct SyscallTable*)&chreDrvWifiTable, }, [SYSCALL_CHRE_DRV_WWAN] = { .subtable = (struct SyscallTable*)&chreDrvWwanTable, }, + [SYSCALL_CHRE_DRV_AUDIO] = { .subtable = (struct SyscallTable*)&chreDrvAudioTable }, }, }; diff --git a/firmware/os/core/seos.c b/firmware/os/core/seos.c index ff8aa7d7..9756964f 100644 --- a/firmware/os/core/seos.c +++ b/firmware/os/core/seos.c @@ -300,12 +300,25 @@ static inline bool osTaskInit(struct Task *task) static void osTaskRelease(struct Task *task) { - uint32_t task_tid = task->tid; + uint32_t taskTid = task->tid; + uint32_t platErr, sensorErr; + int timErr, heapErr; + uint64_t appId; - platFreeResources(task_tid); // HW resources cleanup (IRQ, DMA etc) - sensorFreeAll(task_tid); - timTimerCancelAll(task_tid); - heapFreeAll(task_tid); + if (task->app) + appId = task->app->hdr.appId; + else + appId = 0; + + platErr = platFreeResources(taskTid); // HW resources cleanup (IRQ, DMA etc) + sensorErr = sensorFreeAll(taskTid); + timErr = timTimerCancelAll(taskTid); + heapErr = heapFreeAll(taskTid); + + if (platErr || sensorErr || timErr || heapErr) + osLog(LOG_WARN, "released app ID 0x%" PRIx64 "; plat:%08" PRIx32 " sensor:%08" PRIx32 " tim:%d heap:%d; TID %04" PRIX32 "\n", appId, platErr, sensorErr, timErr, heapErr, taskTid); + else + osLog(LOG_INFO, "released app ID 0x%" PRIx64 "; TID %04" PRIX32 "\n", appId, taskTid); } static inline void osTaskEnd(struct Task *task) @@ -500,6 +513,24 @@ struct Segment *osSegmentGetEnd() return (struct Segment *)(start + size); } +uint32_t osSegmentGetFree() +{ + struct SegmentIterator it; + const struct Segment *storageSeg = NULL; + + osSegmentIteratorInit(&it); + while (osSegmentIteratorNext(&it)) { + if (osSegmentGetState(it.seg) == SEG_ST_EMPTY) { + storageSeg = it.seg; + break; + } + } + if (!storageSeg || storageSeg > it.sharedEnd) + return 0; + + return (uint8_t *)it.sharedEnd - (uint8_t *)storageSeg; +} + struct Segment *osGetSegment(const struct AppHdr *app) { uint32_t size; @@ -601,13 +632,13 @@ bool osAppSegmentClose(struct AppHdr *app, uint32_t segDataSize, uint32_t segSta footerLen = (-fullSize) & 3; memset(footer, 0x00, footerLen); -#ifdef SEGMENT_CRC_SUPPORT - struct SegmentFooter segFooter { - .crc = ~crc32(storageSeg, fullSize, ~0), + wdtDisableClk(); + struct SegmentFooter segFooter = { + .crc = ~soft_crc32(storageSeg, fullSize, ~0), }; + wdtEnableClk(); memcpy(&footer[footerLen], &segFooter, sizeof(segFooter)); footerLen += sizeof(segFooter); -#endif if (ret && footerLen) ret = osWriteShared((uint8_t*)storageSeg + fullSize, footer, footerLen); @@ -660,10 +691,10 @@ static inline bool osAppIsValid(const struct AppHdr *app) static bool osExtAppIsValid(const struct AppHdr *app, uint32_t len) { - //TODO: when CRC support is ready, add CRC check here return osAppIsValid(app) && len >= sizeof(*app) && osAppSegmentGetState(app) == SEG_ST_VALID && + osAppSegmentCalcCrcResidue(app) == CRC_RESIDUE && !(app->hdr.fwFlags & FL_APP_HDR_INTERNAL); } @@ -721,7 +752,7 @@ static bool osStartApp(const struct AppHdr *app) // print external NanoApp info to facilitate NanoApp debugging if (!(task->app->hdr.fwFlags & FL_APP_HDR_INTERNAL)) osLog(LOG_INFO, - "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04X\n", + "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04" PRIX16 "\n", task->app->hdr.appId, (uintptr_t) task->app, (uintptr_t) task->platInfo.data, task->tid); done = osTaskInit(task); @@ -768,20 +799,20 @@ void osTaskAbort(struct Task *task) osStopTask(task, true); } -static bool matchDelayStart(const void *cookie, const struct AppHdr *app) +static bool matchAutoStart(const void *cookie, const struct AppHdr *app) { bool match = (bool)cookie; if (app->hdr.fwFlags & FL_APP_HDR_CHRE) { if (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) - return !match; + return match; else if ((app->hdr.chreApiMajor < 0x01) || (app->hdr.chreApiMajor == 0x01 && app->hdr.chreApiMinor < 0x01)) - return !match; - else return match; + else + return !match; } else { - return !match; + return match; } } @@ -951,11 +982,6 @@ uint32_t osExtAppStartAppsByAppId(uint64_t appId) return osExtAppStartApps(matchAppId, &appId); } -uint32_t osExtAppStartAppsDelayed() -{ - return osExtAppStartApps(matchDelayStart, (void *)true); -} - static void osStartTasks(void) { const struct AppHdr *app; @@ -1006,7 +1032,7 @@ static void osStartTasks(void) } osLog(LOG_DEBUG, "Starting external apps...\n"); - status = osExtAppStartApps(matchDelayStart, (void *)false); + status = osExtAppStartApps(matchAutoStart, (void *)true); osLog(LOG_DEBUG, "Started %" PRIu32 " internal apps; EXT status: %08" PRIX32 "\n", taskCnt, status); } @@ -1203,13 +1229,22 @@ static void osDeferredActionFreeF(void* event) static bool osEventsSubscribeUnsubscribeV(bool sub, uint32_t numEvts, va_list ap) { - union SeosInternalSlabData *act = slabAllocatorAlloc(mMiscInternalThingsSlab); + struct Task *task = osGetCurrentTask(); + union SeosInternalSlabData *act; int i; - if (!act || numEvts > MAX_EVT_SUB_CNT) + if (!sub && osTaskTestFlags(task, FL_TASK_STOPPED)) // stopping, so this is a no-op + return true; + + if (numEvts > MAX_EVT_SUB_CNT) + return false; + + act = slabAllocatorAlloc(mMiscInternalThingsSlab); + + if (!act) return false; - act->evtSub.tid = osGetCurrentTid(); + act->evtSub.tid = task->tid; act->evtSub.numEvts = numEvts; for (i = 0; i < numEvts; i++) act->evtSub.evts[i] = va_arg(ap, uint32_t); @@ -1272,12 +1307,8 @@ static bool osEnqueueEvtCommon(uint32_t evt, void *evtData, TaggedPtr evtFreeInf osTaskAddIoCount(task, 1); - if (osTaskTestFlags(task, FL_TASK_STOPPED)) { - handleEventFreeing(evtType, evtData, evtFreeInfo); - return true; - } - - if (!evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) { + if (osTaskTestFlags(task, FL_TASK_STOPPED) || + !evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) { osTaskAddIoCount(task, -1); return false; } @@ -1377,7 +1408,7 @@ bool osEnqueuePrivateEvtAsApp(uint32_t evtType, void *evtData, uint32_t toTid) return osEnqueuePrivateEvtEx(evtType & EVT_MASK, evtData, taggedPtrMakeFromUint(osGetCurrentTid()), toTid); } -bool osTidById(uint64_t *appId, uint32_t *tid) +bool osTidById(const uint64_t *appId, uint32_t *tid) { struct Task *task; diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c index 513a9c8c..3f272d6c 100644 --- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c +++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c @@ -16,6 +16,7 @@ #include <algos/time_sync.h> #include <atomic.h> +#include <common/math/macros.h> #include <cpu/cpuMath.h> #include <errno.h> #include <gpio.h> @@ -40,7 +41,7 @@ #include <variant/variant.h> #ifdef MAG_SLAVE_PRESENT -#include <calibration/magnetometer/mag_cal.h> +#include <calibration/magnetometer/mag_cal/mag_cal.h> #endif #ifdef ACCEL_CAL_ENABLED @@ -61,13 +62,8 @@ #ifdef GYRO_CAL_ENABLED #include <calibration/gyroscope/gyro_cal.h> -#include <common/math/macros.h> #endif // GYRO_CAL_ENABLED -#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED) -#include <calibration/util/cal_log.h> -#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED - #ifdef OVERTEMPCAL_ENABLED #include <calibration/over_temp/over_temp_cal.h> #endif // OVERTEMPCAL_ENABLED @@ -108,7 +104,7 @@ #define DBG_WM_CALC 0 #define TIMESTAMP_DBG 0 -#define BMI160_APP_VERSION 17 +#define BMI160_APP_VERSION 20 // fixme: to list required definitions for a slave mag #ifdef USE_BMM150 @@ -259,6 +255,18 @@ #define MAX_NUM_COMMS_EVENT_SAMPLES 15 +#ifndef BMI160_ACC_SAMPLES +#define BMI160_ACC_SAMPLES 3000 +#endif + +#ifndef BMI160_GYRO_SAMPLES +#define BMI160_GYRO_SAMPLES 20 +#endif + +#ifndef BMI160_MAG_SAMPLES +#define BMI160_MAG_SAMPLES 600 +#endif + // Default accel range is 8g #ifndef BMI160_ACC_RANGE_G #define BMI160_ACC_RANGE_G 8 @@ -749,18 +757,19 @@ static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] = { #ifdef ACCEL_CAL_ENABLED { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, - NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc, - SENS_TYPE_ACCEL_BIAS) }, + NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW, + 1.0/kScale_acc, SENS_TYPE_ACCEL_BIAS) }, #else { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, - NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) }, + NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW, + 1.0/kScale_acc) }, #endif { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, - NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) }, + NANOHUB_INT_NONWAKEUP, BMI160_GYRO_SAMPLES, SENS_TYPE_GYRO_BIAS) }, #ifdef MAG_SLAVE_PRESENT { DEC_INFO_RATE_RAW_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE, - NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_RAW, 1.0/kScale_mag, - SENS_TYPE_MAG_BIAS) }, + NANOHUB_INT_NONWAKEUP, BMI160_MAG_SAMPLES, SENS_TYPE_MAG_RAW, + 1.0/kScale_mag, SENS_TYPE_MAG_BIAS) }, #endif { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 100) }, @@ -1201,13 +1210,6 @@ static void configFifo(void) int i; uint8_t val = 0x12; bool any_fifo_enabled_prev = anyFifoEnabled(); -#ifdef ACCEL_CAL_ENABLED - struct BMI160Sensor *mSensorAcc; - bool accelCalNewBiasAvailable; - struct TripleAxisDataPoint *sample; - float accelCalBiasX, accelCalBiasY, accelCalBiasZ; - bool fallThrough; -#endif // if ACC is configed, enable ACC bit in fifo_config reg. if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) { @@ -1215,42 +1217,6 @@ static void configFifo(void) mTask.fifo_enabled[ACC] = true; } else { mTask.fifo_enabled[ACC] = false; -#ifdef ACCEL_CAL_ENABLED - // https://source.android.com/devices/sensors/sensor-types.html - // "The bias and scale calibration must only be updated while the sensor is deactivated, - // so as to avoid causing jumps in values during streaming." - accelCalNewBiasAvailable = accelCalUpdateBias(&mTask.acc, &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ); - - mSensorAcc = &mTask.sensors[ACC]; - // notify HAL about new accel bias calibration - if (accelCalNewBiasAvailable) { - fallThrough = true; - if (mSensorAcc->data_evt->samples[0].firstSample.numSamples > 0) { - // flush existing samples so the bias appears after them - flushData(mSensorAcc, - EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType)); - - // try to allocate another data event and break if unsuccessful - if (!allocateDataEvt(mSensorAcc, sensorGetTime())) { - fallThrough = false; - } - } - - if (fallThrough) { - mSensorAcc->data_evt->samples[0].firstSample.biasCurrent = true; - mSensorAcc->data_evt->samples[0].firstSample.biasPresent = 1; - mSensorAcc->data_evt->samples[0].firstSample.biasSample = - mSensorAcc->data_evt->samples[0].firstSample.numSamples; - sample = &mSensorAcc->data_evt->samples[mSensorAcc->data_evt->samples[0].firstSample.numSamples++]; - sample->x = accelCalBiasX; - sample->y = accelCalBiasY; - sample->z = accelCalBiasZ; - flushData(mSensorAcc, sensorGetMyEventType(mSensorInfo[ACC].biasType)); - - allocateDataEvt(mSensorAcc, sensorGetTime()); - } - } -#endif } // if GYR is configed, enable GYR bit in fifo_config reg. @@ -2092,7 +2058,12 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal x, y, z, mTask.tempCelsius); accelCalBiasRemove(&mTask.acc, &x, &y, &z); -#endif + +#ifdef ACCEL_CAL_DBG_ENABLED + // Prints debug data report. + accelCalDebPrint(&mTask.acc, mTask.tempCelsius); +#endif // ACCEL_CAL_DBG_ENABLED +#endif // ACCEL_CAL_ENABLED #ifdef GYRO_CAL_ENABLED // Gyro Cal -- Add accelerometer sample. @@ -2159,12 +2130,54 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal return; } +#ifdef ACCEL_CAL_ENABLED + // https://source.android.com/devices/sensors/sensor-types.html + // "The bias and scale calibration must only be updated while the sensor is deactivated, + // so as to avoid causing jumps in values during streaming." Note, this is now regulated + // by the SensorHAL. + if (mSensor->idx == ACC) { + float accel_offset[3] = {0.0f, 0.0f, 0.0f}; + bool accelCalNewBiasAvailable = accelCalUpdateBias( + &mTask.acc, &accel_offset[0], &accel_offset[1], &accel_offset[2]); + if (accelCalNewBiasAvailable) { + if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { + // Flushes existing samples so the bias appears after them. + flushData(mSensor, + EVENT_TYPE_BIT_DISCARDABLE | + sensorGetMyEventType(mSensorInfo[ACC].sensorType)); + + // Tries to allocate another data event and breaks if unsuccessful. + if (!allocateDataEvt(mSensor, rtc_time)) { + return; + } + } + mSensor->data_evt->samples[0].firstSample.biasCurrent = true; + mSensor->data_evt->samples[0].firstSample.biasPresent = 1; + mSensor->data_evt->samples[0].firstSample.biasSample = + mSensor->data_evt->samples[0].firstSample.numSamples; + sample = &mSensor->data_evt-> + samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; + + // Updates the accel offset in HAL. + sample->x = accel_offset[0]; + sample->y = accel_offset[1]; + sample->z = accel_offset[2]; + + flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType)); + if (!allocateDataEvt(mSensor, rtc_time)) { + return; + } + } + } +#endif // ACCEL_CAL_ENABLED + #ifdef MAG_SLAVE_PRESENT if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) { if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { // flush existing samples so the bias appears after them flushData(mSensor, - EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType)); + EVENT_TYPE_BIT_DISCARDABLE | + sensorGetMyEventType(mSensorInfo[MAG].sensorType)); if (!allocateDataEvt(mSensor, rtc_time)) { return; } @@ -2176,9 +2189,13 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal mSensor->data_evt->samples[0].firstSample.biasPresent = 1; mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples; - sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; + sample = &mSensor->data_evt-> + samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; + + // Updates the mag offset in HAL. magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z); - // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias + + // Bias is non-discardable, if we fail to enqueue, don't clear magBiasPosted. if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) { mTask.magBiasPosted = true; } @@ -2187,17 +2204,20 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal return; } } -#endif +#endif // MAG_SLAVE_PRESENT + #ifdef GYRO_CAL_ENABLED if (mSensor->idx == GYR) { // GyroCal -- Checks for a new offset estimate update. float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; float gyro_offset_temperature_celsius = 0.0f; + uint64_t calibration_time_nanos = 0; bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal); if (new_gyrocal_offset_update) { // GyroCal -- Gets the GyroCal offset estimate. gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1], - &gyro_offset[2], &gyro_offset_temperature_celsius); + &gyro_offset[2], &gyro_offset_temperature_celsius, + &calibration_time_nanos); #ifdef OVERTEMPCAL_ENABLED // OTC-Gyro Cal -- Sends a new GyroCal estimate to the OTC-Gyro. @@ -2243,17 +2263,6 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal sample->y = gyro_offset[1]; sample->z = gyro_offset[2]; -#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED) - CAL_DEBUG_LOG("[GYRO_OFFSET:STORED]", - "Offset|Temp|Time: %s%d.%06d, %s%d.%06d, %s%d.%06d | " - "%s%d.%06d | %llu", - CAL_ENCODE_FLOAT(sample->x, 6), - CAL_ENCODE_FLOAT(sample->y, 6), - CAL_ENCODE_FLOAT(sample->z, 6), - CAL_ENCODE_FLOAT(gyro_offset_temperature_celsius, 6), - (unsigned long long int)rtc_time); -#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED - flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType)); if (!allocateDataEvt(mSensor, rtc_time)) { return; @@ -2284,7 +2293,7 @@ static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScal //DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z)); - //TODO: This was added to prevent to much data of the same type accumulate in internal buffer. + //TODO: This was added to prevent too much data of the same type accumulate in internal buffer. // It might no longer be necessary and can be removed. if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { flushAllData(); @@ -3116,8 +3125,11 @@ static bool gyrCfgData(void *data, void *cookie) bias->hardwareBias[2] & 0xFF); #ifdef GYRO_CAL_ENABLED - gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1], - bias->softwareBias[2], sensorGetTime()); + const float dummy_temperature_celsius = 25.0f; + gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], + bias->softwareBias[1], bias->softwareBias[2], + dummy_temperature_celsius, + sensorGetTime()); #endif // GYRO_CAL_ENABLED if (!saveCalibration()) { T(pending_calibration_save) = true; @@ -3217,9 +3229,8 @@ static bool magCfgData(void *data, void *cookie) (int)(d->inclination * 180 / M_PI + 0.5f)); // Passing local field information to mag calibration routine -#ifdef DIVERSITY_CHECK_ENABLED diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength); -#endif + // TODO: pass local field information to rotation vector sensor. } else { ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size); @@ -3333,7 +3344,7 @@ static void processPendingEvt(void) } } if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) { - T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData(); + T(pending_step_cnt) = !stepCntFlushGetData() && T(pending_step_cnt); return; } if (mTask.pending_calibration_save) { @@ -3833,83 +3844,98 @@ static bool startTask(uint32_t task_id) osEventSubscribe(mTask.tid, EVT_APP_START); #ifdef ACCEL_CAL_ENABLED - // Init Accel Cal - accelCalInit(&mTask.acc, - 800000000, /* Stillness Time in ns (0.8s) */ - 5, /* Minimum Sample Number */ - 0.00025, /* Threshold */ - 15, /* nx bucket count */ - 15, /* nxb bucket count */ - 15, /* ny bucket count */ - 15, /* nyb bucket count */ - 15, /* nz bucket count */ - 15, /* nzb bucket count */ - 15); /* nle bucket count */ -#endif + // Initializes the accelerometer offset calibration algorithm. + const struct AccelCalParameters accel_cal_parameters = { + MSEC_TO_NANOS(800), // t0 + 5, // n_s + 15, // fx + 15, // fxb + 15, // fy + 15, // fyb + 15, // fz + 15, // fzb + 15, // fle + 0.00025f // th + }; + accelCalInit(&mTask.acc, &accel_cal_parameters); +#endif // ACCEL_CAL_ENABLED #ifdef GYRO_CAL_ENABLED - // Gyro Cal -- Initialization. - gyroCalInit(&mTask.gyro_cal, - SEC_TO_NANOS(5.0f), // Min stillness period = 5.0 seconds - SEC_TO_NANOS(5.9f), // Max stillness period = 6.0 seconds (NOTE 1) - 0, 0, 0, // Initial bias offset calibration - 0, // Time stamp of initial bias calibration - SEC_TO_NANOS(1.5f), // Analysis window length = 1.5 seconds - 7.5e-5f, // Gyroscope variance threshold [rad/sec]^2 - 1.5e-5f, // Gyroscope confidence delta [rad/sec]^2 - 4.5e-3f, // Accelerometer variance threshold [m/sec^2]^2 - 9.0e-4f, // Accelerometer confidence delta [m/sec^2]^2 - 5.0f, // Magnetometer variance threshold [uT]^2 - 1.0f, // Magnetometer confidence delta [uT]^2 - 0.95f, // Stillness threshold [0,1] - 40.0f * MDEG_TO_RAD, // Stillness mean variation limit [rad/sec] - 1.5f, // Max temperature delta during stillness [C] - true); // Gyro calibration enable - // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness - // period of 6.0 seconds and avoid buffer boundary conditions that could push - // the max stillness to the next multiple of the analysis window length - // (i.e., 7.5 seconds). + // Initializes the gyroscope offset calibration algorithm. + const struct GyroCalParameters gyro_cal_parameters = { + SEC_TO_NANOS(5), // min_still_duration_nanos + SEC_TO_NANOS(5.9f), // max_still_duration_nanos [see, NOTE 1] + 0, // calibration_time_nanos + SEC_TO_NANOS(1.5f), // window_time_duration_nanos + 0, // bias_x + 0, // bias_y + 0, // bias_z + 0.95f, // stillness_threshold + MDEG_TO_RAD * 40.0f, // stillness_mean_delta_limit [rad/sec] + 7.5e-5f, // gyro_var_threshold [rad/sec]^2 + 1.5e-5f, // gyro_confidence_delta [rad/sec]^2 + 4.5e-3f, // accel_var_threshold [m/sec^2]^2 + 9.0e-4f, // accel_confidence_delta [m/sec^2]^2 + 5.0f, // mag_var_threshold [uTesla]^2 + 1.0f, // mag_confidence_delta [uTesla]^2 + 1.5f, // temperature_delta_limit_celsius + true // gyro_calibration_enable + }; + // [NOTE 1]: 'max_still_duration_nanos' is set to 5.9 seconds to achieve a + // max stillness period of 6.0 seconds and avoid buffer boundary conditions + // that could push the max stillness to the next multiple of the analysis + // window length (i.e., 7.5 seconds). + gyroCalInit(&mTask.gyro_cal, &gyro_cal_parameters); #ifdef OVERTEMPCAL_ENABLED - // Initialize over-temp calibration. - overTempCalInit(&mTask.over_temp_gyro_cal, - 5, // Min num of points to enable model update - SEC_TO_NANOS(0.5f), // Min temperature update interval [nsec] - 0.75f, // Temperature span of bin method [C] - 40.0f * MDEG_TO_RAD, // Jump tolerance [rad/sec] - 50.0f * MDEG_TO_RAD, // Outlier rejection tolerance [rad/sec] - DAYS_TO_NANOS(2), // Model data point age limit [nsec] - 80.0f * MDEG_TO_RAD, // Limit for temp. sensitivity [rad/sec/C] - 3.0e3f * MDEG_TO_RAD, // Limit for model intercept parameter [rad/sec] - 0.1f * MDEG_TO_RAD, // Significant offset change [rad/sec] - true); // Over-temp compensation enable + // Initializes the gyroscope over-temperature offset compensation algorithm. + const struct OverTempCalParameters gyro_otc_parameters = { + MSEC_TO_NANOS(500), // min_temp_update_period_nanos + DAYS_TO_NANOS(2), // age_limit_nanos + 0.75f, // delta_temp_per_bin + 40.0f * MDEG_TO_RAD, // jump_tolerance + 50.0f * MDEG_TO_RAD, // outlier_limit + 80.0f * MDEG_TO_RAD, // temp_sensitivity_limit + 3.0e3f * MDEG_TO_RAD, // sensor_intercept_limit + 0.1f * MDEG_TO_RAD, // significant_offset_change + 5, // min_num_model_pts + true // over_temp_enable + }; + overTempCalInit(&mTask.over_temp_gyro_cal, &gyro_otc_parameters); + #endif // OVERTEMPCAL_ENABLED #endif // GYRO_CAL_ENABLED #ifdef MAG_SLAVE_PRESENT -#ifdef DIVERSITY_CHECK_ENABLED - initMagCal(&mTask.moc, - 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 - 3000000, // min_batch_window_in_micros - 8, // min_num_diverse_vectors - 1, // max_num_max_distance - 6.0f, // var_threshold - 10.0f, // max_min_threshold - 48.f, // local_field - 0.5f, // threshold_tuning_param - 2.552f); // max_distance_tuning_param -#else - initMagCal(&mTask.moc, - 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 - 3000000); // min_batch_window_in_micros -#endif -#endif + const struct MagCalParameters mag_cal_parameters = { + 3000000, // min_batch_window_in_micros + 0.0f, // x_bias + 0.0f, // y_bias + 0.0f, // z_bias + 1.0f, // c00 + 0.0f, // c01 + 0.0f, // c02 + 0.0f, // c10 + 1.0f, // c11 + 0.0f, // c12 + 0.0f, // c20 + 0.0f, // c21 + 1.0f // c22 + }; + + // Initializes the magnetometer offset calibration algorithm with diversity + // checker. + const struct DiversityCheckerParameters mag_diversity_parameters = { + 6.0f, // var_threshold + 10.0f, // max_min_threshold + 48.0f, // local_field + 0.5f, // threshold_tuning_param + 2.552f, // max_distance_tuning_param + 8, // min_num_diverse_vectors + 1 // max_num_max_distance + }; + initMagCal(&mTask.moc, &mag_cal_parameters, &mag_diversity_parameters); +#endif // MAG_SLAVE_PRESENT slabSize = sizeof(struct TripleAxisDataEvent) + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); diff --git a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c index 39b52063..49509732 100644 --- a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c +++ b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c @@ -32,7 +32,7 @@ #define BMP280_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 5) -#define BMP280_APP_VERSION 3 +#define BMP280_APP_VERSION 4 #ifndef BMP280_I2C_BUS_ID #define BMP280_I2C_BUS_ID 0 @@ -44,13 +44,15 @@ #define BOSCH_BMP280_ID 0x58 -#define BOSCH_BMP280_REG_RESET 0x60 +#define BOSCH_BMP280_REG_RESET 0xE0 #define BOSCH_BMP280_REG_DIG_T1 0x88 #define BOSCH_BMP280_REG_ID 0xd0 #define BOSCH_BMP280_REG_CTRL_MEAS 0xf4 #define BOSCH_BMP280_REG_CONFIG 0xf5 #define BOSCH_BMP280_REG_PRES_MSB 0xf7 +#define BOSCH_BMP280_SOFT_RESET_CMD 0xB6 + #define BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS 4 #define BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE 6 @@ -62,22 +64,28 @@ #define CTRL_ON ((2 << 5) | (5 << 2) | 3) // temp: 2x oversampling, baro: 16x oversampling, power: sleep #define CTRL_SLEEP ((2 << 5) | (5 << 2)) +// config: standby time: 62.5ms, IIR filter coefficient: 4 +#define CTRL_CFG ((1 << 5) | (2 << 2)) enum BMP280SensorEvents { EVT_SENSOR_I2C = EVT_APP_START + 1, EVT_SENSOR_BARO_TIMER, EVT_SENSOR_TEMP_TIMER, + EVT_SENSOR_SOFTRESET_TIMER, }; enum BMP280TaskState { STATE_RESET, + STATE_SOFTRESET, + STATE_SOFTRESET_MODE, STATE_VERIFY_ID, STATE_AWAITING_COMP_PARAMS, STATE_CONFIG, STATE_FINISH_INIT, STATE_IDLE, + STATE_ENABLING_BARO_TEMP, STATE_ENABLING_BARO, STATE_ENABLING_TEMP, STATE_DISABLING_BARO, @@ -114,11 +122,14 @@ static struct BMP280Task uint32_t tempHandle; uint32_t baroTimerHandle; uint32_t tempTimerHandle; + uint32_t resetHandle; float offset; struct I2cTransfer transfers[BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS]; + bool tmpbaroOn; + bool tmptempOn; bool baroOn; bool tempOn; bool baroReading; @@ -262,6 +273,16 @@ static void tempTimerCallback(uint32_t timerId, void *cookie) osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, cookie, NULL, mTask.id); } +static void softresetCallback(uint32_t timerId, void *cookie) +{ + osEnqueuePrivateEvt(EVT_SENSOR_SOFTRESET_TIMER, cookie, NULL, mTask.id); +} + +static void softreset() +{ + writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_SOFTRESET); +} + static void setMode(bool on, uint8_t state) { writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, (on) ? CTRL_ON : CTRL_SLEEP, state); @@ -298,12 +319,37 @@ static bool sensorPowerBaro(bool on, void *cookie) mTask.baroReading = false; } + if (!on && mTask.tmpbaroOn && mTask.resetHandle) + { + if (!mTask.tmptempOn) { + timTimerCancel(mTask.resetHandle); + mTask.resetHandle = 0; + } + mTask.tmpbaroOn = 0; + sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); + } + if (oldMode != newMode) - setMode(newMode, (on ? STATE_ENABLING_BARO : STATE_DISABLING_BARO)); + { + if (newMode == 0) + { + setMode(newMode, STATE_DISABLING_BARO); + mTask.baroOn = false; + } + else + { + mTask.tmpbaroOn = true; + if (!mTask.tmptempOn) { + // do soft reset first when newMode is on + softreset(); + } + } + } else + { sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); - - mTask.baroOn = on; + mTask.baroOn = on; + } return true; } @@ -362,12 +408,38 @@ static bool sensorPowerTemp(bool on, void *cookie) mTask.tempReading = false; } + if (!on && mTask.tmptempOn && mTask.resetHandle) + { + if(!mTask.tmpbaroOn) { + timTimerCancel(mTask.resetHandle); + mTask.resetHandle = 0; + } + mTask.tmptempOn = 0; + sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); + } + if (oldMode != newMode) - setMode(newMode, (on ? STATE_ENABLING_TEMP : STATE_DISABLING_TEMP)); + { + if (newMode == 0) + { + setMode(newMode, STATE_DISABLING_TEMP); + mTask.tempOn = false; + } + else + { + mTask.tmptempOn = true; + if (!mTask.tmpbaroOn) { + // do soft reset first when newMode is on + softreset(); + } + } + } else + { sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); + mTask.tempOn = on; + } - mTask.tempOn = on; return true; } @@ -495,6 +567,31 @@ static void handleI2cEvent(struct I2cTransfer *xfer) break; } + case STATE_SOFTRESET: { + //create timer for 2ms delay + mTask.resetHandle = timTimerSet(2000000ull, 0, 50, softresetCallback, NULL, true); + break; + } + + case STATE_SOFTRESET_MODE: { + if (mTask.tmpbaroOn && mTask.tmptempOn) { + setMode(true,STATE_ENABLING_BARO_TEMP); + mTask.tmpbaroOn = false; + mTask.baroOn = true; + mTask.tmptempOn = false; + mTask.tempOn = true; + } else if (mTask.tmpbaroOn) { + setMode(true,STATE_ENABLING_BARO); + mTask.tmpbaroOn = false; + mTask.baroOn = true; + } else if (mTask.tmptempOn) { + setMode(true,STATE_ENABLING_TEMP); + mTask.tmptempOn = false; + mTask.tempOn = true; + } + break; + } + case STATE_VERIFY_ID: { /* Check the sensor ID */ if (xfer->err != 0 || xfer->txrxBuf[0] != BOSCH_BMP280_ID) { @@ -521,7 +618,13 @@ static void handleI2cEvent(struct I2cTransfer *xfer) case STATE_CONFIG: { // standby time: 62.5ms, IIR filter coefficient: 4 - writeRegister(BOSCH_BMP280_REG_CONFIG, (1 << 5) | (2 << 2), STATE_FINISH_INIT); + writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_FINISH_INIT); + break; + } + + case STATE_ENABLING_BARO_TEMP: { + sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); + sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0); break; } @@ -606,7 +709,7 @@ static void handleEvent(uint32_t evtType, const void* evtData) i2cMasterRequest(I2C_BUS_ID, I2C_SPEED); /* Reset chip */ - writeRegister(BOSCH_BMP280_REG_RESET, 0xB6, STATE_RESET); + writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_RESET); break; } @@ -649,6 +752,12 @@ static void handleEvent(uint32_t evtType, const void* evtData) mTask.tempReading = true; break; } + + case EVT_SENSOR_SOFTRESET_TIMER: + { + writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_SOFTRESET_MODE); + break; + } } } diff --git a/firmware/os/inc/chreApi.h b/firmware/os/inc/chreApi.h index e355ed5a..2c157010 100644 --- a/firmware/os/inc/chreApi.h +++ b/firmware/os/inc/chreApi.h @@ -74,13 +74,16 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t)); #define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID 2 // (uint64_t, struct chreNanoappInfo *) -> bool #define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID 3 // (uint32_t, struct chreNanoappInfo *) -> bool #define SYSCALL_CHRE_MAIN_EVENT_CFG_INFO 4 // (bool) -> void -#define SYSCALL_CHRE_MAIN_EVENT_LAST 5 // always last. holes are allowed, but not immediately before this +#define SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP 5 // (bool) -> void +#define SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE 6 // (void) -> bool +#define SYSCALL_CHRE_MAIN_EVENT_LAST 7 // always last. holes are allowed, but not immediately before this //level 2 indices in the CHRE.drivers table #define SYSCALL_CHRE_DRV_GNSS 0 #define SYSCALL_CHRE_DRV_WIFI 1 #define SYSCALL_CHRE_DRV_WWAN 2 -#define SYSCALL_CHRE_DRV_LAST 3 // always last. holes are allowed, but not immediately before this +#define SYSCALL_CHRE_DRV_AUDIO 3 +#define SYSCALL_CHRE_DRV_LAST 4 // always last. holes are allowed, but not immediately before this //level 3 indices in the CHRE.drivers.gnss table #define SYSCALL_CHRE_DRV_GNSS_GET_CAP 0 // (void) -> uint32_t @@ -88,7 +91,8 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t)); #define SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC 2 // (const void *) -> bool #define SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC 3 // (uint32_t, const void *) -> bool #define SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC 4 // (const void *) -> bool -#define SYSCALL_CHRE_DRV_GNSS_LAST 5 // always last. holes are allowed, but not immediately before this +#define SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS 5 // (bool) -> bool +#define SYSCALL_CHRE_DRV_GNSS_LAST 6 // always last. holes are allowed, but not immediately before this //level 3 indices in the CHRE.drivers.wifi table #define SYSCALL_CHRE_DRV_WIFI_GET_CAP 0 // (void) -> uint32_t @@ -98,9 +102,15 @@ C_STATIC_ASSERT(uintptr_size, sizeof(uintptr_t) >= sizeof(uint32_t)); //level 3 indices in the CHRE.drivers.wwan table #define SYSCALL_CHRE_DRV_WWAN_GET_CAP 0 // (void) -> uint32_t -#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC 1 // (const void *cookie) -> bool +#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC 1 // (const void *) -> bool #define SYSCALL_CHRE_DRV_WWAN_LAST 2 // always last. holes are allowed, but not immediately before this +//level 3 indicies in the CHRE.drivers.audio table +#define SYSCALL_CHRE_DRV_AUDIO_GET_SRC 0 // (uint32_t, struct chreAudioSource *) -> bool +#define SYSCALL_CHRE_DRV_AUDIO_CONF_SRC 1 // (uint32_t, bool, uint64_t, uint64_t) -> bool +#define SYSCALL_CHRE_DRV_AUDIO_GET_STATUS 2 // (uint32_t, struct chreAudioSourceStatus *) -> bool +#define SYSCALL_CHRE_DRV_AUDIO_LAST 3 // always last. holes are allowed, but not immediately before this + //called by os entry point to export the api void osChreApiExport(void); // release CHRE event and optionally call completion callback diff --git a/firmware/os/inc/eeData.h b/firmware/os/inc/eeData.h index 9b313eed..0a6eca6a 100644 --- a/firmware/os/inc/eeData.h +++ b/firmware/os/inc/eeData.h @@ -45,6 +45,9 @@ bool eeDataGet(uint32_t name, void *buf, uint32_t *szP); bool eeDataSet(uint32_t name, const void *buf, uint32_t len); +uint32_t eeDataGetSize(); +uint32_t eeDataGetFree(); + //allow getting old "versions". Set state to NULL initially, call till you get NULL as return value void *eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP); bool eeDataEraseOldVersion(uint32_t name, void *addr); // addr is non-NULL address returned by call to eeDataGetAllVersions diff --git a/firmware/os/inc/eventnums.h b/firmware/os/inc/eventnums.h index 5746433e..cda24f31 100644 --- a/firmware/os/inc/eventnums.h +++ b/firmware/os/inc/eventnums.h @@ -32,6 +32,7 @@ #define EVT_APP_TO_SENSOR_HAL_DATA 0x00000404 //sensor driver out of band data update to sensor hal #define EVT_APP_STARTED 0x00000405 //sent when a app has successfully started #define EVT_APP_STOPPED 0x00000406 //sent when a app has stopped +#define EVT_APP_TO_HOST_CHRE 0x00000407 //app data to host. Type is struct HostHubChrePacket #define EVT_DEBUG_LOG 0x00007F01 //send message payload to Linux kernel log #define EVT_MASK 0x0000FFFF @@ -55,6 +56,18 @@ struct HostHubRawPacket { }ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF +#define HOST_HUB_CHRE_PACKET_MAX_LEN 128 + +SET_PACKED_STRUCT_MODE_ON +struct HostHubChrePacket { + uint64_t appId; + uint8_t messageSize; //not incl this header, 128 bytes max + uint32_t messageType; + uint16_t hostEndpoint; + //raw data in unspecified format here +}ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + SET_PACKED_STRUCT_MODE_ON struct NanohubMsgChreHdrV10 { uint8_t size; diff --git a/firmware/os/inc/heap.h b/firmware/os/inc/heap.h index c5bea591..cb1ccb2b 100644 --- a/firmware/os/inc/heap.h +++ b/firmware/os/inc/heap.h @@ -24,19 +24,16 @@ extern "C" { #include <stdint.h> #include <stdbool.h> - - - bool heapInit(void); void* heapAlloc(uint32_t sz); void heapFree(void* ptr); int heapFreeAll(uint32_t tid); - +int heapGetFreeSize(int *numChunks, int *largestChunk); +int heapGetTaskSize(uint32_t tid); #ifdef __cplusplus } #endif - #endif diff --git a/firmware/os/inc/nanohubCommand.h b/firmware/os/inc/nanohubCommand.h index 408cffdc..e1009804 100644 --- a/firmware/os/inc/nanohubCommand.h +++ b/firmware/os/inc/nanohubCommand.h @@ -34,11 +34,20 @@ void nanohubInitCommand(void); void nanohubPrefetchTx(uint32_t interrupt, uint32_t wakeup, uint32_t nonwakeup); const struct NanohubCommand *nanohubFindCommand(uint32_t packetReason); -struct NanohubHalCommand { +struct NanohubHalLegacyCommand { uint8_t msg; void (*handler)(void *, uint8_t); }; +const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg); + +struct NanohubHalCommand { + uint8_t msg; + void (*handler)(void *, uint8_t, uint32_t); + uint8_t minDataLen; + uint8_t maxDataLen; +}; + const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg); uint64_t hostGetTime(void); int64_t hostGetTimeDelta(void); diff --git a/firmware/os/inc/nanohubPacket.h b/firmware/os/inc/nanohubPacket.h index ad962ea7..df4af664 100644 --- a/firmware/os/inc/nanohubPacket.h +++ b/firmware/os/inc/nanohubPacket.h @@ -166,6 +166,7 @@ enum NanohubFirmwareChunkReply { NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART, NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL, NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY, + NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE, }; SET_PACKED_STRUCT_MODE_ON @@ -278,18 +279,6 @@ struct NanohubWriteEventResponse { } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -SET_PACKED_STRUCT_MODE_ON -struct NanohubHalHdr { - uint64_t appId; - uint8_t len; - uint8_t msg; -} ATTRIBUTE_PACKED; -SET_PACKED_STRUCT_MODE_OFF - -#define NANOHUB_HAL_EXT_APPS_ON 0 -#define NANOHUB_HAL_EXT_APPS_OFF 1 -#define NANOHUB_HAL_EXT_APP_DELETE 2 - // this behaves more stable w.r.t. endianness than bit field // this is setting byte fields in MgmtStatus response // the high-order bit, if set, is indication of counter overflow @@ -310,32 +299,46 @@ struct MgmtStatus { } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF +#ifdef LEGACY_HAL_ENABLED + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalLegacyHdr { + uint64_t appId; + uint8_t len; + uint8_t msg; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_LEGACY_EXT_APPS_ON 0 +#define NANOHUB_HAL_LEGACY_EXT_APPS_OFF 1 +#define NANOHUB_HAL_LEGACY_EXT_APP_DELETE 2 + SET_PACKED_STRUCT_MODE_ON -struct NanohubHalMgmtRx { +struct NanohubHalLegacyMgmtRx { __le64 appId; struct MgmtStatus stat; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF SET_PACKED_STRUCT_MODE_ON -struct NanohubHalMgmtTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyMgmtTx { + struct NanohubHalLegacyHdr hdr; __le32 status; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_QUERY_MEMINFO 3 -#define NANOHUB_HAL_QUERY_APPS 4 +#define NANOHUB_HAL_LEGACY_QUERY_MEMINFO 3 +#define NANOHUB_HAL_LEGACY_QUERY_APPS 4 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalQueryAppsRx { +struct NanohubHalLegacyQueryAppsRx { __le32 idx; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF SET_PACKED_STRUCT_MODE_ON -struct NanohubHalQueryAppsTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyQueryAppsTx { + struct NanohubHalLegacyHdr hdr; __le64 appId; __le32 version; __le32 flashUse; @@ -343,69 +346,245 @@ struct NanohubHalQueryAppsTx { } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_QUERY_RSA_KEYS 5 +#define NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS 5 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalQueryRsaKeysRx { +struct NanohubHalLegacyQueryRsaKeysRx { __le32 offset; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF SET_PACKED_STRUCT_MODE_ON -struct NanohubHalQueryRsaKeysTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyQueryRsaKeysTx { + struct NanohubHalLegacyHdr hdr; uint8_t data[]; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_START_UPLOAD 6 +#define NANOHUB_HAL_LEGACY_START_UPLOAD 6 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalStartUploadRx { +struct NanohubHalLegacyStartUploadRx { uint8_t isOs; __le32 length; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF SET_PACKED_STRUCT_MODE_ON -struct NanohubHalStartUploadTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyStartUploadTx { + struct NanohubHalLegacyHdr hdr; uint8_t success; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_CONT_UPLOAD 7 +#define NANOHUB_HAL_LEGACY_CONT_UPLOAD 7 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalContUploadRx { +struct NanohubHalLegacyContUploadRx { __le32 offset; uint8_t data[]; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF SET_PACKED_STRUCT_MODE_ON -struct NanohubHalContUploadTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyContUploadTx { + struct NanohubHalLegacyHdr hdr; uint8_t success; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_FINISH_UPLOAD 8 +#define NANOHUB_HAL_LEGACY_FINISH_UPLOAD 8 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalFinishUploadTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyFinishUploadTx { + struct NanohubHalLegacyHdr hdr; uint8_t success; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF -#define NANOHUB_HAL_REBOOT 9 +#define NANOHUB_HAL_LEGACY_REBOOT 9 SET_PACKED_STRUCT_MODE_ON -struct NanohubHalRebootTx { - struct NanohubHalHdr hdr; +struct NanohubHalLegacyRebootTx { + struct NanohubHalLegacyHdr hdr; __le32 reason; } ATTRIBUTE_PACKED; SET_PACKED_STRUCT_MODE_OFF +#endif /* LEGACY_HAL_ENABLED */ + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalHdr { + __le64 appId; + uint8_t len; + __le32 transactionId; + __le16 unused; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalRet { + uint8_t msg; + __le32 status; +} ATTRIBUTE_PACKED; + +#define NANOHUB_HAL_APP_MGMT 0x10 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalAppMgmtRx { + __le64 appId; + uint8_t cmd; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_APP_MGMT_START 0 +#define NANOHUB_HAL_APP_MGMT_STOP 1 +#define NANOHUB_HAL_APP_MGMT_UNLOAD 2 +#define NANOHUB_HAL_APP_MGMT_DELETE 3 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalAppMgmtTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + uint8_t cmd; + struct MgmtStatus stat; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_SYS_MGMT 0x11 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalSysMgmtRx { + uint8_t cmd; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_SYS_MGMT_ERASE 0 +#define NANOHUB_HAL_SYS_MGMT_REBOOT 1 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalSysMgmtTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + uint8_t cmd; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_APP_INFO 0x12 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalAppInfoRx { + __le32 addr; + uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(__le32)]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_APP_INFO_APPID 0x00 +#define NANOHUB_HAL_APP_INFO_CRC 0x01 +#define NANOHUB_HAL_APP_INFO_TID 0x02 +#define NANOHUB_HAL_APP_INFO_VERSION 0x03 +#define NANOHUB_HAL_APP_INFO_ADDR 0x04 +#define NANOHUB_HAL_APP_INFO_SIZE 0x05 +#define NANOHUB_HAL_APP_INFO_HEAP 0x06 +#define NANOHUB_HAL_APP_INFO_DATA 0x07 +#define NANOHUB_HAL_APP_INFO_BSS 0x08 +#define NANOHUB_HAL_APP_INFO_CHRE_MAJOR 0x09 +#define NANOHUB_HAL_APP_INFO_CHRE_MINOR 0x0A +#define NANOHUB_HAL_APP_INFO_END 0xFF + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalAppInfoTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_SYS_INFO 0x13 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalSysInfoRx { + uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_SYS_INFO_HEAP_FREE 0x0F +#define NANOHUB_HAL_SYS_INFO_RAM_SIZE 0x12 +#define NANOHUB_HAL_SYS_INFO_EEDATA_SIZE 0x13 +#define NANOHUB_HAL_SYS_INFO_EEDATA_FREE 0x14 +#define NANOHUB_HAL_SYS_INFO_CODE_SIZE 0x15 +#define NANOHUB_HAL_SYS_INFO_CODE_FREE 0x16 +#define NANOHUB_HAL_SYS_INFO_SHARED_SIZE 0x17 +#define NANOHUB_HAL_SYS_INFO_SHARED_FREE 0x18 +#define NANOHUB_HAL_SYS_INFO_END 0xFF + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalSysInfoTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_KEY_INFO 0x14 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalKeyInfoRx { + uint32_t keyNum; + uint32_t dataOffset; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalKeyInfoTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + uint32_t keyLength; + uint8_t data[NANOHUB_RSA_KEY_CHUNK_LEN]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_START_UPLOAD 0x16 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalStartUploadRx { + uint8_t isOs; + __le32 length; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalStartUploadTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_CONT_UPLOAD 0x17 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalContUploadRx { + __le32 offset; + uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN-sizeof(__le32)]; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_ON + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalContUploadTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + +#define NANOHUB_HAL_FINISH_UPLOAD 0x18 + +SET_PACKED_STRUCT_MODE_ON +struct NanohubHalFinishUploadTx { + struct NanohubHalHdr hdr; + struct NanohubHalRet ret; + __le32 addr; + __le32 crc; +} ATTRIBUTE_PACKED; +SET_PACKED_STRUCT_MODE_OFF + #endif /* __NANOHUBPACKET_H */ diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h index c66d8976..c8a3d481 100644 --- a/firmware/os/inc/seos.h +++ b/firmware/os/inc/seos.h @@ -22,6 +22,7 @@ extern "C" { #endif #include <plat/taggedPtr.h> +#include <plat/wdt.h> #include <stdbool.h> #include <stdint.h> #include <stdarg.h> @@ -30,12 +31,11 @@ extern "C" { #include <plat/app.h> #include <eventnums.h> #include <variant/variant.h> +#include <crc.h> #include "toolchain.h" #include <nanohub/nanohub.h> -//#define SEGMENT_CRC_SUPPORT - #ifndef MAX_TASKS /* Default to 16 tasks, override may come from variant.h */ #define MAX_TASKS 16 @@ -177,7 +177,7 @@ void osRemovePendingEvents(bool (*match)(uint32_t evtType, const void *evtData, bool osDefer(OsDeferCbkF callback, void *cookie, bool urgent); -bool osTidById(uint64_t *appId, uint32_t *tid); +bool osTidById(const uint64_t *appId, uint32_t *tid); bool osAppInfoById(uint64_t appId, uint32_t *appIdx, uint32_t *appVer, uint32_t *appSize); bool osAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize); bool osExtAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize); @@ -189,8 +189,9 @@ bool osAppSegmentClose(struct AppHdr *app, uint32_t segSize, uint32_t segState); bool osAppSegmentSetState(const struct AppHdr *app, uint32_t segState); bool osSegmentSetSize(struct Segment *seg, uint32_t size); bool osAppWipeData(struct AppHdr *app); -struct Segment *osGetSegment(const struct AppHdr *app); struct Segment *osSegmentGetEnd(); +uint32_t osSegmentGetFree(); +struct Segment *osGetSegment(const struct AppHdr *app); static inline int32_t osSegmentGetSize(const struct Segment *seg) { @@ -207,17 +208,19 @@ static inline struct AppHdr *osSegmentGetData(const struct Segment *seg) return (struct AppHdr*)(&seg[1]); } -#ifdef SEGMENT_CRC_SUPPORT - struct SegmentFooter { uint32_t crc; }; #define FOOTER_SIZE sizeof(struct SegmentFooter) -#else -#define FOOTER_SIZE 0 -#endif + +static inline uint32_t osSegmentGetCrc(const struct Segment *seg) +{ + struct SegmentFooter *footer = (struct SegmentFooter *)(((uint8_t*)seg) + + ((osSegmentGetSize(seg) + 3) & ~3) + sizeof(*seg)); + return footer ? footer->crc : 0xFFFFFFFF; +} static inline uint32_t osSegmentSizeAlignedWithFooter(uint32_t size) { @@ -243,6 +246,24 @@ static inline uint32_t osAppSegmentGetState(const struct AppHdr *app) return osSegmentGetState(osGetSegment(app)); } +static inline uint32_t osAppSegmentGetCrc(const struct AppHdr *app) +{ + return osSegmentGetCrc(osGetSegment(app)); +} + +static inline uint32_t osAppSegmentCalcCrcResidue(const struct AppHdr *app) +{ + struct Segment *seg = osGetSegment(app); + uint32_t size = osSegmentSizeAlignedWithFooter(osSegmentGetSize(seg)); + uint32_t crc; + + wdtDisableClk(); + crc = soft_crc32((uint8_t*)seg, size + sizeof(*seg), ~0); + wdtEnableClk(); + + return crc; +} + struct SegmentIterator { const struct Segment *shared; const struct Segment *sharedEnd; @@ -272,7 +293,6 @@ void osFreeRetainedEvent(uint32_t evtType, void *evtData, TaggedPtr *evtFreeingI uint32_t osExtAppStopAppsByAppId(uint64_t appId); uint32_t osExtAppEraseAppsByAppId(uint64_t appId); uint32_t osExtAppStartAppsByAppId(uint64_t appId); -uint32_t osExtAppStartAppsDelayed(); bool osAppIsChre(uint16_t tid); uint32_t osAppChreVersion(uint16_t tid); @@ -293,15 +313,33 @@ void osLog(enum LogLevel level, const char *str, ...) PRINTF_ATTRIBUTE(2, 3); #define INTERNAL_APP_INIT(_id, _ver, _init, _end, _event) \ SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr \ SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \ - .hdr.magic = APP_HDR_MAGIC, \ - .hdr.fwVer = APP_HDR_VER_CUR, \ - .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION, \ - .hdr.appId = (_id), \ - .hdr.appVer = (_ver), \ + .hdr.magic = APP_HDR_MAGIC, \ + .hdr.fwVer = APP_HDR_VER_CUR, \ + .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION, \ + .hdr.appId = (_id), \ + .hdr.appVer = (_ver), \ .hdr.payInfoType = LAYOUT_APP, \ - .vec.init = (uint32_t)(_init), \ - .vec.end = (uint32_t)(_end), \ - .vec.handle = (uint32_t)(_event) \ + .vec.init = (uint32_t)(_init), \ + .vec.end = (uint32_t)(_end), \ + .vec.handle = (uint32_t)(_event) \ +} +#endif + +#ifndef INTERNAL_CHRE_APP_INIT +#define INTERNAL_CHRE_APP_INIT(_id, _ver, _init, _end, _event) \ +SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr \ +SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \ + .hdr.magic = APP_HDR_MAGIC, \ + .hdr.fwVer = APP_HDR_VER_CUR, \ + .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION | FL_APP_HDR_CHRE, \ + .hdr.chreApiMajor = 0x01, \ + .hdr.chreApiMinor = 0x02, \ + .hdr.appId = (_id), \ + .hdr.appVer = (_ver), \ + .hdr.payInfoType = LAYOUT_APP, \ + .vec.init = (uint32_t)(_init), \ + .vec.end = (uint32_t)(_end), \ + .vec.handle = (uint32_t)(_event) \ } #endif diff --git a/firmware/os/platform/stm32/eeData.c b/firmware/os/platform/stm32/eeData.c index 20e859ef..2710ba07 100644 --- a/firmware/os/platform/stm32/eeData.c +++ b/firmware/os/platform/stm32/eeData.c @@ -90,6 +90,34 @@ static void *eeDataGetEx(uint32_t name, uint32_t *offsetP, bool first, void *buf return (uint32_t*)data - 1; } +uint32_t eeDataGetSize() +{ + return __eedata_end - __eedata_start; +} + +uint32_t eeDataGetFree() +{ + uint32_t *p = __eedata_start; + + //find the last incarnation of "name" in flash area + while (p < __eedata_end) { + uint32_t info = *p; + uint32_t name = info & EE_DATA_NAME_MAX; + uint32_t sz = info / (EE_DATA_NAME_MAX + 1); + + //check for ending condition (name == max) + if (name == EE_DATA_NAME_MAX) + break; + + p++; + + //skip over to next data chunk header + p += (sz + 3) / 4; + } + + return __eedata_end - p; +} + bool eeDataGet(uint32_t name, void *buf, uint32_t *szP) { uint32_t offset = 0; |