766 lines
29 KiB
C
766 lines
29 KiB
C
|
/*
|
||
|
|
* 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/accelerometer/accel_cal.h"
|
||
|
|
|
||
|
|
#include <inttypes.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#if defined(ACCEL_CAL_DBG_ENABLED) || defined(IMU_TEMP_DBG_ENABLED)
|
||
|
|
#include "calibration/util/cal_log.h"
|
||
|
|
#endif // ACCEL_CAL_DBG_ENABLED || IMU_TEMP_DBG_ENABLED
|
||
|
|
|
||
|
|
// 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.
|
||
|
|
#define PHI 0.707f // = 1/sqrt(2) gives a 45 degree angle for sorting data.
|
||
|
|
#define PHIb -0.707f
|
||
|
|
#define PHIZ 0.866f // smaller Z sphere cap, opening angle is 30 degrees.
|
||
|
|
#define PHIZb -0.866f
|
||
|
|
#define G_NORM_MAX \
|
||
|
|
1.38f // Norm during stillness should be 1 g, checking from max min values.
|
||
|
|
#define G_NORM_MIN 0.68f
|
||
|
|
#define MAX_OFF 0.1f // Will not accept offsets that are larger than 100 mg.
|
||
|
|
#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 \
|
||
|
|
((MAX_TEMP - MIN_TEMP) / \
|
||
|
|
ACCEL_CAL_NUM_TEMP_WINDOWS) // Separation window size for temperature buckets
|
||
|
|
// in degrees C.
|
||
|
|
#define EIGEN_RATIO 0.35f // EIGEN_RATIO (must be greater than 0.35).
|
||
|
|
#define EIGEN_MAG 0.97f // Eigen value magnitude (must be greater than 0.97).
|
||
|
|
#define ACCEL_NEW_BIAS_THRESHOLD (0.0f) // Bias update detection threshold.
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
#define TEMP_HIST_LOW \
|
||
|
|
16 // Putting all Temp counts in first bucket for temp < 16 degree C.
|
||
|
|
#define TEMP_HIST_HIGH \
|
||
|
|
62 // Putting all Temp counts in last bucket for temp > 62 degree C.
|
||
|
|
#define HIST_COUNT 9
|
||
|
|
#endif
|
||
|
|
#ifdef IMU_TEMP_DBG_ENABLED
|
||
|
|
#define IMU_TEMP_DELTA_TIME_NANOS \
|
||
|
|
5000000000 // Printing every 5 seconds IMU temp.
|
||
|
|
#endif
|
||
|
|
// clang-format on
|
||
|
|
|
||
|
|
/////////// Start Debug //////////////////////
|
||
|
|
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
// Total bucket Counter.
|
||
|
|
static void accelStatsCounter(struct AccelStillDet *asd,
|
||
|
|
struct AccelStatsMem *adf) {
|
||
|
|
// Sorting the data in the different buckets
|
||
|
|
// x bucket ntx.
|
||
|
|
if (PHI < asd->mean_x) {
|
||
|
|
adf->ntx += 1;
|
||
|
|
}
|
||
|
|
// Negative x bucket ntxb.
|
||
|
|
if (PHIb > asd->mean_x) {
|
||
|
|
adf->ntxb += 1;
|
||
|
|
}
|
||
|
|
// Y bucket nty.
|
||
|
|
if (PHI < asd->mean_y) {
|
||
|
|
adf->nty += 1;
|
||
|
|
}
|
||
|
|
// Negative y bucket ntyb.
|
||
|
|
if (PHIb > asd->mean_y) {
|
||
|
|
adf->ntyb += 1;
|
||
|
|
}
|
||
|
|
// Z bucket ntz.
|
||
|
|
if (PHIZ < asd->mean_z) {
|
||
|
|
adf->ntz += 1;
|
||
|
|
}
|
||
|
|
// Negative z bucket ntzb.
|
||
|
|
if (PHIZb > asd->mean_z) {
|
||
|
|
adf->ntzb += 1;
|
||
|
|
}
|
||
|
|
// The leftover bucket ntle.
|
||
|
|
if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
|
||
|
|
PHIb < asd->mean_y && PHIZ > asd->mean_z && PHIZb < asd->mean_z) {
|
||
|
|
adf->ntle += 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Temp histogram generation.
|
||
|
|
static void accelTempHisto(struct AccelStatsMem *adf, float temp) {
|
||
|
|
int index = 0;
|
||
|
|
|
||
|
|
// Take temp at every stillness detection.
|
||
|
|
adf->start_time_nanos = 0;
|
||
|
|
if (temp <= TEMP_HIST_LOW) {
|
||
|
|
adf->t_hist[0] += 1;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if (temp >= TEMP_HIST_HIGH) {
|
||
|
|
adf->t_hist[TEMP_HISTOGRAM - 1] += 1;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
index = (int)(((temp - TEMP_HIST_LOW) / 2) + 1);
|
||
|
|
adf->t_hist[index] += 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|
||
|
|
///////// End Debug ////////////////////
|
||
|
|
|
||
|
|
// Stillness detector reset.
|
||
|
|
static void asdReset(struct AccelStillDet *asd) {
|
||
|
|
asd->nsamples = 0;
|
||
|
|
asd->start_time = 0;
|
||
|
|
asd->acc_x = asd->acc_y = asd->acc_z = 0.0f;
|
||
|
|
asd->acc_xx = asd->acc_yy = asd->acc_zz = 0.0f;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Stillness detector init.
|
||
|
|
static void accelStillInit(struct AccelStillDet *asd, uint32_t t0, uint32_t n_s,
|
||
|
|
float th) {
|
||
|
|
memset(asd, 0, sizeof(struct AccelStillDet));
|
||
|
|
asd->var_th = th;
|
||
|
|
asd->min_batch_window = t0;
|
||
|
|
asd->max_batch_window = t0 + 100000000;
|
||
|
|
asd->min_batch_size = n_s;
|
||
|
|
asd->n_still = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Good data reset.
|
||
|
|
static void agdReset(struct AccelGoodData *agd) {
|
||
|
|
agd->nx = agd->nxb = 0;
|
||
|
|
agd->ny = agd->nyb = 0;
|
||
|
|
agd->nz = agd->nzb = 0;
|
||
|
|
agd->nle = 0;
|
||
|
|
agd->acc_t = agd->acc_tt = 0;
|
||
|
|
agd->e_x = agd->e_y = agd->e_z = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Good data init.
|
||
|
|
static void accelGoodDataInit(struct AccelGoodData *agd, uint32_t fx,
|
||
|
|
uint32_t fxb, uint32_t fy, uint32_t fyb,
|
||
|
|
uint32_t fz, uint32_t fzb, uint32_t fle) {
|
||
|
|
memset(agd, 0, sizeof(struct AccelGoodData));
|
||
|
|
agd->nfx = fx;
|
||
|
|
agd->nfxb = fxb;
|
||
|
|
agd->nfy = fy;
|
||
|
|
agd->nfyb = fyb;
|
||
|
|
agd->nfz = fz;
|
||
|
|
agd->nfzb = fzb;
|
||
|
|
agd->nfle = fle;
|
||
|
|
agd->var_t = 0;
|
||
|
|
agd->mean_t = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Accel cal algo init (ready for temp buckets).
|
||
|
|
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);
|
||
|
|
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,
|
||
|
|
const struct AccelCalParameters *parameters) {
|
||
|
|
int i;
|
||
|
|
|
||
|
|
for (i = 0; i < ACCEL_CAL_NUM_TEMP_WINDOWS; ++i) {
|
||
|
|
// Init core accel data.
|
||
|
|
accelCalAlgoInit(&acc->ac1[i], parameters->fx, parameters->fxb,
|
||
|
|
parameters->fy, parameters->fyb, parameters->fz,
|
||
|
|
parameters->fzb, parameters->fle);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Stillness Reset.
|
||
|
|
accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
|
||
|
|
|
||
|
|
// Debug data init.
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
memset(&acc->adf, 0, sizeof(struct AccelStatsMem));
|
||
|
|
#endif
|
||
|
|
|
||
|
|
acc->x_bias = acc->y_bias = acc->z_bias = 0;
|
||
|
|
acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
|
||
|
|
acc->average_temperature_celsius = 0;
|
||
|
|
|
||
|
|
#ifdef IMU_TEMP_DBG_ENABLED
|
||
|
|
acc->temp_time_nanos = 0;
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
// Stillness time check.
|
||
|
|
static int stillnessBatchComplete(struct AccelStillDet *asd,
|
||
|
|
uint64_t sample_time_nanos) {
|
||
|
|
int complete = 0;
|
||
|
|
|
||
|
|
// Checking if enough data is accumulated to calc Mean and Var.
|
||
|
|
if ((sample_time_nanos - asd->start_time > asd->min_batch_window) &&
|
||
|
|
(asd->nsamples > asd->min_batch_size)) {
|
||
|
|
if (sample_time_nanos - asd->start_time < asd->max_batch_window) {
|
||
|
|
complete = 1;
|
||
|
|
} else {
|
||
|
|
// Checking for too long batch window, if yes reset and start over.
|
||
|
|
asdReset(asd);
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
} else if (sample_time_nanos - asd->start_time > asd->min_batch_window &&
|
||
|
|
(asd->nsamples < asd->min_batch_size)) {
|
||
|
|
// Not enough samples collected in max_batch_window during sample window.
|
||
|
|
asdReset(asd);
|
||
|
|
}
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Releasing Memory.
|
||
|
|
void accelCalDestroy(struct AccelCal *acc) { (void)acc; }
|
||
|
|
|
||
|
|
// Stillness Detection.
|
||
|
|
static int accelStillnessDetection(struct AccelStillDet *asd,
|
||
|
|
uint64_t sample_time_nanos, float x, float y,
|
||
|
|
float z) {
|
||
|
|
float inv = 0.0f;
|
||
|
|
int complete = 0.0f;
|
||
|
|
float g_norm = 0.0f;
|
||
|
|
|
||
|
|
// Accumulate for mean and VAR.
|
||
|
|
asd->acc_x += x;
|
||
|
|
asd->acc_xx += x * x;
|
||
|
|
asd->acc_y += y;
|
||
|
|
asd->acc_yy += y * y;
|
||
|
|
asd->acc_z += z;
|
||
|
|
asd->acc_zz += z * z;
|
||
|
|
|
||
|
|
// Setting a new start time and wait until T0 is reached.
|
||
|
|
if (++asd->nsamples == 1) {
|
||
|
|
asd->start_time = sample_time_nanos;
|
||
|
|
}
|
||
|
|
if (stillnessBatchComplete(asd, sample_time_nanos)) {
|
||
|
|
// Getting 1/#samples and checking asd->nsamples != 0.
|
||
|
|
if (0 < asd->nsamples) {
|
||
|
|
inv = 1.0f / asd->nsamples;
|
||
|
|
} else {
|
||
|
|
// Something went wrong resetting and start over.
|
||
|
|
asdReset(asd);
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
// Calculating the VAR = sum(x^2)/n - sum(x)^2/n^2.
|
||
|
|
asd->var_x = (asd->acc_xx - (asd->acc_x * asd->acc_x) * inv) * inv;
|
||
|
|
asd->var_y = (asd->acc_yy - (asd->acc_y * asd->acc_y) * inv) * inv;
|
||
|
|
asd->var_z = (asd->acc_zz - (asd->acc_z * asd->acc_z) * inv) * inv;
|
||
|
|
// Checking if sensor is still.
|
||
|
|
if (asd->var_x < asd->var_th && asd->var_y < asd->var_th &&
|
||
|
|
asd->var_z < asd->var_th) {
|
||
|
|
// Calcluating the MEAN = sum(x) / n.
|
||
|
|
asd->mean_x = asd->acc_x * inv;
|
||
|
|
asd->mean_y = asd->acc_y * inv;
|
||
|
|
asd->mean_z = asd->acc_z * inv;
|
||
|
|
// Calculating g_norm^2.
|
||
|
|
g_norm = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
|
||
|
|
asd->mean_z * asd->mean_z;
|
||
|
|
// Magnitude check, still passsing when we have worse case offset.
|
||
|
|
if (g_norm < G_NORM_MAX && g_norm > G_NORM_MIN) {
|
||
|
|
complete = 1;
|
||
|
|
asd->n_still += 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
asdReset(asd);
|
||
|
|
}
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Good data detection, sorting and accumulate the data for Kasa.
|
||
|
|
static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
|
||
|
|
float temp) {
|
||
|
|
int complete = 0;
|
||
|
|
float inv = 0.0f;
|
||
|
|
|
||
|
|
// Sorting the data in the different buckets and accum
|
||
|
|
// x bucket nx.
|
||
|
|
if (PHI < asd->mean_x && ac1->agd.nx < ac1->agd.nfx) {
|
||
|
|
ac1->agd.nx += 1;
|
||
|
|
ac1->agd.acc_t += temp;
|
||
|
|
ac1->agd.acc_tt += temp * temp;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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;
|
||
|
|
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 &&
|
||
|
|
PHIb < asd->mean_y && PHIZ > asd->mean_z && PHIZb < asd->mean_z &&
|
||
|
|
ac1->agd.nle < ac1->agd.nfle) {
|
||
|
|
ac1->agd.nle += 1;
|
||
|
|
ac1->agd.acc_t += temp;
|
||
|
|
ac1->agd.acc_tt += temp * temp;
|
||
|
|
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 &&
|
||
|
|
ac1->agd.ny == ac1->agd.nfy && ac1->agd.nyb == ac1->agd.nfyb &&
|
||
|
|
ac1->agd.nz == ac1->agd.nfz && ac1->agd.nzb == ac1->agd.nfzb) {
|
||
|
|
// Check if akf->nsamples is zero.
|
||
|
|
if (ac1->akf.nsamples == 0) {
|
||
|
|
agdReset(&ac1->agd);
|
||
|
|
kasaReset(&ac1->akf);
|
||
|
|
complete = 0;
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Normalize the data to the sample numbers.
|
||
|
|
kasaNormalize(&ac1->akf);
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
complete = 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
// If any of the buckets has a bigger number as specified, reset and start
|
||
|
|
// over.
|
||
|
|
if (ac1->agd.nx > ac1->agd.nfx || ac1->agd.nxb > ac1->agd.nfxb ||
|
||
|
|
ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
|
||
|
|
ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
|
||
|
|
agdReset(&ac1->agd);
|
||
|
|
kasaReset(&ac1->akf);
|
||
|
|
complete = 0;
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
return complete;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Eigen value magnitude and ratio test.
|
||
|
|
static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) {
|
||
|
|
// covariance matrix.
|
||
|
|
struct Mat33 S;
|
||
|
|
S.elem[0][0] = akf->acc_xx - akf->acc_x * akf->acc_x;
|
||
|
|
S.elem[0][1] = S.elem[1][0] = akf->acc_xy - akf->acc_x * akf->acc_y;
|
||
|
|
S.elem[0][2] = S.elem[2][0] = akf->acc_xz - akf->acc_x * akf->acc_z;
|
||
|
|
S.elem[1][1] = akf->acc_yy - akf->acc_y * akf->acc_y;
|
||
|
|
S.elem[1][2] = S.elem[2][1] = akf->acc_yz - akf->acc_y * akf->acc_z;
|
||
|
|
S.elem[2][2] = akf->acc_zz - akf->acc_z * akf->acc_z;
|
||
|
|
|
||
|
|
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;
|
||
|
|
|
||
|
|
// Passing when evmin/evmax> EIGEN_RATIO.
|
||
|
|
int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG);
|
||
|
|
|
||
|
|
agd->e_x = eigenvals.x;
|
||
|
|
agd->e_y = eigenvals.y;
|
||
|
|
agd->e_z = eigenvals.z;
|
||
|
|
|
||
|
|
return eigen_pass;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Updating the new bias and save to pointers. Return true if the bias changed.
|
||
|
|
bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z) {
|
||
|
|
*x = acc->x_bias_new;
|
||
|
|
*y = acc->y_bias_new;
|
||
|
|
*z = acc->z_bias_new;
|
||
|
|
|
||
|
|
// Check to see if the bias changed since last call to accelCalUpdateBias.
|
||
|
|
// Compiler does not allow us to use "==" and "!=" when comparing floats, so
|
||
|
|
// just use "<" and ">".
|
||
|
|
if ((acc->x_bias < acc->x_bias_new) || (acc->x_bias > acc->x_bias_new) ||
|
||
|
|
(acc->y_bias < acc->y_bias_new) || (acc->y_bias > acc->y_bias_new) ||
|
||
|
|
(acc->z_bias < acc->z_bias_new) || (acc->z_bias > acc->z_bias_new)) {
|
||
|
|
acc->x_bias = acc->x_bias_new;
|
||
|
|
acc->y_bias = acc->y_bias_new;
|
||
|
|
acc->z_bias = acc->z_bias_new;
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Set the (initial) bias.
|
||
|
|
void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z) {
|
||
|
|
acc->x_bias = acc->x_bias_new = x;
|
||
|
|
acc->y_bias = acc->y_bias_new = y;
|
||
|
|
acc->z_bias = acc->z_bias_new = z;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Removing the bias.
|
||
|
|
void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z) {
|
||
|
|
*x = *x - acc->x_bias;
|
||
|
|
*y = *y - acc->y_bias;
|
||
|
|
*z = *z - acc->z_bias;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Accel Cal Runner.
|
||
|
|
void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
|
||
|
|
float y, float z, float temp) {
|
||
|
|
// Scaling to 1g, better for the algorithm.
|
||
|
|
x *= KSCALE;
|
||
|
|
y *= KSCALE;
|
||
|
|
z *= KSCALE;
|
||
|
|
|
||
|
|
// DBG: IMU temp messages every 5s.
|
||
|
|
#ifdef IMU_TEMP_DBG_ENABLED
|
||
|
|
if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
|
||
|
|
CAL_DEBUG_LOG("IMU Temp Data: ",
|
||
|
|
", " 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;
|
||
|
|
|
||
|
|
// Temp GATE.
|
||
|
|
if (temp < MAX_TEMP && temp > MIN_TEMP) {
|
||
|
|
// Checking if accel is still.
|
||
|
|
if (accelStillnessDetection(&acc->asd, sample_time_nanos, x, y, z)) {
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
// Creating temp hist data.
|
||
|
|
accelTempHisto(&acc->adf, temp);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
temp_gate = (int) ((temp - MIN_TEMP) / TEMP_CUT);
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
accelStatsCounter(&acc->asd, &acc->adf);
|
||
|
|
#endif
|
||
|
|
// If still -> pass the averaged accel data (mean) to the
|
||
|
|
// sorting, counting and accum function.
|
||
|
|
if (accelGoodData(&acc->asd, &acc->ac1[temp_gate], temp)) {
|
||
|
|
// Running the Kasa fit.
|
||
|
|
struct Vec3 bias;
|
||
|
|
float radius;
|
||
|
|
|
||
|
|
// Grabbing the fit from the MAG cal.
|
||
|
|
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 &&
|
||
|
|
fabsf(bias.z) < MAX_OFF) {
|
||
|
|
// Eigen Ratio Test.
|
||
|
|
if (accEigenTest(&acc->ac1[temp_gate].akf,
|
||
|
|
&acc->ac1[temp_gate].agd)) {
|
||
|
|
// Storing the new offsets and average temperature.
|
||
|
|
acc->x_bias_new = bias.x * KSCALE2;
|
||
|
|
acc->y_bias_new = bias.y * KSCALE2;
|
||
|
|
acc->z_bias_new = bias.z * KSCALE2;
|
||
|
|
acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t;
|
||
|
|
}
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
//// Debug ///////
|
||
|
|
acc->adf.noff += 1;
|
||
|
|
// Resetting the counter for the offset history.
|
||
|
|
if (acc->adf.n_o > HIST_COUNT) {
|
||
|
|
acc->adf.n_o = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Storing the Debug data.
|
||
|
|
acc->adf.x_o[acc->adf.n_o] = bias.x;
|
||
|
|
acc->adf.y_o[acc->adf.n_o] = bias.y;
|
||
|
|
acc->adf.z_o[acc->adf.n_o] = bias.z;
|
||
|
|
acc->adf.e_x[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_x;
|
||
|
|
acc->adf.e_y[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_y;
|
||
|
|
acc->adf.e_z[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_z;
|
||
|
|
acc->adf.var_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.var_t;
|
||
|
|
acc->adf.mean_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.mean_t;
|
||
|
|
acc->adf.cal_time[acc->adf.n_o] = sample_time_nanos;
|
||
|
|
acc->adf.rad[acc->adf.n_o] = radius;
|
||
|
|
acc->adf.n_o += 1;
|
||
|
|
#endif
|
||
|
|
} else {
|
||
|
|
#ifdef ACCEL_CAL_DBG_ENABLED
|
||
|
|
acc->adf.noff_max += 1;
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
///////////////
|
||
|
|
|
||
|
|
// Resetting the structs for a new accel cal run.
|
||
|
|
agdReset(&acc->ac1[temp_gate].agd);
|
||
|
|
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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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("[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
|