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/magnetometer/mag_cal/mag_cal.h" 18 19 #include <errno.h> 20 #include <inttypes.h> 21 #include <string.h> 22 23 #include "calibration/util/cal_log.h" 24 25 // Local helper macro for printing log messages. 26 #ifdef MAG_CAL_DEBUG_ENABLE 27 #ifdef CAL_NO_FLOAT_FORMAT_STRINGS 28 #define CAL_FORMAT_MAG_MEMORY \ 29 "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ 30 "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ 31 "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \ 32 "%s%d.%03d, %s%d.%03d" 33 #else 34 #define CAL_FORMAT_MAG_MEMORY \ 35 "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, " \ 36 "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f" 37 #endif // CAL_NO_FLOAT_FORMAT_STRINGS 38 #endif // MAG_CAL_DEBUG_ENABLE 39 40 // clang-format off 41 #define MAX_EIGEN_RATIO 15.0f 42 #define MAX_EIGEN_MAG 70.0f // uT 43 #define MIN_EIGEN_MAG 20.0f // uT 44 #define MAX_FIT_MAG 70.0f 45 #define MIN_FIT_MAG 20.0f 46 #define MAX_BATCH_WINDOW 15000000UL // 15 sec 47 #define MIN_BATCH_SIZE 25 // samples 48 #define MAX_DISTANCE_VIOLATIONS 2 49 // clang-format 50 51 // eigen value magnitude and ratio test. 52 static int moc_eigen_test(struct KasaFit *kasa) { 53 // covariance matrix. 54 struct Mat33 S; 55 S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x; 56 S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y; 57 S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z; 58 S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y; 59 S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z; 60 S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z; 61 62 struct Vec3 eigenvals; 63 struct Mat33 eigenvecs; 64 mat33GetEigenbasis(&S, &eigenvals, &eigenvecs); 65 66 float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y; 67 evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax; 68 69 float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y; 70 evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin; 71 72 float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z; 73 74 // Testing for negative number. 75 float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0; 76 77 int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) && 78 (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG); 79 80 return eigen_pass; 81 } 82 83 void magCalReset(struct MagCal *moc) { 84 kasaReset(&moc->kasa); 85 diversityCheckerReset(&moc->diversity_checker); 86 moc->start_time = 0; 87 moc->kasa_batching = false; 88 } 89 90 static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) { 91 bool complete = false; 92 93 if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) && 94 (moc->kasa.nsamples > MIN_BATCH_SIZE)) { 95 complete = true; 96 97 } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) { 98 // not enough samples collected in MAX_BATCH_WINDOW or too many 99 // maximum distance violations detected. 100 magCalReset(moc); 101 } 102 103 return complete; 104 } 105 106 void initMagCal(struct MagCal *moc, 107 const struct MagCalParameters *mag_cal_parameters, 108 const struct DiversityCheckerParameters *diverse_parameters) { 109 magCalReset(moc); 110 moc->update_time = 0; 111 moc->min_batch_window_in_micros = 112 mag_cal_parameters->min_batch_window_in_micros; 113 moc->radius = 0.0f; 114 115 moc->x_bias = mag_cal_parameters->x_bias; 116 moc->y_bias = mag_cal_parameters->y_bias; 117 moc->z_bias = mag_cal_parameters->z_bias; 118 119 moc->c00 = mag_cal_parameters->c00; 120 moc->c01 = mag_cal_parameters->c01; 121 moc->c02 = mag_cal_parameters->c02; 122 moc->c10 = mag_cal_parameters->c10; 123 moc->c11 = mag_cal_parameters->c11; 124 moc->c12 = mag_cal_parameters->c12; 125 moc->c20 = mag_cal_parameters->c20; 126 moc->c21 = mag_cal_parameters->c21; 127 moc->c22 = mag_cal_parameters->c22; 128 129 #ifdef MAG_CAL_DEBUG_ENABLE 130 moc->mag_dbg.mag_trigger_count = 0; 131 moc->mag_dbg.kasa_count = 0; 132 #endif // MAG_CAL_DEBUG_ENABLE 133 134 // Diversity Checker 135 diversityCheckerInit(&moc->diversity_checker, diverse_parameters); 136 } 137 138 void magCalDestroy(struct MagCal *moc) { (void)moc; } 139 140 enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us, 141 float x, float y, float z) { 142 enum MagUpdate new_bias = NO_UPDATE; 143 144 // Diversity Checker Update. 145 diversityCheckerUpdate(&moc->diversity_checker, x, y, z); 146 147 // 1. run accumulators 148 kasaAccumulate(&moc->kasa, x, y, z); 149 150 if (moc->kasa.nsamples == 1) { 151 moc->start_time = sample_time_us; 152 moc->kasa_batching = true; 153 } 154 155 // 2. batch has enough samples? 156 if (moc_batch_complete(moc, sample_time_us)) { 157 kasaNormalize(&moc->kasa); 158 159 // 3. eigen test 160 if (moc_eigen_test(&moc->kasa)) { 161 struct Vec3 bias; 162 float radius; 163 // 4. Kasa sphere fitting 164 if (kasaFit(&moc->kasa, &bias, &radius, MAX_FIT_MAG, MIN_FIT_MAG)) { 165 #ifdef MAG_CAL_DEBUG_ENABLE 166 moc->mag_dbg.kasa_count++; 167 CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET 168 ", " CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32, 169 CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3), 170 CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3), 171 moc->mag_dbg.kasa_count, moc->mag_dbg.mag_trigger_count); 172 #endif // MAG_CAL_DEBUG_ENABLE 173 174 // Update the local field. 175 diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius); 176 177 // checking if data is diverse. 178 if (diversityCheckerNormQuality(&moc->diversity_checker, bias.x, bias.y, 179 bias.z) && 180 moc->diversity_checker.num_max_dist_violations <= 181 MAX_DISTANCE_VIOLATIONS) { 182 // DEBUG PRINT OUT. 183 #ifdef MAG_CAL_DEBUG_ENABLE 184 moc->mag_dbg.mag_trigger_count++; 185 moc->diversity_checker.diversity_dbg.new_trigger = 1; 186 CAL_DEBUG_LOG( 187 "[MAG_CAL:BIAS UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET ", " 188 CAL_FORMAT_3DIGITS ", " CAL_FORMAT_6DIGITS ", " 189 CAL_FORMAT_3DIGITS_TRIPLET ", %zu, " CAL_FORMAT_3DIGITS ", " 190 CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32 ", %" PRIu64 ", " 191 CAL_FORMAT_3DIGITS_TRIPLET ", %" PRIu64 "", 192 CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3), 193 CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3), 194 CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.var_log, 6), 195 CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.mean_log, 196 3), 197 CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.max_log, 3), 198 CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.min_log, 3), 199 moc->diversity_checker.num_points, 200 CAL_ENCODE_FLOAT(moc->diversity_checker.threshold, 3), 201 CAL_ENCODE_FLOAT(moc->diversity_checker.max_distance, 3), 202 moc->mag_dbg.mag_trigger_count, 203 moc->mag_dbg.kasa_count, 204 sample_time_us, 205 CAL_ENCODE_FLOAT(moc->x_bias, 3), 206 CAL_ENCODE_FLOAT(moc->y_bias, 3), 207 CAL_ENCODE_FLOAT(moc->z_bias, 3), 208 moc->update_time); 209 #endif // MAG_CAL_DEBUG_ENABLE 210 moc->x_bias = bias.x; 211 moc->y_bias = bias.y; 212 moc->z_bias = bias.z; 213 214 moc->radius = radius; 215 moc->update_time = sample_time_us; 216 217 new_bias = UPDATE_BIAS; 218 } 219 } 220 } 221 // 5. reset for next batch 222 magCalReset(moc); 223 } 224 225 return new_bias; 226 } 227 228 void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z) { 229 *x = moc->x_bias; 230 *y = moc->y_bias; 231 *z = moc->z_bias; 232 } 233 234 void magCalAddBias(struct MagCal *moc, float x, float y, float z) { 235 moc->x_bias += x; 236 moc->y_bias += y; 237 moc->z_bias += z; 238 } 239 240 void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi, 241 float *xo, float *yo, float *zo) { 242 *xo = xi - moc->x_bias; 243 *yo = yi - moc->y_bias; 244 *zo = zi - moc->z_bias; 245 } 246 247 void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02, 248 float c10, float c11, float c12, float c20, float c21, 249 float c22) { 250 moc->c00 = c00; 251 moc->c01 = c01; 252 moc->c02 = c02; 253 moc->c10 = c10; 254 moc->c11 = c11; 255 moc->c12 = c12; 256 moc->c20 = c20; 257 moc->c21 = c21; 258 moc->c22 = c22; 259 } 260 261 void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi, 262 float *xo, float *yo, float *zo) { 263 *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi; 264 *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi; 265 *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi; 266 } 267 268 #if defined MAG_CAL_DEBUG_ENABLE 269 // This function prints every second sample parts of the dbg diverse_data_log, 270 // which ensures that all the messages get printed into the log file. 271 void magLogPrint(struct DiversityChecker *diverse_data, float temp) { 272 // Sample counter. 273 static size_t sample_counter = 0; 274 const float *data_log_ptr = &diverse_data->diversity_dbg.diverse_data_log[0]; 275 if (diverse_data->diversity_dbg.new_trigger == 1) { 276 sample_counter++; 277 if (sample_counter == 2) { 278 CAL_DEBUG_LOG( 279 "[MAG_CAL:MEMORY X] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY ", " 280 CAL_FORMAT_3DIGITS, 281 diverse_data->diversity_dbg.diversity_count, 282 CAL_ENCODE_FLOAT(data_log_ptr[0 * 3], 3), 283 CAL_ENCODE_FLOAT(data_log_ptr[1 * 3], 3), 284 CAL_ENCODE_FLOAT(data_log_ptr[2 * 3], 3), 285 CAL_ENCODE_FLOAT(data_log_ptr[3 * 3], 3), 286 CAL_ENCODE_FLOAT(data_log_ptr[4 * 3], 3), 287 CAL_ENCODE_FLOAT(data_log_ptr[5 * 3], 3), 288 CAL_ENCODE_FLOAT(data_log_ptr[6 * 3], 3), 289 CAL_ENCODE_FLOAT(data_log_ptr[7 * 3], 3), 290 CAL_ENCODE_FLOAT(data_log_ptr[8 * 3], 3), 291 CAL_ENCODE_FLOAT(data_log_ptr[9 * 3], 3), 292 CAL_ENCODE_FLOAT(data_log_ptr[10 * 3], 3), 293 CAL_ENCODE_FLOAT(data_log_ptr[11 * 3], 3), 294 CAL_ENCODE_FLOAT(data_log_ptr[12 * 3], 3), 295 CAL_ENCODE_FLOAT(data_log_ptr[13 * 3], 3), 296 CAL_ENCODE_FLOAT(data_log_ptr[14 * 3], 3), 297 CAL_ENCODE_FLOAT(data_log_ptr[15 * 3], 3), 298 CAL_ENCODE_FLOAT(data_log_ptr[16 * 3], 3), 299 CAL_ENCODE_FLOAT(data_log_ptr[17 * 3], 3), 300 CAL_ENCODE_FLOAT(data_log_ptr[18 * 3], 3), 301 CAL_ENCODE_FLOAT(data_log_ptr[19 * 3], 3), CAL_ENCODE_FLOAT(temp, 3)); 302 } 303 304 if (sample_counter == 4) { 305 CAL_DEBUG_LOG( 306 "[MAG_CAL:MEMORY Y] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY, 307 diverse_data->diversity_dbg.diversity_count, 308 CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 1], 3), 309 CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 1], 3), 310 CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 1], 3), 311 CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 1], 3), 312 CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 1], 3), 313 CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 1], 3), 314 CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 1], 3), 315 CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 1], 3), 316 CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 1], 3), 317 CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 1], 3), 318 CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 1], 3), 319 CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 1], 3), 320 CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 1], 3), 321 CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 1], 3), 322 CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 1], 3), 323 CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 1], 3), 324 CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 1], 3), 325 CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 1], 3), 326 CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 1], 3), 327 CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 1], 3)); 328 } 329 if (sample_counter == 6) { 330 CAL_DEBUG_LOG( 331 "[MAG_CAL:MEMORY Z] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY, 332 diverse_data->diversity_dbg.diversity_count, 333 CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 2], 3), 334 CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 2], 3), 335 CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 2], 3), 336 CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 2], 3), 337 CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 2], 3), 338 CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 2], 3), 339 CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 2], 3), 340 CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 2], 3), 341 CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 2], 3), 342 CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 2], 3), 343 CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 2], 3), 344 CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 2], 3), 345 CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 2], 3), 346 CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 2], 3), 347 CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 2], 3), 348 CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 2], 3), 349 CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 2], 3), 350 CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 2], 3), 351 CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 2], 3), 352 CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 2], 3)); 353 sample_counter = 0; 354 diverse_data->diversity_dbg.new_trigger = 0; 355 } 356 } 357 } 358 #endif // MAG_CAL_DEBUG_ENABLE 359
