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 // adapted from frameworks/native/services/sensorservice/Fusion.cpp
18
19 #include <algos/fusion.h>
20 #include <algos/mat.h>
21
22 #include <errno.h>
23 #include <nanohub_math.h>
24 #include <stdio.h>
25
26 #include <seos.h>
27
28 #ifdef DEBUG_CH
29 // change to 0 to disable fusion debugging output
30 #define DEBUG_FUSION 0
31 #endif
32
33 #define ACC 1
34 #define MAG 2
35 #define GYRO 4
36
37 #define DEFAULT_GYRO_VAR 1e-7f
38 #define DEFAULT_GYRO_BIAS_VAR 1e-12f
39 #define DEFAULT_ACC_STDEV 5e-2f
40 #define DEFAULT_MAG_STDEV 5e-1f
41
42 #define GEOMAG_GYRO_VAR 2e-4f
43 #define GEOMAG_GYRO_BIAS_VAR 1e-4f
44 #define GEOMAG_ACC_STDEV 0.02f
45 #define GEOMAG_MAG_STDEV 0.02f
46
47 #define SYMMETRY_TOLERANCE 1e-10f
48 #define FAKE_MAG_INTERVAL 1.0f //sec
49
50 #define NOMINAL_GRAVITY 9.81f
51 #define FREE_FALL_THRESHOLD (0.1f * NOMINAL_GRAVITY)
52 #define FREE_FALL_THRESHOLD_SQ (FREE_FALL_THRESHOLD * FREE_FALL_THRESHOLD)
53
54 #define MAX_VALID_MAGNETIC_FIELD 75.0f
55 #define MAX_VALID_MAGNETIC_FIELD_SQ (MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD)
56
57 #define MIN_VALID_MAGNETIC_FIELD 20.0f //norminal mag field strength is 25uT in some area
58 #define MIN_VALID_MAGNETIC_FIELD_SQ (MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD)
59
60 #define MIN_VALID_CROSS_PRODUCT_MAG 1.0e-3
61 #define MIN_VALID_CROSS_PRODUCT_MAG_SQ (MIN_VALID_CROSS_PRODUCT_MAG * MIN_VALID_CROSS_PRODUCT_MAG)
62
63 #define DELTA_TIME_MARGIN 1.0e-9f
64
65 #define TRUST_DURATION_MANUAL_MAG_CAL 5.f //unit: seconds
66
initFusion(struct Fusion * fusion,uint32_t flags)67 void initFusion(struct Fusion *fusion, uint32_t flags) {
68 fusion->flags = flags;
69
70 if (flags & FUSION_USE_GYRO) {
71 // normal fusion mode
72 fusion->param.gyro_var = DEFAULT_GYRO_VAR;
73 fusion->param.gyro_bias_var = DEFAULT_GYRO_BIAS_VAR;
74 fusion->param.acc_stdev = DEFAULT_ACC_STDEV;
75 fusion->param.mag_stdev = DEFAULT_MAG_STDEV;
76 } else {
77 // geo mag mode
78 fusion->param.gyro_var = GEOMAG_GYRO_VAR;
79 fusion->param.gyro_bias_var = GEOMAG_GYRO_BIAS_VAR;
80 fusion->param.acc_stdev = GEOMAG_ACC_STDEV;
81 fusion->param.mag_stdev = GEOMAG_MAG_STDEV;
82 }
83
84 if (flags & FUSION_REINITIALIZE)
85 {
86 initVec3(&fusion->Ba, 0.0f, 0.0f, 1.0f);
87 initVec3(&fusion->Bm, 0.0f, 1.0f, 0.0f);
88
89 initVec4(&fusion->x0, 0.0f, 0.0f, 0.0f, 0.0f);
90 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f);
91
92 fusion->mInitState = 0;
93
94 fusion->mPredictDt = 0.0f;
95 fusion->mCount[0] = fusion->mCount[1] = fusion->mCount[2] = 0;
96
97 initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f);
98 initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f);
99 initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f);
100
101 } else {
102 // mask off disabled sensor bit
103 fusion->mInitState &= (ACC
104 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
105 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
106 }
107
108 fusionSetMagTrust(fusion, NORMAL);
109 fusion->lastMagInvalid = false;
110 }
111
fusionHasEstimate(const struct Fusion * fusion)112 int fusionHasEstimate(const struct Fusion *fusion) {
113 // waive sensor init depends on the mode
114 return fusion->mInitState == (ACC
115 | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0)
116 | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0));
117 }
118
updateDt(struct Fusion * fusion,float dT)119 static void updateDt(struct Fusion *fusion, float dT) {
120 if (fabsf(fusion->mPredictDt - dT) > DELTA_TIME_MARGIN) {
121 float dT2 = dT * dT;
122 float dT3 = dT2 * dT;
123
124 float q00 = fusion->param.gyro_var * dT +
125 0.33333f * fusion->param.gyro_bias_var * dT3;
126 float q11 = fusion->param.gyro_bias_var * dT;
127 float q10 = 0.5f * fusion->param.gyro_bias_var * dT2;
128 float q01 = q10;
129
130 initDiagonalMatrix(&fusion->GQGt[0][0], q00);
131 initDiagonalMatrix(&fusion->GQGt[0][1], -q10);
132 initDiagonalMatrix(&fusion->GQGt[1][0], -q01);
133 initDiagonalMatrix(&fusion->GQGt[1][1], q11);
134 fusion->mPredictDt = dT;
135 }
136 }
137
fusion_init_complete(struct Fusion * fusion,int what,const struct Vec3 * d,float dT)138 static int fusion_init_complete(struct Fusion *fusion, int what, const struct Vec3 *d, float dT) {
139 if (fusionHasEstimate(fusion)) {
140 return 1;
141 }
142
143 switch (what) {
144 case ACC:
145 {
146 if (!(fusion->flags & FUSION_USE_GYRO)) {
147 updateDt(fusion, dT);
148 }
149 struct Vec3 unityD = *d;
150 vec3Normalize(&unityD);
151
152 vec3Add(&fusion->mData[0], &unityD);
153 ++fusion->mCount[0];
154
155 if (fusion->mCount[0] == 8) {
156 fusion->mInitState |= ACC;
157 }
158 break;
159 }
160
161 case MAG:
162 {
163 struct Vec3 unityD = *d;
164 vec3Normalize(&unityD);
165
166 vec3Add(&fusion->mData[1], &unityD);
167 ++fusion->mCount[1];
168
169 fusion->mInitState |= MAG;
170 break;
171 }
172
173 case GYRO:
174 {
175 updateDt(fusion, dT);
176
177 struct Vec3 scaledD = *d;
178 vec3ScalarMul(&scaledD, dT);
179
180 vec3Add(&fusion->mData[2], &scaledD);
181 ++fusion->mCount[2];
182
183 fusion->mInitState |= GYRO;
184 break;
185 }
186
187 default:
188 // assert(!"should not be here");
189 break;
190 }
191
192 if (fusionHasEstimate(fusion)) {
193 vec3ScalarMul(&fusion->mData[0], 1.0f / fusion->mCount[0]);
194
195 if (fusion->flags & FUSION_USE_MAG) {
196 vec3ScalarMul(&fusion->mData[1], 1.0f / fusion->mCount[1]);
197 } else {
198 fusion->fake_mag_decimation = 0.f;
199 }
200
201 struct Vec3 up = fusion->mData[0];
202
203 struct Vec3 east;
204 if (fusion->flags & FUSION_USE_MAG) {
205 vec3Cross(&east, &fusion->mData[1], &up);
206 vec3Normalize(&east);
207 } else {
208 findOrthogonalVector(up.x, up.y, up.z, &east.x, &east.y, &east.z);
209 }
210
211 struct Vec3 north;
212 vec3Cross(&north, &up, &east);
213
214 struct Mat33 R;
215 initMatrixColumns(&R, &east, &north, &up);
216
217 //Quat q;
218 //initQuat(&q, &R);
219
220 initQuat(&fusion->x0, &R);
221 initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f);
222
223 initZeroMatrix(&fusion->P[0][0]);
224 initZeroMatrix(&fusion->P[0][1]);
225 initZeroMatrix(&fusion->P[1][0]);
226 initZeroMatrix(&fusion->P[1][1]);
227
228 fusionSetMagTrust(fusion, INITIALIZATION);
229 }
230
231 return 0;
232 }
233
matrixCross(struct Mat33 * out,struct Vec3 * p,float diag)234 static void matrixCross(struct Mat33 *out, struct Vec3 *p, float diag) {
235 out->elem[0][0] = diag;
236 out->elem[1][1] = diag;
237 out->elem[2][2] = diag;
238 out->elem[1][0] = p->z;
239 out->elem[0][1] = -p->z;
240 out->elem[2][0] = -p->y;
241 out->elem[0][2] = p->y;
242 out->elem[2][1] = p->x;
243 out->elem[1][2] = -p->x;
244 }
245
fusionCheckState(struct Fusion * fusion)246 static void fusionCheckState(struct Fusion *fusion) {
247
248 if (!mat33IsPositiveSemidefinite(&fusion->P[0][0], SYMMETRY_TOLERANCE)
249 || !mat33IsPositiveSemidefinite(
250 &fusion->P[1][1], SYMMETRY_TOLERANCE)) {
251
252 initZeroMatrix(&fusion->P[0][0]);
253 initZeroMatrix(&fusion->P[0][1]);
254 initZeroMatrix(&fusion->P[1][0]);
255 initZeroMatrix(&fusion->P[1][1]);
256 }
257 }
258
259 #define kEps 1.0E-4f
260
261 UNROLLED
fusionPredict(struct Fusion * fusion,const struct Vec3 * w)262 static void fusionPredict(struct Fusion *fusion, const struct Vec3 *w) {
263 const float dT = fusion->mPredictDt;
264
265 Quat q = fusion->x0;
266 struct Vec3 b = fusion->x1;
267
268 struct Vec3 we = *w;
269 vec3Sub(&we, &b);
270
271 struct Mat33 I33;
272 initDiagonalMatrix(&I33, 1.0f);
273
274 struct Mat33 I33dT;
275 initDiagonalMatrix(&I33dT, dT);
276
277 struct Mat33 wx;
278 matrixCross(&wx, &we, 0.0f);
279
280 struct Mat33 wx2;
281 mat33Multiply(&wx2, &wx, &wx);
282
283 float norm_we = vec3Norm(&we);
284
285 if (fabsf(norm_we) < kEps) {
286 return;
287 }
288
289 float lwedT = norm_we * dT;
290 float hlwedT = 0.5f * lwedT;
291 float ilwe = 1.0f / norm_we;
292 float k0 = (1.0f - cosf(lwedT)) * (ilwe * ilwe);
293 float k1 = sinf(lwedT);
294 float k2 = cosf(hlwedT);
295
296 struct Vec3 psi = we;
297 vec3ScalarMul(&psi, sinf(hlwedT) * ilwe);
298
299 struct Vec3 negPsi = psi;
300 vec3ScalarMul(&negPsi, -1.0f);
301
302 struct Mat33 O33;
303 matrixCross(&O33, &negPsi, k2);
304
305 struct Mat44 O;
306 uint32_t i;
307 for (i = 0; i < 3; ++i) {
308 uint32_t j;
309 for (j = 0; j < 3; ++j) {
310 O.elem[i][j] = O33.elem[i][j];
311 }
312 }
313
314 O.elem[3][0] = -psi.x;
315 O.elem[3][1] = -psi.y;
316 O.elem[3][2] = -psi.z;
317 O.elem[3][3] = k2;
318
319 O.elem[0][3] = psi.x;
320 O.elem[1][3] = psi.y;
321 O.elem[2][3] = psi.z;
322
323 struct Mat33 tmp = wx;
324 mat33ScalarMul(&tmp, k1 * ilwe);
325
326 fusion->Phi0[0] = I33;
327 mat33Sub(&fusion->Phi0[0], &tmp);
328
329 tmp = wx2;
330 mat33ScalarMul(&tmp, k0);
331
332 mat33Add(&fusion->Phi0[0], &tmp);
333
334 tmp = wx;
335 mat33ScalarMul(&tmp, k0);
336 fusion->Phi0[1] = tmp;
337
338 mat33Sub(&fusion->Phi0[1], &I33dT);
339
340 tmp = wx2;
341 mat33ScalarMul(&tmp, ilwe * ilwe * ilwe * (lwedT - k1));
342
343 mat33Sub(&fusion->Phi0[1], &tmp);
344
345 mat44Apply(&fusion->x0, &O, &q);
346
347 if (fusion->x0.w < 0.0f) {
348 fusion->x0.x = -fusion->x0.x;
349 fusion->x0.y = -fusion->x0.y;
350 fusion->x0.z = -fusion->x0.z;
351 fusion->x0.w = -fusion->x0.w;
352 }
353
354 // Pnew = Phi * P
355
356 struct Mat33 Pnew[2][2];
357 mat33Multiply(&Pnew[0][0], &fusion->Phi0[0], &fusion->P[0][0]);
358 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][0]);
359 mat33Add(&Pnew[0][0], &tmp);
360
361 mat33Multiply(&Pnew[0][1], &fusion->Phi0[0], &fusion->P[0][1]);
362 mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][1]);
363 mat33Add(&Pnew[0][1], &tmp);
364
365 Pnew[1][0] = fusion->P[1][0];
366 Pnew[1][1] = fusion->P[1][1];
367
368 // P = Pnew * Phi^T
369
370 mat33MultiplyTransposed2(&fusion->P[0][0], &Pnew[0][0], &fusion->Phi0[0]);
371 mat33MultiplyTransposed2(&tmp, &Pnew[0][1], &fusion->Phi0[1]);
372 mat33Add(&fusion->P[0][0], &tmp);
373
374 fusion->P[0][1] = Pnew[0][1];
375
376 mat33MultiplyTransposed2(&fusion->P[1][0], &Pnew[1][0], &fusion->Phi0[0]);
377 mat33MultiplyTransposed2(&tmp, &Pnew[1][1], &fusion->Phi0[1]);
378 mat33Add(&fusion->P[1][0], &tmp);
379
380 fusion->P[1][1] = Pnew[1][1];
381
382 mat33Add(&fusion->P[0][0], &fusion->GQGt[0][0]);
383 mat33Add(&fusion->P[0][1], &fusion->GQGt[0][1]);
384 mat33Add(&fusion->P[1][0], &fusion->GQGt[1][0]);
385 mat33Add(&fusion->P[1][1], &fusion->GQGt[1][1]);
386
387 fusionCheckState(fusion);
388 }
389
fusionHandleGyro(struct Fusion * fusion,const struct Vec3 * w,float dT)390 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT) {
391 if (!fusion_init_complete(fusion, GYRO, w, dT)) {
392 return;
393 }
394
395 updateDt(fusion, dT);
396
397 fusionPredict(fusion, w);
398 }
399
400 UNROLLED
scaleCovariance(struct Mat33 * out,const struct Mat33 * A,const struct Mat33 * P)401 static void scaleCovariance(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *P) {
402 uint32_t r;
403 for (r = 0; r < 3; ++r) {
404 uint32_t j;
405 for (j = r; j < 3; ++j) {
406 float apat = 0.0f;
407 uint32_t c;
408 for (c = 0; c < 3; ++c) {
409 float v = A->elem[c][r] * P->elem[c][c] * 0.5f;
410 uint32_t k;
411 for (k = c + 1; k < 3; ++k) {
412 v += A->elem[k][r] * P->elem[c][k];
413 }
414
415 apat += 2.0f * v * A->elem[c][j];
416 }
417
418 out->elem[r][j] = apat;
419 out->elem[j][r] = apat;
420 }
421 }
422 }
423
getF(struct Vec4 F[3],const struct Vec4 * q)424 static void getF(struct Vec4 F[3], const struct Vec4 *q) {
425 F[0].x = q->w; F[1].x = -q->z; F[2].x = q->y;
426 F[0].y = q->z; F[1].y = q->w; F[2].y = -q->x;
427 F[0].z = -q->y; F[1].z = q->x; F[2].z = q->w;
428 F[0].w = -q->x; F[1].w = -q->y; F[2].w = -q->z;
429 }
430
fusionUpdate(struct Fusion * fusion,const struct Vec3 * z,const struct Vec3 * Bi,float sigma)431 static void fusionUpdate(
432 struct Fusion *fusion, const struct Vec3 *z, const struct Vec3 *Bi, float sigma) {
433 struct Mat33 A;
434 quatToMatrix(&A, &fusion->x0);
435
436 struct Vec3 Bb;
437 mat33Apply(&Bb, &A, Bi);
438
439 struct Mat33 L;
440 matrixCross(&L, &Bb, 0.0f);
441
442 struct Mat33 R;
443 initDiagonalMatrix(&R, sigma * sigma);
444
445 struct Mat33 S;
446 scaleCovariance(&S, &L, &fusion->P[0][0]);
447
448 mat33Add(&S, &R);
449
450 struct Mat33 Si;
451 mat33Invert(&Si, &S);
452
453 struct Mat33 LtSi;
454 mat33MultiplyTransposed(&LtSi, &L, &Si);
455
456 struct Mat33 K[2];
457 mat33Multiply(&K[0], &fusion->P[0][0], &LtSi);
458 mat33MultiplyTransposed(&K[1], &fusion->P[0][1], &LtSi);
459
460 struct Mat33 K0L;
461 mat33Multiply(&K0L, &K[0], &L);
462
463 struct Mat33 K1L;
464 mat33Multiply(&K1L, &K[1], &L);
465
466 struct Mat33 tmp;
467 mat33Multiply(&tmp, &K0L, &fusion->P[0][0]);
468 mat33Sub(&fusion->P[0][0], &tmp);
469
470 mat33Multiply(&tmp, &K1L, &fusion->P[0][1]);
471 mat33Sub(&fusion->P[1][1], &tmp);
472
473 mat33Multiply(&tmp, &K0L, &fusion->P[0][1]);
474 mat33Sub(&fusion->P[0][1], &tmp);
475
476 mat33Transpose(&fusion->P[1][0], &fusion->P[0][1]);
477
478 struct Vec3 e = *z;
479 vec3Sub(&e, &Bb);
480
481 struct Vec3 dq;
482 mat33Apply(&dq, &K[0], &e);
483
484
485 struct Vec4 F[3];
486 getF(F, &fusion->x0);
487
488 // 4x3 * 3x1 => 4x1
489
490 struct Vec4 q;
491 q.x = fusion->x0.x + 0.5f * (F[0].x * dq.x + F[1].x * dq.y + F[2].x * dq.z);
492 q.y = fusion->x0.y + 0.5f * (F[0].y * dq.x + F[1].y * dq.y + F[2].y * dq.z);
493 q.z = fusion->x0.z + 0.5f * (F[0].z * dq.x + F[1].z * dq.y + F[2].z * dq.z);
494 q.w = fusion->x0.w + 0.5f * (F[0].w * dq.x + F[1].w * dq.y + F[2].w * dq.z);
495
496 fusion->x0 = q;
497 quatNormalize(&fusion->x0);
498
499 if (fusion->flags & FUSION_USE_MAG) {
500 // accumulate gyro bias (causes self spin) only if not
501 // game rotation vector
502 struct Vec3 db;
503 mat33Apply(&db, &K[1], &e);
504 vec3Add(&fusion->x1, &db);
505 }
506
507 fusionCheckState(fusion);
508 }
509
510 #define ACC_TRUSTWORTHY(abs_norm_err) ((abs_norm_err) < 1.f)
511 #define ACC_COS_CONV_FACTOR 0.01f
512 #define ACC_COS_CONV_LIMIT 3.f
513
fusionHandleAcc(struct Fusion * fusion,const struct Vec3 * a,float dT)514 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT) {
515 if (!fusion_init_complete(fusion, ACC, a, dT)) {
516 return -EINVAL;
517 }
518
519 float norm2 = vec3NormSquared(a);
520
521 if (norm2 < FREE_FALL_THRESHOLD_SQ) {
522 return -EINVAL;
523 }
524
525 float l = sqrtf(norm2);
526 float l_inv = 1.0f / l;
527
528 if (!(fusion->flags & FUSION_USE_GYRO)) {
529 // geo mag mode
530 // drive the Kalman filter with zero mean dummy gyro vector
531 struct Vec3 w_dummy;
532
533 // avoid (fabsf(norm_we) < kEps) in fusionPredict()
534 initVec3(&w_dummy, fusion->x1.x + kEps, fusion->x1.y + kEps,
535 fusion->x1.z + kEps);
536
537 updateDt(fusion, dT);
538 fusionPredict(fusion, &w_dummy);
539 }
540
541 struct Mat33 R;
542 fusionGetRotationMatrix(fusion, &R);
543
544 if (!(fusion->flags & FUSION_USE_MAG) &&
545 (fusion->fake_mag_decimation += dT) > FAKE_MAG_INTERVAL) {
546 // game rotation mode, provide fake mag update to prevent
547 // P to diverge over time
548 struct Vec3 m;
549 mat33Apply(&m, &R, &fusion->Bm);
550
551 fusionUpdate(fusion, &m, &fusion->Bm,
552 fusion->param.mag_stdev);
553 fusion->fake_mag_decimation = 0.f;
554 }
555
556 struct Vec3 unityA = *a;
557 vec3ScalarMul(&unityA, l_inv);
558
559 float d = fabsf(l - NOMINAL_GRAVITY);
560 float p;
561 if (fusion->flags & FUSION_USE_GYRO) {
562 float fc = 0;
563 // Enable faster convergence
564 if (ACC_TRUSTWORTHY(d)) {
565 struct Vec3 aa;
566 mat33Apply(&aa, &R, &fusion->Ba);
567 float cos_err = vec3Dot(&aa, &unityA);
568 cos_err = cos_err < (1.f - ACC_COS_CONV_FACTOR) ?
569 (1.f - ACC_COS_CONV_FACTOR) : cos_err;
570 fc = (1.f - cos_err) *
571 (1.0f / ACC_COS_CONV_FACTOR * ACC_COS_CONV_LIMIT);
572 }
573 p = fusion->param.acc_stdev * expf(3 * d - fc);
574 } else {
575 // Adaptive acc weighting (trust acc less as it deviates from nominal g
576 // more), acc_stdev *= e(sqrt(| |acc| - g_nominal|))
577 //
578 // The weighting equation comes from heuristics.
579 p = fusion->param.acc_stdev * expf(sqrtf(d));
580 }
581
582 fusionUpdate(fusion, &unityA, &fusion->Ba, p);
583
584 return 0;
585 }
586
587 #define MAG_COS_CONV_FACTOR 0.02f
588 #define MAG_COS_CONV_LIMIT 3.5f
589 #define MAG_STDEV_REDUCTION 0.005f // lower stdev means more trust
590
fusionHandleMag(struct Fusion * fusion,const struct Vec3 * m,float dT)591 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) {
592 if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) {
593 return -EINVAL;
594 }
595
596 float magFieldSq = vec3NormSquared(m);
597
598 if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ
599 || magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) {
600 fusionSetMagTrust(fusion, NORMAL);
601 fusion->lastMagInvalid = true;
602 return -EINVAL;
603 }
604
605 struct Mat33 R;
606 fusionGetRotationMatrix(fusion, &R);
607
608 struct Vec3 up;
609 mat33Apply(&up, &R, &fusion->Ba);
610
611 struct Vec3 east;
612 vec3Cross(&east, m, &up);
613
614 if (vec3NormSquared(&east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) {
615 fusionSetMagTrust(fusion, NORMAL);
616 fusion->lastMagInvalid = true;
617 return -EINVAL;
618 }
619
620 if (fusion->lastMagInvalid) {
621 fusion->lastMagInvalid = false;
622 fusionSetMagTrust(fusion, BACK_TO_VALID);
623 }
624
625 struct Vec3 north;
626 vec3Cross(&north, &up, &east);
627
628 float invNorm = 1.0f / vec3Norm(&north);
629 vec3ScalarMul(&north, invNorm);
630
631 float p = fusion->param.mag_stdev;
632
633 if (fusion->flags & FUSION_USE_GYRO) {
634 struct Vec3 mm;
635 mat33Apply(&mm, &R, &fusion->Bm);
636 float cos_err = vec3Dot(&mm, &north);
637
638 if (fusion->trustedMagDuration > 0) {
639 // if the trust mag time period is not finished
640 if (cos_err < (1.f - MAG_COS_CONV_FACTOR/4)) {
641 // if the mag direction and the fusion north has not converged, lower the
642 // standard deviation of mag to speed up convergence.
643 p *= MAG_STDEV_REDUCTION;
644 fusion->trustedMagDuration -= dT;
645 } else {
646 // it has converged already, so no need to keep the trust period any longer
647 fusionSetMagTrust(fusion, NORMAL);
648 }
649 } else {
650 cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ?
651 (1.f - MAG_COS_CONV_FACTOR) : cos_err;
652
653 float fc;
654 fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT);
655 p *= expf(-fc);
656 }
657 }
658
659 fusionUpdate(fusion, &north, &fusion->Bm, p);
660
661 return 0;
662 }
663
fusionSetMagTrust(struct Fusion * fusion,int mode)664 void fusionSetMagTrust(struct Fusion *fusion, int mode) {
665 switch(mode) {
666 case NORMAL:
667 fusion->trustedMagDuration = 0; // disable
668 break;
669 case INITIALIZATION:
670 case BACK_TO_VALID:
671 fusion->trustedMagDuration = 0; // no special treatment for these two
672 break;
673 case MANUAL_MAG_CAL:
674 fusion->trustedMagDuration = TRUST_DURATION_MANUAL_MAG_CAL;
675 break;
676 default:
677 fusion->trustedMagDuration = 0; // by default it is disable
678 break;
679 }
680 }
681
fusionGetAttitude(const struct Fusion * fusion,struct Vec4 * attitude)682 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) {
683 *attitude = fusion->x0;
684 }
685
fusionGetBias(const struct Fusion * fusion,struct Vec3 * bias)686 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) {
687 *bias = fusion->x1;
688 }
689
fusionGetRotationMatrix(const struct Fusion * fusion,struct Mat33 * R)690 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R) {
691 quatToMatrix(R, &fusion->x0);
692 }
693