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 "vec.h"
22 #include "mat.h"
23 #include "quat.h"
24 
25 #include <stdint.h>
26 #include <sys/types.h>
27 
28 #ifdef __cplusplus
29 extern "C" {
30 #endif
31 
32 struct FusionParam {
33   float gyro_var;
34   float gyro_bias_var;
35   float acc_stdev;
36   float mag_stdev;
37 };
38 
39 struct Fusion {
40     Quat x0;
41     struct Vec3 x1;
42 
43     struct Mat33 P[2][2];
44     struct Mat33 GQGt[2][2];
45 
46     struct Mat33 Phi0[2];
47     struct Vec3 Ba, Bm;
48     uint32_t mInitState;
49     float mPredictDt;
50     struct Vec3 mData[3];
51     uint32_t mCount[3];
52     uint32_t flags;
53 
54     float  fake_mag_decimation;
55     struct FusionParam param;
56 };
57 
58 enum FusionFlagBits {
59     FUSION_USE_MAG      = 1 << 0,
60     FUSION_USE_GYRO     = 1 << 1,
61     FUSION_REINITIALIZE = 1 << 2,
62 };
63 
64 void initFusion(struct Fusion *fusion, uint32_t flags);
65 
66 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT);
67 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT);
68 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m);
69 
70 void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude);
71 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias);
72 void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R);
73 int fusionHasEstimate(const struct Fusion *fusion);
74 
75 #ifdef __cplusplus
76 }
77 #endif
78 
79 #endif  // FUSION_H_
80