1 /*
2  * Copyright (C) 2016 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include "calibration/accelerometer/accel_cal.h"
18 
19 #include <errno.h>
20 #include <inttypes.h>
21 #include <math.h>
22 #include <stdio.h>
23 #include <string.h>
24 
25 #if defined(ACCEL_CAL_DBG_ENABLED) || defined(IMU_TEMP_DBG_ENABLED)
26 #include "calibration/util/cal_log.h"
27 #endif  // ACCEL_CAL_DBG_ENABLED || IMU_TEMP_DBG_ENABLED
28 
29 // clang-format off
30 #define KSCALE \
31   0.101936799f         // Scaling from m/s^2 to g (0.101 = 1/(9.81 m/s^2)).
32 #define KSCALE2 9.81f  // Scaling from g to m/s^2.
33 #define PHI 0.707f     // = 1/sqrt(2) gives a 45 degree angle for sorting data.
34 #define PHIb -0.707f
35 #define PHIZ 0.866f    // smaller Z sphere cap, opening angle is 30 degrees.
36 #define PHIZb -0.866f
37 #define G_NORM_MAX \
38   1.38f  // Norm during stillness should be 1 g, checking from max min values.
39 #define G_NORM_MIN 0.68f
40 #define MAX_OFF 0.1f    // Will not accept offsets that are larger than 100 mg.
41 #define MIN_TEMP 20.0f  // No Data is collected below 20 degree C.
42 #define MAX_TEMP 45.0f  // No Data is collected above 45 degree C.
43 #define TEMP_CUT 30     // Separation point for temperature buckets 30 degree C.
44 #define EIGEN_RATIO 0.35f  // EIGEN_RATIO (must be greater than 0.35).
45 #define EIGEN_MAG 0.97f    // Eigen value magnitude (must be greater than 0.97).
46 #define ACCEL_NEW_BIAS_THRESHOLD (0.0f)  // Bias update detection threshold.
47 #ifdef ACCEL_CAL_DBG_ENABLED
48 #define TEMP_HIST_LOW \
49   16  // Putting all Temp counts in first bucket for temp < 16 degree C.
50 #define TEMP_HIST_HIGH \
51   62  // Putting all Temp counts in last bucket for temp > 62 degree C.
52 #define HIST_COUNT 9
53 #endif
54 #ifdef IMU_TEMP_DBG_ENABLED
55 #define IMU_TEMP_DELTA_TIME_NANOS \
56   5000000000   // Printing every 5 seconds IMU temp.
57 #endif
58 // clang-format on
59 
60 /////////// Start Debug //////////////////////
61 
62 #ifdef ACCEL_CAL_DBG_ENABLED
63 // Total bucket Counter.
accelStatsCounter(struct AccelStillDet * asd,struct AccelStatsMem * adf)64 static void accelStatsCounter(struct AccelStillDet *asd,
65                               struct AccelStatsMem *adf) {
66   // Sorting the data in the different buckets
67   // x bucket ntx.
68   if (PHI < asd->mean_x) {
69     adf->ntx += 1;
70   }
71   // Negative x bucket ntxb.
72   if (PHIb > asd->mean_x) {
73     adf->ntxb += 1;
74   }
75   // Y bucket nty.
76   if (PHI < asd->mean_y) {
77     adf->nty += 1;
78   }
79   // Negative y bucket ntyb.
80   if (PHIb > asd->mean_y) {
81     adf->ntyb += 1;
82   }
83   // Z bucket ntz.
84   if (PHIZ < asd->mean_z) {
85     adf->ntz += 1;
86   }
87   // Negative z bucket ntzb.
88   if (PHIZb > asd->mean_z) {
89     adf->ntzb += 1;
90   }
91   // The leftover bucket ntle.
92   if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
93       PHIb < asd->mean_y && PHIZ > asd->mean_z && PHIZb < asd->mean_z) {
94     adf->ntle += 1;
95   }
96 }
97 
98 // Temp histogram generation.
accelTempHisto(struct AccelStatsMem * adf,float temp)99 static void accelTempHisto(struct AccelStatsMem *adf, float temp) {
100   int index = 0;
101 
102   // Take temp at every stillness detection.
103   adf->start_time_nanos = 0;
104   if (temp <= TEMP_HIST_LOW) {
105     adf->t_hist[0] += 1;
106     return;
107   }
108   if (temp >= TEMP_HIST_HIGH) {
109     adf->t_hist[TEMP_HISTOGRAM - 1] += 1;
110     return;
111   }
112   index = (int)(((temp - TEMP_HIST_LOW) / 2) + 1);
113   adf->t_hist[index] += 1;
114 }
115 
116 #endif
117 ///////// End Debug ////////////////////
118 
119 // Stillness detector reset.
asdReset(struct AccelStillDet * asd)120 static void asdReset(struct AccelStillDet *asd) {
121   asd->nsamples = 0;
122   asd->start_time = 0;
123   asd->acc_x = asd->acc_y = asd->acc_z = 0.0f;
124   asd->acc_xx = asd->acc_yy = asd->acc_zz = 0.0f;
125 }
126 
127 // Stillness detector init.
accelStillInit(struct AccelStillDet * asd,uint32_t t0,uint32_t n_s,float th)128 static void accelStillInit(struct AccelStillDet *asd, uint32_t t0, uint32_t n_s,
129                            float th) {
130   memset(asd, 0, sizeof(struct AccelStillDet));
131   asd->var_th = th;
132   asd->min_batch_window = t0;
133   asd->max_batch_window = t0 + 100000000;
134   asd->min_batch_size = n_s;
135   asd->n_still = 0;
136 }
137 
138 // Good data reset.
agdReset(struct AccelGoodData * agd)139 static void agdReset(struct AccelGoodData *agd) {
140   agd->nx = agd->nxb = 0;
141   agd->ny = agd->nyb = 0;
142   agd->nz = agd->nzb = 0;
143   agd->nle = 0;
144   agd->acc_t = agd->acc_tt = 0;
145   agd->e_x = agd->e_y = agd->e_z = 0;
146 }
147 
148 // Good data init.
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)149 static void accelGoodDataInit(struct AccelGoodData *agd, uint32_t fx,
150                               uint32_t fxb, uint32_t fy, uint32_t fyb,
151                               uint32_t fz, uint32_t fzb, uint32_t fle) {
152   memset(agd, 0, sizeof(struct AccelGoodData));
153   agd->nfx = fx;
154   agd->nfxb = fxb;
155   agd->nfy = fy;
156   agd->nfyb = fyb;
157   agd->nfz = fz;
158   agd->nfzb = fzb;
159   agd->nfle = fle;
160   agd->var_t = 0;
161   agd->mean_t = 0;
162 }
163 
164 // Accel cal algo init (ready for temp buckets).
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)165 static void accelCalAlgoInit(struct AccelCalAlgo *acc, uint32_t fx,
166                              uint32_t fxb, uint32_t fy, uint32_t fyb,
167                              uint32_t fz, uint32_t fzb, uint32_t fle) {
168   accelGoodDataInit(&acc->agd, fx, fxb, fy, fyb, fz, fzb, fle);
169   kasaInit(&acc->akf);
170 }
171 
172 // Returns true when a new accel calibration is available.
accelCalNewBiasAvailable(struct AccelCal * acc)173 bool accelCalNewBiasAvailable(struct AccelCal *acc) {
174   return fabsf(acc->x_bias - acc->x_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
175          fabsf(acc->y_bias - acc->y_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
176          fabsf(acc->z_bias - acc->z_bias_new) > ACCEL_NEW_BIAS_THRESHOLD;
177 }
178 
179 // Accel cal init.
accelCalInit(struct AccelCal * acc,const struct AccelCalParameters * parameters)180 void accelCalInit(struct AccelCal *acc,
181                   const struct AccelCalParameters *parameters) {
182   // Init core accel data.
183   accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb,
184                    parameters->fy, parameters->fyb, parameters->fz,
185                    parameters->fzb, parameters->fle);
186   accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb,
187                    parameters->fy, parameters->fyb, parameters->fz,
188                    parameters->fzb, parameters->fle);
189 
190   // Stillness Reset.
191   accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
192 
193 // Debug data init.
194 #ifdef ACCEL_CAL_DBG_ENABLED
195   memset(&acc->adf, 0, sizeof(struct AccelStatsMem));
196 #endif
197 
198   acc->x_bias = acc->y_bias = acc->z_bias = 0;
199   acc->x_bias_new = acc->y_bias_new = acc->z_bias_new = 0;
200   acc->average_temperature_celsius = 0;
201 
202 #ifdef IMU_TEMP_DBG_ENABLED
203   acc->temp_time_nanos = 0;
204 #endif
205 }
206 
207 // Stillness time check.
stillnessBatchComplete(struct AccelStillDet * asd,uint64_t sample_time_nanos)208 static int stillnessBatchComplete(struct AccelStillDet *asd,
209                                   uint64_t sample_time_nanos) {
210   int complete = 0;
211 
212   // Checking if enough data is accumulated to calc Mean and Var.
213   if ((sample_time_nanos - asd->start_time > asd->min_batch_window) &&
214       (asd->nsamples > asd->min_batch_size)) {
215     if (sample_time_nanos - asd->start_time < asd->max_batch_window) {
216       complete = 1;
217     } else {
218       // Checking for too long batch window, if yes reset and start over.
219       asdReset(asd);
220       return complete;
221     }
222   } else if (sample_time_nanos - asd->start_time > asd->min_batch_window &&
223              (asd->nsamples < asd->min_batch_size)) {
224     // Not enough samples collected in max_batch_window during sample window.
225     asdReset(asd);
226   }
227   return complete;
228 }
229 
230 // Releasing Memory.
accelCalDestroy(struct AccelCal * acc)231 void accelCalDestroy(struct AccelCal *acc) { (void)acc; }
232 
233 // Stillness Detection.
accelStillnessDetection(struct AccelStillDet * asd,uint64_t sample_time_nanos,float x,float y,float z)234 static int accelStillnessDetection(struct AccelStillDet *asd,
235                                    uint64_t sample_time_nanos, float x, float y,
236                                    float z) {
237   float inv = 0.0f;
238   int complete = 0.0f;
239   float g_norm = 0.0f;
240 
241   // Accumulate for mean and VAR.
242   asd->acc_x += x;
243   asd->acc_xx += x * x;
244   asd->acc_y += y;
245   asd->acc_yy += y * y;
246   asd->acc_z += z;
247   asd->acc_zz += z * z;
248 
249   // Setting a new start time and wait until T0 is reached.
250   if (++asd->nsamples == 1) {
251     asd->start_time = sample_time_nanos;
252   }
253   if (stillnessBatchComplete(asd, sample_time_nanos)) {
254     // Getting 1/#samples and checking asd->nsamples != 0.
255     if (0 < asd->nsamples) {
256       inv = 1.0f / asd->nsamples;
257     } else {
258       // Something went wrong resetting and start over.
259       asdReset(asd);
260       return complete;
261     }
262     // Calculating the VAR = sum(x^2)/n - sum(x)^2/n^2.
263     asd->var_x = (asd->acc_xx - (asd->acc_x * asd->acc_x) * inv) * inv;
264     asd->var_y = (asd->acc_yy - (asd->acc_y * asd->acc_y) * inv) * inv;
265     asd->var_z = (asd->acc_zz - (asd->acc_z * asd->acc_z) * inv) * inv;
266     // Checking if sensor is still.
267     if (asd->var_x < asd->var_th && asd->var_y < asd->var_th &&
268         asd->var_z < asd->var_th) {
269       // Calcluating the MEAN = sum(x) / n.
270       asd->mean_x = asd->acc_x * inv;
271       asd->mean_y = asd->acc_y * inv;
272       asd->mean_z = asd->acc_z * inv;
273       // Calculating g_norm^2.
274       g_norm = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
275                asd->mean_z * asd->mean_z;
276       // Magnitude check, still passsing when we have worse case offset.
277       if (g_norm < G_NORM_MAX && g_norm > G_NORM_MIN) {
278         complete = 1;
279         asd->n_still += 1;
280       }
281     }
282     asdReset(asd);
283   }
284   return complete;
285 }
286 
287 // Good data detection, sorting and accumulate the data for Kasa.
accelGoodData(struct AccelStillDet * asd,struct AccelCalAlgo * ac1,float temp)288 static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
289                          float temp) {
290   int complete = 0;
291   float inv = 0.0f;
292 
293   // Sorting the data in the different buckets and accum
294   // x bucket nx.
295   if (PHI < asd->mean_x && ac1->agd.nx < ac1->agd.nfx) {
296     ac1->agd.nx += 1;
297     ac1->agd.acc_t += temp;
298     ac1->agd.acc_tt += temp * temp;
299     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
300   }
301   // Negative x bucket nxb.
302   if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) {
303     ac1->agd.nxb += 1;
304     ac1->agd.acc_t += temp;
305     ac1->agd.acc_tt += temp * temp;
306     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
307   }
308   // Y bucket ny.
309   if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) {
310     ac1->agd.ny += 1;
311     ac1->agd.acc_t += temp;
312     ac1->agd.acc_tt += temp * temp;
313     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
314   }
315   // Negative y bucket nyb.
316   if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) {
317     ac1->agd.nyb += 1;
318     ac1->agd.acc_t += temp;
319     ac1->agd.acc_tt += temp * temp;
320     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
321   }
322   // Z bucket nz.
323   if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) {
324     ac1->agd.nz += 1;
325     ac1->agd.acc_t += temp;
326     ac1->agd.acc_tt += temp * temp;
327     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
328   }
329   // Negative z bucket nzb.
330   if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) {
331     ac1->agd.nzb += 1;
332     ac1->agd.acc_t += temp;
333     ac1->agd.acc_tt += temp * temp;
334     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
335   }
336   // The leftover bucket nle.
337   if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
338       PHIb < asd->mean_y && PHIZ > asd->mean_z && PHIZb < asd->mean_z &&
339       ac1->agd.nle < ac1->agd.nfle) {
340     ac1->agd.nle += 1;
341     ac1->agd.acc_t += temp;
342     ac1->agd.acc_tt += temp * temp;
343     kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
344   }
345   // Checking if all buckets are full.
346   if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb &&
347       ac1->agd.ny == ac1->agd.nfy && ac1->agd.nyb == ac1->agd.nfyb &&
348       ac1->agd.nz == ac1->agd.nfz && ac1->agd.nzb == ac1->agd.nfzb) {
349     //  Check if akf->nsamples is zero.
350     if (ac1->akf.nsamples == 0) {
351       agdReset(&ac1->agd);
352       kasaReset(&ac1->akf);
353       complete = 0;
354       return complete;
355     }
356 
357     // Normalize the data to the sample numbers.
358     kasaNormalize(&ac1->akf);
359 
360     // Calculate the temp VAR and MEAN.
361     inv = 1.0f / ac1->akf.nsamples;
362     ac1->agd.var_t =
363         (ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv;
364     ac1->agd.mean_t = ac1->agd.acc_t * inv;
365     complete = 1;
366   }
367 
368   // If any of the buckets has a bigger number as specified, reset and start
369   // over.
370   if (ac1->agd.nx > ac1->agd.nfx || ac1->agd.nxb > ac1->agd.nfxb ||
371       ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
372       ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
373     agdReset(&ac1->agd);
374     kasaReset(&ac1->akf);
375     complete = 0;
376     return complete;
377   }
378   return complete;
379 }
380 
381 // Eigen value magnitude and ratio test.
accEigenTest(struct KasaFit * akf,struct AccelGoodData * agd)382 static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) {
383   // covariance matrix.
384   struct Mat33 S;
385   S.elem[0][0] = akf->acc_xx - akf->acc_x * akf->acc_x;
386   S.elem[0][1] = S.elem[1][0] = akf->acc_xy - akf->acc_x * akf->acc_y;
387   S.elem[0][2] = S.elem[2][0] = akf->acc_xz - akf->acc_x * akf->acc_z;
388   S.elem[1][1] = akf->acc_yy - akf->acc_y * akf->acc_y;
389   S.elem[1][2] = S.elem[2][1] = akf->acc_yz - akf->acc_y * akf->acc_z;
390   S.elem[2][2] = akf->acc_zz - akf->acc_z * akf->acc_z;
391 
392   struct Vec3 eigenvals;
393   struct Mat33 eigenvecs;
394   mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
395 
396   float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
397   evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
398 
399   float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
400   evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
401 
402   float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
403 
404   // Testing for negative number.
405   float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
406 
407   // Passing when evmin/evmax> EIGEN_RATIO.
408   int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG);
409 
410   agd->e_x = eigenvals.x;
411   agd->e_y = eigenvals.y;
412   agd->e_z = eigenvals.z;
413 
414   return eigen_pass;
415 }
416 
417 // Updating the new bias and save to pointers. Return true if the bias changed.
accelCalUpdateBias(struct AccelCal * acc,float * x,float * y,float * z)418 bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z) {
419   *x = acc->x_bias_new;
420   *y = acc->y_bias_new;
421   *z = acc->z_bias_new;
422 
423   // Check to see if the bias changed since last call to accelCalUpdateBias.
424   // Compiler does not allow us to use "==" and "!=" when comparing floats, so
425   // just use "<" and ">".
426   if ((acc->x_bias < acc->x_bias_new) || (acc->x_bias > acc->x_bias_new) ||
427       (acc->y_bias < acc->y_bias_new) || (acc->y_bias > acc->y_bias_new) ||
428       (acc->z_bias < acc->z_bias_new) || (acc->z_bias > acc->z_bias_new)) {
429     acc->x_bias = acc->x_bias_new;
430     acc->y_bias = acc->y_bias_new;
431     acc->z_bias = acc->z_bias_new;
432     return true;
433   }
434 
435   return false;
436 }
437 
438 // Set the (initial) bias.
accelCalBiasSet(struct AccelCal * acc,float x,float y,float z)439 void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z) {
440   acc->x_bias = acc->x_bias_new = x;
441   acc->y_bias = acc->y_bias_new = y;
442   acc->z_bias = acc->z_bias_new = z;
443 }
444 
445 // Removing the bias.
accelCalBiasRemove(struct AccelCal * acc,float * x,float * y,float * z)446 void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z) {
447   *x = *x - acc->x_bias;
448   *y = *y - acc->y_bias;
449   *z = *z - acc->z_bias;
450 }
451 
452 // Accel Cal Runner.
accelCalRun(struct AccelCal * acc,uint64_t sample_time_nanos,float x,float y,float z,float temp)453 void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x,
454                  float y, float z, float temp) {
455   // Scaling to 1g, better for the algorithm.
456   x *= KSCALE;
457   y *= KSCALE;
458   z *= KSCALE;
459 
460   // DBG: IMU temp messages every 5s.
461 #ifdef IMU_TEMP_DBG_ENABLED
462   if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
463     CAL_DEBUG_LOG("IMU Temp Data: ",
464                   ", " CAL_FORMAT_3DIGITS ",  %" PRIu64
465                   ", " CAL_FORMAT_6DIGITS_TRIPLET " \n",
466                   CAL_ENCODE_FLOAT(temp, 3),
467                   sample_time_nanos,
468                   CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
469                   CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
470                   CAL_ENCODE_FLOAT(acc->z_bias_new, 6));
471     acc->temp_time_nanos = sample_time_nanos;
472   }
473 #endif
474 
475   int temp_gate = 0;
476 
477   // Temp GATE.
478   if (temp < MAX_TEMP && temp > MIN_TEMP) {
479     // Checking if accel is still.
480     if (accelStillnessDetection(&acc->asd, sample_time_nanos, x, y, z)) {
481 #ifdef ACCEL_CAL_DBG_ENABLED
482       // Creating temp hist data.
483       accelTempHisto(&acc->adf, temp);
484 #endif
485 
486       // Two temp buckets.
487       if (temp < TEMP_CUT) {
488         temp_gate = 0;
489       } else {
490         temp_gate = 1;
491       }
492 #ifdef ACCEL_CAL_DBG_ENABLED
493       accelStatsCounter(&acc->asd, &acc->adf);
494 #endif
495       // If still -> pass the averaged accel data (mean) to the
496       // sorting, counting and accum function.
497       if (accelGoodData(&acc->asd, &acc->ac1[temp_gate], temp)) {
498         // Running the Kasa fit.
499         struct Vec3 bias;
500         float radius;
501 
502         // Grabbing the fit from the MAG cal.
503         kasaFit(&acc->ac1[temp_gate].akf, &bias, &radius, G_NORM_MAX,
504                 G_NORM_MIN);
505 
506         // If offset is too large don't take.
507         if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF &&
508             fabsf(bias.z) < MAX_OFF) {
509           // Eigen Ratio Test.
510           if (accEigenTest(&acc->ac1[temp_gate].akf,
511                            &acc->ac1[temp_gate].agd)) {
512             // Storing the new offsets and average temperature.
513             acc->x_bias_new = bias.x * KSCALE2;
514             acc->y_bias_new = bias.y * KSCALE2;
515             acc->z_bias_new = bias.z * KSCALE2;
516             acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t;
517           }
518 #ifdef ACCEL_CAL_DBG_ENABLED
519           //// Debug ///////
520           acc->adf.noff += 1;
521           // Resetting the counter for the offset history.
522           if (acc->adf.n_o > HIST_COUNT) {
523             acc->adf.n_o = 0;
524           }
525 
526           // Storing the Debug data.
527           acc->adf.x_o[acc->adf.n_o] = bias.x;
528           acc->adf.y_o[acc->adf.n_o] = bias.y;
529           acc->adf.z_o[acc->adf.n_o] = bias.z;
530           acc->adf.e_x[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_x;
531           acc->adf.e_y[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_y;
532           acc->adf.e_z[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_z;
533           acc->adf.var_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.var_t;
534           acc->adf.mean_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.mean_t;
535           acc->adf.cal_time[acc->adf.n_o] = sample_time_nanos;
536           acc->adf.rad[acc->adf.n_o] = radius;
537           acc->adf.n_o += 1;
538 #endif
539         } else {
540 #ifdef ACCEL_CAL_DBG_ENABLED
541           acc->adf.noff_max += 1;
542 #endif
543         }
544         ///////////////
545 
546         // Resetting the structs for a new accel cal run.
547         agdReset(&acc->ac1[temp_gate].agd);
548         kasaReset(&acc->ac1[temp_gate].akf);
549       }
550     }
551   }
552 }
553 
554 #ifdef ACCEL_CAL_DBG_ENABLED
555 
556 // Local helper macro for printing log messages.
557 #ifdef CAL_NO_FLOAT_FORMAT_STRINGS
558 #define CAL_FORMAT_ACCEL_HISTORY                                           \
559   "%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d," \
560   "%s%d.%06d,%s%d.%06d,%s%d.%06d"
561 #else
562 #define CAL_FORMAT_ACCEL_HISTORY \
563   "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f"
564 #endif  // CAL_NO_FLOAT_FORMAT_STRINGS
565 
566 // Debug Print Output
accelCalDebPrint(struct AccelCal * acc,float temp)567 void accelCalDebPrint(struct AccelCal *acc, float temp) {
568   static int32_t kk = 0;
569   if (++kk == 1000) {
570     // X offset history last 10 values.
571     CAL_DEBUG_LOG("[ACCEL_CAL]",
572                   "{11," CAL_FORMAT_ACCEL_HISTORY "}(x_off history)\n",
573                   CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
574                   CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
575                   CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
576                   CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
577                   CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
578                   CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
579                   CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
580                   CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
581                   CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
582                   CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
583 
584     // Y offset history last 10 values.
585     CAL_DEBUG_LOG("[ACCEL_CAL]",
586                   "{12," CAL_FORMAT_ACCEL_HISTORY "}(y_off history)\n",
587                   CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
588                   CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
589                   CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
590                   CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
591                   CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
592                   CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
593                   CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
594                   CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
595                   CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
596                   CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
597 
598     // Z offset history last 10 values.
599     CAL_DEBUG_LOG("[ACCEL_CAL]",
600                   "{13," CAL_FORMAT_ACCEL_HISTORY "}(z_off history)\n",
601                   CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
602                   CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
603                   CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
604                   CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
605                   CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
606                   CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
607                   CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
608                   CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
609                   CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
610                   CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
611 
612     // Temp history variation VAR of offset.
613     CAL_DEBUG_LOG("[ACCEL_CAL]",
614                   "{14," CAL_FORMAT_ACCEL_HISTORY "}(VAR temp history)\n",
615                   CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
616                   CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
617                   CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
618                   CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
619                   CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
620                   CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
621                   CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
622                   CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
623                   CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
624                   CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
625 
626     // Temp mean history of offset.
627     CAL_DEBUG_LOG("[ACCEL_CAL]",
628                   "{15," CAL_FORMAT_ACCEL_HISTORY "}(MEAN Temp history)\n",
629                   CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
630                   CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
631                   CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
632                   CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
633                   CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
634                   CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
635                   CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
636                   CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
637                   CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
638                   CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
639 
640     // KASA radius history.
641     CAL_DEBUG_LOG("[ACCEL_CAL]", "{16," CAL_FORMAT_ACCEL_HISTORY "}(radius)\n",
642                   CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
643                   CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
644                   CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
645                   CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
646                   CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
647                   CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
648                   CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
649                   CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
650                   CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
651                   CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
652     kk = 0;
653   }
654 
655   if (kk == 750) {
656     // Eigen Vector X.
657     CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 7," CAL_FORMAT_ACCEL_HISTORY "}(eigen x)\n",
658                   CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
659                   CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
660                   CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
661                   CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
662                   CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
663                   CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
664                   CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
665                   CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
666                   CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
667                   CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
668     // Y.
669     CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 8," CAL_FORMAT_ACCEL_HISTORY "}(eigen y)\n",
670                   CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
671                   CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
672                   CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
673                   CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
674                   CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
675                   CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
676                   CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
677                   CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
678                   CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
679                   CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
680     // Z.
681     CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 9," CAL_FORMAT_ACCEL_HISTORY "}(eigen z)\n",
682                   CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
683                   CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
684                   CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
685                   CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
686                   CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
687                   CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
688                   CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
689                   CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
690                   CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
691                   CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
692     // Accel Time in ns.
693     CAL_DEBUG_LOG("[ACCEL_CAL]",
694                   "{10,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
695                   ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
696                   "}(timestamp ns)\n",
697                   acc->adf.cal_time[0], acc->adf.cal_time[1],
698                   acc->adf.cal_time[2], acc->adf.cal_time[3],
699                   acc->adf.cal_time[4], acc->adf.cal_time[5],
700                   acc->adf.cal_time[6], acc->adf.cal_time[7],
701                   acc->adf.cal_time[8], acc->adf.cal_time[9]);
702   }
703 
704   if (kk == 500) {
705     // Total bucket count.
706     CAL_DEBUG_LOG("[ACCEL_CAL]",
707                   "{ 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d}(Total Bucket #)\n",
708                   (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb,
709                   (unsigned)acc->adf.nty, (unsigned)acc->adf.ntyb,
710                   (unsigned)acc->adf.ntz, (unsigned)acc->adf.ntzb,
711                   (unsigned)acc->adf.ntle);
712     // Live bucket count lower.
713     CAL_DEBUG_LOG("[ACCEL_CAL]",
714                   "{ 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
715                   "lower)\n",
716                   (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
717                   (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
718                   (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
719                   (unsigned)acc->ac1[0].agd.nle,
720                   (unsigned)acc->ac1[0].akf.nsamples);
721     // Live bucket count hogher.
722     CAL_DEBUG_LOG("[ACCEL_CAL]",
723                   "{ 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
724                   "higher)\n",
725                   (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
726                   (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
727                   (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
728                   (unsigned)acc->ac1[1].agd.nle,
729                   (unsigned)acc->ac1[1].akf.nsamples);
730     // Offset used.
731     CAL_DEBUG_LOG("[ACCEL_CAL]",
732                   "{ 3,"CAL_FORMAT_6DIGITS_TRIPLET", %2d}(updated offset "
733                   "x,y,z, total # of offsets)\n",
734                   CAL_ENCODE_FLOAT(acc->x_bias, 6),
735                   CAL_ENCODE_FLOAT(acc->y_bias, 6),
736                   CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
737     // Offset New.
738     CAL_DEBUG_LOG("[ACCEL_CAL]",
739                   "{ 4," CAL_FORMAT_6DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS
740                   "}(New offset x,y,z, live temp)\n",
741                   CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
742                   CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
743                   CAL_ENCODE_FLOAT(acc->z_bias_new, 6),
744                   CAL_ENCODE_FLOAT(temp, 6));
745     // Temp Histogram.
746     CAL_DEBUG_LOG("[ACCEL_CAL]",
747                   "{ 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
748                   "%7d, %7d}(temp histo)\n",
749                   (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
750                   (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
751                   (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
752                   (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
753                   (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
754                   (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
755                   (unsigned)acc->adf.t_hist[12]);
756     CAL_DEBUG_LOG("[ACCEL_CAL]",
757                   "{ 6,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
758                   "%7d}(temp histo)\n",
759                   (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
760                   (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
761                   (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
762                   (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
763                   (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
764                   (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
765   }
766 }
767 #endif
768