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