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. 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. 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. 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. 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. 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. 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). 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. 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 acc->average_temperature_celsius = 0; 184 185 #ifdef IMU_TEMP_DBG_ENABLED 186 acc->temp_time_nanos = 0; 187 #endif 188 } 189 190 // Stillness time check. 191 static int stillnessBatchComplete(struct AccelStillDet *asd, 192 uint64_t sample_time_nanos) { 193 int complete = 0; 194 195 // Checking if enough data is accumulated to calc Mean and Var. 196 if ((sample_time_nanos - asd->start_time > asd->min_batch_window) && 197 (asd->nsamples > asd->min_batch_size)) { 198 if (sample_time_nanos - asd->start_time < asd->max_batch_window) { 199 complete = 1; 200 } else { 201 // Checking for too long batch window, if yes reset and start over. 202 asdReset(asd); 203 return complete; 204 } 205 } else if (sample_time_nanos - asd->start_time > asd->min_batch_window && 206 (asd->nsamples < asd->min_batch_size)) { 207 // Not enough samples collected in max_batch_window during sample window. 208 asdReset(asd); 209 } 210 return complete; 211 } 212 213 // Releasing Memory. 214 void accelCalDestroy(struct AccelCal *acc) { (void)acc; } 215 216 // Stillness Detection. 217 static int accelStillnessDetection(struct AccelStillDet *asd, 218 uint64_t sample_time_nanos, float x, float y, 219 float z) { 220 float inv = 0.0f; 221 int complete = 0.0f; 222 float g_norm = 0.0f; 223 224 // Accumulate for mean and VAR. 225 asd->acc_x += x; 226 asd->acc_xx += x * x; 227 asd->acc_y += y; 228 asd->acc_yy += y * y; 229 asd->acc_z += z; 230 asd->acc_zz += z * z; 231 232 // Setting a new start time and wait until T0 is reached. 233 if (++asd->nsamples == 1) { 234 asd->start_time = sample_time_nanos; 235 } 236 if (stillnessBatchComplete(asd, sample_time_nanos)) { 237 // Getting 1/#samples and checking asd->nsamples != 0. 238 if (0 < asd->nsamples) { 239 inv = 1.0f / asd->nsamples; 240 } else { 241 // Something went wrong resetting and start over. 242 asdReset(asd); 243 return complete; 244 } 245 // Calculating the VAR = sum(x^2)/n - sum(x)^2/n^2. 246 asd->var_x = (asd->acc_xx - (asd->acc_x * asd->acc_x) * inv) * inv; 247 asd->var_y = (asd->acc_yy - (asd->acc_y * asd->acc_y) * inv) * inv; 248 asd->var_z = (asd->acc_zz - (asd->acc_z * asd->acc_z) * inv) * inv; 249 // Checking if sensor is still. 250 if (asd->var_x < asd->var_th && asd->var_y < asd->var_th && 251 asd->var_z < asd->var_th) { 252 // Calcluating the MEAN = sum(x) / n. 253 asd->mean_x = asd->acc_x * inv; 254 asd->mean_y = asd->acc_y * inv; 255 asd->mean_z = asd->acc_z * inv; 256 // Calculating g_norm^2. 257 g_norm = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y + 258 asd->mean_z * asd->mean_z; 259 // Magnitude check, still passsing when we have worse case offset. 260 if (g_norm < G_NORM_MAX && g_norm > G_NORM_MIN) { 261 complete = 1; 262 asd->n_still += 1; 263 } 264 } 265 asdReset(asd); 266 } 267 return complete; 268 } 269 270 // Accumulate data for KASA fit. 271 static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) { 272 // Run accumulators. 273 float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y + 274 asd->mean_z * asd->mean_z; 275 276 akf->acc_x += asd->mean_x; 277 akf->acc_y += asd->mean_y; 278 akf->acc_z += asd->mean_z; 279 akf->acc_w += w; 280 281 akf->acc_xx += asd->mean_x * asd->mean_x; 282 akf->acc_xy += asd->mean_x * asd->mean_y; 283 akf->acc_xz += asd->mean_x * asd->mean_z; 284 akf->acc_xw += asd->mean_x * w; 285 286 akf->acc_yy += asd->mean_y * asd->mean_y; 287 akf->acc_yz += asd->mean_y * asd->mean_z; 288 akf->acc_yw += asd->mean_y * w; 289 290 akf->acc_zz += asd->mean_z * asd->mean_z; 291 akf->acc_zw += asd->mean_z * w; 292 akf->nsamples += 1; 293 } 294 295 // Good data detection, sorting and accumulate the data for Kasa. 296 static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1, 297 float temp) { 298 int complete = 0; 299 float inv = 0.0f; 300 301 // Sorting the data in the different buckets and accum 302 // x bucket nx. 303 if (PHI < asd->mean_x && ac1->agd.nx < ac1->agd.nfx) { 304 ac1->agd.nx += 1; 305 ac1->agd.acc_t += temp; 306 ac1->agd.acc_tt += temp * temp; 307 accelCalUpdate(&ac1->akf, asd); 308 } 309 // Negative x bucket nxb. 310 if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) { 311 ac1->agd.nxb += 1; 312 ac1->agd.acc_t += temp; 313 ac1->agd.acc_tt += temp * temp; 314 accelCalUpdate(&ac1->akf, asd); 315 } 316 // Y bucket ny. 317 if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) { 318 ac1->agd.ny += 1; 319 ac1->agd.acc_t += temp; 320 ac1->agd.acc_tt += temp * temp; 321 accelCalUpdate(&ac1->akf, asd); 322 } 323 // Negative y bucket nyb. 324 if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) { 325 ac1->agd.nyb += 1; 326 ac1->agd.acc_t += temp; 327 ac1->agd.acc_tt += temp * temp; 328 accelCalUpdate(&ac1->akf, asd); 329 } 330 // Z bucket nz. 331 if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) { 332 ac1->agd.nz += 1; 333 ac1->agd.acc_t += temp; 334 ac1->agd.acc_tt += temp * temp; 335 accelCalUpdate(&ac1->akf, asd); 336 } 337 // Negative z bucket nzb. 338 if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) { 339 ac1->agd.nzb += 1; 340 ac1->agd.acc_t += temp; 341 ac1->agd.acc_tt += temp * temp; 342 accelCalUpdate(&ac1->akf, asd); 343 } 344 // The leftover bucket nle. 345 if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y && 346 PHIb < asd->mean_y && PHIZ > asd->mean_z && PHIZb < asd->mean_z && 347 ac1->agd.nle < ac1->agd.nfle) { 348 ac1->agd.nle += 1; 349 ac1->agd.acc_t += temp; 350 ac1->agd.acc_tt += temp * temp; 351 accelCalUpdate(&ac1->akf, asd); 352 } 353 // Checking if all buckets are full. 354 if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb && 355 ac1->agd.ny == ac1->agd.nfy && ac1->agd.nyb == ac1->agd.nfyb && 356 ac1->agd.nz == ac1->agd.nfz && ac1->agd.nzb == ac1->agd.nfzb) { 357 // Check if akf->nsamples is zero. 358 if (ac1->akf.nsamples == 0) { 359 agdReset(&ac1->agd); 360 magKasaReset(&ac1->akf); 361 complete = 0; 362 return complete; 363 } else { 364 // Normalize the data to the sample numbers. 365 inv = 1.0f / ac1->akf.nsamples; 366 } 367 368 ac1->akf.acc_x *= inv; 369 ac1->akf.acc_y *= inv; 370 ac1->akf.acc_z *= inv; 371 ac1->akf.acc_w *= inv; 372 373 ac1->akf.acc_xx *= inv; 374 ac1->akf.acc_xy *= inv; 375 ac1->akf.acc_xz *= inv; 376 ac1->akf.acc_xw *= inv; 377 378 ac1->akf.acc_yy *= inv; 379 ac1->akf.acc_yz *= inv; 380 ac1->akf.acc_yw *= inv; 381 382 ac1->akf.acc_zz *= inv; 383 ac1->akf.acc_zw *= inv; 384 385 // Calculate the temp VAR and MEA.N 386 ac1->agd.var_t = 387 (ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv; 388 ac1->agd.mean_t = ac1->agd.acc_t * inv; 389 complete = 1; 390 } 391 392 // If any of the buckets has a bigger number as specified, reset and start 393 // over. 394 if (ac1->agd.nx > ac1->agd.nfx || ac1->agd.nxb > ac1->agd.nfxb || 395 ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb || 396 ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) { 397 agdReset(&ac1->agd); 398 magKasaReset(&ac1->akf); 399 complete = 0; 400 return complete; 401 } 402 return complete; 403 } 404 405 // Eigen value magnitude and ratio test. 406 static int accEigenTest(struct KasaFit *akf, struct AccelGoodData *agd) { 407 // covariance matrix. 408 struct Mat33 S; 409 S.elem[0][0] = akf->acc_xx - akf->acc_x * akf->acc_x; 410 S.elem[0][1] = S.elem[1][0] = akf->acc_xy - akf->acc_x * akf->acc_y; 411 S.elem[0][2] = S.elem[2][0] = akf->acc_xz - akf->acc_x * akf->acc_z; 412 S.elem[1][1] = akf->acc_yy - akf->acc_y * akf->acc_y; 413 S.elem[1][2] = S.elem[2][1] = akf->acc_yz - akf->acc_y * akf->acc_z; 414 S.elem[2][2] = akf->acc_zz - akf->acc_z * akf->acc_z; 415 416 struct Vec3 eigenvals; 417 struct Mat33 eigenvecs; 418 mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); 419 420 float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; 421 evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; 422 423 float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; 424 evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; 425 426 float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; 427 428 // Testing for negative number. 429 float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; 430 431 // Passing when evmin/evmax> EIGEN_RATIO. 432 int eigen_pass = (evmin > evmax * EIGEN_RATIO) && (evmag > EIGEN_MAG); 433 434 agd->e_x = eigenvals.x; 435 agd->e_y = eigenvals.y; 436 agd->e_z = eigenvals.z; 437 438 return eigen_pass; 439 } 440 441 // Updating the new bias and save to pointers. Return true if the bias changed. 442 bool accelCalUpdateBias(struct AccelCal *acc, float *x, float *y, float *z) { 443 *x = acc->x_bias_new; 444 *y = acc->y_bias_new; 445 *z = acc->z_bias_new; 446 447 // Check to see if the bias changed since last call to accelCalUpdateBias. 448 // Compiler does not allow us to use "==" and "!=" when comparing floats, so 449 // just use "<" and ">". 450 if ((acc->x_bias < acc->x_bias_new) || (acc->x_bias > acc->x_bias_new) || 451 (acc->y_bias < acc->y_bias_new) || (acc->y_bias > acc->y_bias_new) || 452 (acc->z_bias < acc->z_bias_new) || (acc->z_bias > acc->z_bias_new)) { 453 acc->x_bias = acc->x_bias_new; 454 acc->y_bias = acc->y_bias_new; 455 acc->z_bias = acc->z_bias_new; 456 return true; 457 } 458 459 return false; 460 } 461 462 // Set the (initial) bias. 463 void accelCalBiasSet(struct AccelCal *acc, float x, float y, float z) { 464 acc->x_bias = acc->x_bias_new = x; 465 acc->y_bias = acc->y_bias_new = y; 466 acc->z_bias = acc->z_bias_new = z; 467 } 468 469 // Removing the bias. 470 void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z) { 471 *x = *x - acc->x_bias; 472 *y = *y - acc->y_bias; 473 *z = *z - acc->z_bias; 474 } 475 476 // Accel Cal Runner. 477 void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, 478 float y, float z, float temp) { 479 // Scaling to 1g, better for the algorithm. 480 x *= KSCALE; 481 y *= KSCALE; 482 z *= KSCALE; 483 484 // DBG: IMU temp messages every 5s. 485 #ifdef IMU_TEMP_DBG_ENABLED 486 if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) { 487 CAL_DEBUG_LOG("IMU Temp Data: ", 488 ", %s%d.%02d, %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n", 489 CAL_ENCODE_FLOAT(temp, 2), 490 (unsigned long long int)sample_time_nanos, 491 CAL_ENCODE_FLOAT(acc->x_bias_new,5), 492 CAL_ENCODE_FLOAT(acc->y_bias_new,5), 493 CAL_ENCODE_FLOAT(acc->z_bias_new,5)); 494 acc->temp_time_nanos = sample_time_nanos; 495 } 496 #endif 497 498 int temp_gate = 0; 499 500 // Temp GATE. 501 if (temp < MAX_TEMP && temp > MIN_TEMP) { 502 // Checking if accel is still. 503 if (accelStillnessDetection(&acc->asd, sample_time_nanos, x, y, z)) { 504 #ifdef ACCEL_CAL_DBG_ENABLED 505 // Creating temp hist data. 506 accelTempHisto(&acc->adf, temp); 507 #endif 508 509 // Two temp buckets. 510 if (temp < TEMP_CUT) { 511 temp_gate = 0; 512 } else { 513 temp_gate = 1; 514 } 515 #ifdef ACCEL_CAL_DBG_ENABLED 516 accelStatsCounter(&acc->asd, &acc->adf); 517 #endif 518 // If still -> pass the averaged accel data (mean) to the 519 // sorting, counting and accum function. 520 if (accelGoodData(&acc->asd, &acc->ac1[temp_gate], temp)) { 521 // Running the Kasa fit. 522 struct Vec3 bias; 523 float radius; 524 525 // Grabbing the fit from the MAG cal. 526 magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius); 527 528 // If offset is too large don't take. 529 if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF && 530 fabsf(bias.z) < MAX_OFF) { 531 // Eigen Ratio Test. 532 if (accEigenTest(&acc->ac1[temp_gate].akf, 533 &acc->ac1[temp_gate].agd)) { 534 // Storing the new offsets and average temperature. 535 acc->x_bias_new = bias.x * KSCALE2; 536 acc->y_bias_new = bias.y * KSCALE2; 537 acc->z_bias_new = bias.z * KSCALE2; 538 acc->average_temperature_celsius = acc->ac1[temp_gate].agd.mean_t; 539 } 540 #ifdef ACCEL_CAL_DBG_ENABLED 541 //// Debug /////// 542 acc->adf.noff += 1; 543 // Resetting the counter for the offset history. 544 if (acc->adf.n_o > HIST_COUNT) { 545 acc->adf.n_o = 0; 546 } 547 548 // Storing the Debug data. 549 acc->adf.x_o[acc->adf.n_o] = bias.x; 550 acc->adf.y_o[acc->adf.n_o] = bias.y; 551 acc->adf.z_o[acc->adf.n_o] = bias.z; 552 acc->adf.e_x[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_x; 553 acc->adf.e_y[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_y; 554 acc->adf.e_z[acc->adf.n_o] = acc->ac1[temp_gate].agd.e_z; 555 acc->adf.var_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.var_t; 556 acc->adf.mean_t[acc->adf.n_o] = acc->ac1[temp_gate].agd.mean_t; 557 acc->adf.cal_time[acc->adf.n_o] = sample_time_nanos; 558 acc->adf.rad[acc->adf.n_o] = radius; 559 acc->adf.n_o += 1; 560 #endif 561 } else { 562 #ifdef ACCEL_CAL_DBG_ENABLED 563 acc->adf.noff_max += 1; 564 #endif 565 } 566 /////////////// 567 568 // Resetting the structs for a new accel cal run. 569 agdReset(&acc->ac1[temp_gate].agd); 570 magKasaReset(&acc->ac1[temp_gate].akf); 571 } 572 } 573 } 574 } 575 576 #ifdef ACCEL_CAL_DBG_ENABLED 577 // Debug Print Output 578 void accelCalDebPrint(struct AccelCal *acc, float temp) { 579 static int32_t kk = 0; 580 if (++kk == 1000) { 581 // X offset history last 10 values. 582 CAL_DEBUG_LOG( 583 "[BMI160]", 584 "{MK_ACCEL,11,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 585 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(x_off history)\n", 586 CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6), 587 CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6), 588 CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6), 589 CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6), 590 CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6), 591 CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6), 592 CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6), 593 CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6), 594 CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6), 595 CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6)); 596 597 // Y offset history last 10 values. 598 CAL_DEBUG_LOG( 599 "[BMI160]", 600 "{MK_ACCEL,12,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 601 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(y_off history)\n", 602 CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6), 603 CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6), 604 CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6), 605 CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6), 606 CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6), 607 CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6), 608 CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6), 609 CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6), 610 CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6), 611 CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6)); 612 613 // Z offset history last 10 values. 614 CAL_DEBUG_LOG( 615 "[BMI160]", 616 "{MK_ACCEL,13,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 617 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(z_off history)\n", 618 CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6), 619 CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6), 620 CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6), 621 CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6), 622 CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6), 623 CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6), 624 CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6), 625 CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6), 626 CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6), 627 CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6)); 628 629 // Temp history variation VAR of offset. 630 CAL_DEBUG_LOG( 631 "[BMI160]", 632 "{MK_ACCEL,14,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 633 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(VAR temp history)\n", 634 CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6), 635 CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6), 636 CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6), 637 CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6), 638 CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6), 639 CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6), 640 CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6), 641 CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6), 642 CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6), 643 CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6)); 644 645 // Temp mean history of offset. 646 CAL_DEBUG_LOG( 647 "[BMI160]", 648 "{MK_ACCEL,15,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 649 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(MEAN Temp history)\n", 650 CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6), 651 CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6), 652 CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6), 653 CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6), 654 CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6), 655 CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6), 656 CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6), 657 CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6), 658 CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6), 659 CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6)); 660 661 // KASA radius history. 662 CAL_DEBUG_LOG( 663 "[BMI160]", 664 "{MK_ACCEL,16,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 665 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(radius)\n", 666 CAL_ENCODE_FLOAT(acc->adf.rad[0], 6), 667 CAL_ENCODE_FLOAT(acc->adf.rad[1], 6), 668 CAL_ENCODE_FLOAT(acc->adf.rad[2], 6), 669 CAL_ENCODE_FLOAT(acc->adf.rad[3], 6), 670 CAL_ENCODE_FLOAT(acc->adf.rad[4], 6), 671 CAL_ENCODE_FLOAT(acc->adf.rad[5], 6), 672 CAL_ENCODE_FLOAT(acc->adf.rad[6], 6), 673 CAL_ENCODE_FLOAT(acc->adf.rad[7], 6), 674 CAL_ENCODE_FLOAT(acc->adf.rad[8], 6), 675 CAL_ENCODE_FLOAT(acc->adf.rad[9], 6)); 676 kk = 0; 677 } 678 679 if (kk == 750) { 680 // Eigen Vector X. 681 CAL_DEBUG_LOG( 682 "[BMI160]", 683 "{MK_ACCEL, " 684 "7,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 685 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen x)\n", 686 CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6), 687 CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6), 688 CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6), 689 CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6), 690 CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6), 691 CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6), 692 CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6), 693 CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6), 694 CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6), 695 CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6)); 696 // Y. 697 CAL_DEBUG_LOG( 698 "[BMI160]", 699 "{MK_ACCEL, " 700 "8,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 701 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen y)\n", 702 CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6), 703 CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6), 704 CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6), 705 CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6), 706 CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6), 707 CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6), 708 CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6), 709 CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6), 710 CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6), 711 CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6)); 712 // Z. 713 CAL_DEBUG_LOG( 714 "[BMI160]", 715 "{MK_ACCEL, " 716 "9,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%" 717 "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen z)\n", 718 CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6), 719 CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6), 720 CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6), 721 CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6), 722 CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6), 723 CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6), 724 CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6), 725 CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6), 726 CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6), 727 CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6)); 728 // Accel Time in ns. 729 CAL_DEBUG_LOG( 730 "[BMI160]", 731 "{MK_ACCEL,10,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,}(" 732 "timestamp ns)\n", 733 acc->adf.cal_time[0], acc->adf.cal_time[1], acc->adf.cal_time[2], 734 acc->adf.cal_time[3], acc->adf.cal_time[4], acc->adf.cal_time[5], 735 acc->adf.cal_time[6], acc->adf.cal_time[7], acc->adf.cal_time[8], 736 acc->adf.cal_time[9]); 737 } 738 739 if (kk == 500) { 740 // Total bucket count. 741 CAL_DEBUG_LOG( 742 "[BMI160]", 743 "{MK_ACCEL, 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d,}(Total Bucket #)\n", 744 (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, (unsigned)acc->adf.nty, 745 (unsigned)acc->adf.ntyb, (unsigned)acc->adf.ntz, 746 (unsigned)acc->adf.ntzb, (unsigned)acc->adf.ntle); 747 // Live bucket count lower. 748 CAL_DEBUG_LOG( 749 "[BMI160]", 750 "{MK_ACCEL, 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # " 751 "lower)\n", 752 (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb, 753 (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb, 754 (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb, 755 (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples); 756 // Live bucket count hogher. 757 CAL_DEBUG_LOG( 758 "[BMI160]", 759 "{MK_ACCEL, 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # " 760 "higher)\n", 761 (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb, 762 (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb, 763 (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb, 764 (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples); 765 // Offset used. 766 CAL_DEBUG_LOG( 767 "[BMI160]", 768 "{MK_ACCEL, 3,%s%d.%06d, %s%d.%06d, %s%d.%06d, %2d,}(updated offset " 769 "x,y,z, total # of offsets)\n", 770 CAL_ENCODE_FLOAT(acc->x_bias, 6), CAL_ENCODE_FLOAT(acc->y_bias, 6), 771 CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff); 772 // Offset New. 773 CAL_DEBUG_LOG( 774 "[BMI160]", 775 "{MK_ACCEL, 4,%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d,}(New offset " 776 "x,y,z, live temp)\n", 777 CAL_ENCODE_FLOAT(acc->x_bias_new, 6), 778 CAL_ENCODE_FLOAT(acc->y_bias_new, 6), 779 CAL_ENCODE_FLOAT(acc->z_bias_new, 6), CAL_ENCODE_FLOAT(temp, 6)); 780 // Temp Histogram. 781 CAL_DEBUG_LOG( 782 "[BMI160]", 783 "{MK_ACCEL, 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " 784 "%7d, %7d,}(temp histo)\n", 785 (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1], 786 (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3], 787 (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5], 788 (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7], 789 (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9], 790 (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11], 791 (unsigned)acc->adf.t_hist[12]); 792 CAL_DEBUG_LOG( 793 "[BMI160]", 794 "M{K_ACCEL, 6,%7d, %7d, %7d,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, " 795 "%7d,}(temp histo)\n", 796 (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14], 797 (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16], 798 (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18], 799 (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20], 800 (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22], 801 (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]); 802 } 803 } 804 #endif 805
