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