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