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. 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. 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. 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. 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. 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. 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). 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. 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. 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. 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. 229 void accelCalDestroy(struct AccelCal *acc) { (void)acc; } 230 231 // Stillness Detection. 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. 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. 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. 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. 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. 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. 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 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
