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 #ifndef FUSION_H_
18 
19 #define FUSION_H_
20 
21 #include <common/math/vec.h>
22 #include <common/math/mat.h>
23 #include <common/math/quat.h>
24 
25 #include <stdbool.h>
26 #include <stdint.h>
27 #include <sys/types.h>
28 
29 #ifdef __cplusplus
30 extern "C" {
31 #endif
32 
33 struct FusionParam {
34   float gyro_var;
35   float gyro_bias_var;
36   float acc_stdev;
37   float mag_stdev;
38 };
39 
40 struct Fusion {
41     Quat x0;
42     struct Vec3 x1;
43 
44     struct Mat33 P[2][2];
45     struct Mat33 GQGt[2][2];
46 
47     struct Mat33 Phi0[2];
48     struct Vec3 Ba, Bm;
49     uint32_t mInitState;
50     float mPredictDt;
51     struct Vec3 mData[3];
52     uint32_t mCount[3];
53     uint32_t flags;
54 
55     float trustedMagDuration;
56     bool    lastMagInvalid;
57 
58     float  fake_mag_decimation;
59     struct FusionParam param;
60 };
61 
62 enum FusionFlagBits {
63     FUSION_USE_MAG      = 1 << 0,
64     FUSION_USE_GYRO     = 1 << 1,
65     FUSION_REINITIALIZE = 1 << 2,
66 };
67 
68 enum MagTrustMode {
69     NORMAL,
70     INITIALIZATION,  // right after initialization of fusion
71     BACK_TO_VALID,   // when the mag value goes from invalid to valid
72     MANUAL_MAG_CAL   // right after a manual calibration
73 };
74 
75 void initFusion(struct Fusion *fusion, uint32_t flags);
76 
77 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT);
78 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT);
79 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT);
80 
81 // set trust mode of mag sensors depending on scenarios, see MagTrustMode
82 void fusionSetMagTrust(struct Fusion *fusion, int mode);
83 
84 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude);
85 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias);
86 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R);
87 int fusionHasEstimate(const struct Fusion *fusion);
88 
89 
90 #ifdef __cplusplus
91 }
92 #endif
93 
94 #endif  // FUSION_H_
95