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