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