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