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