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