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
