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