firmware/inc/fusion/fusion.h

#ifndef FUSION_H_

#define FUSION_H_

#include "vec.h"
#include "mat.h"
#include "quat.h"

#include <stdint.h>
#include <sys/types.h>

#ifdef __cplusplus
extern "C" {
#endif

struct FusionParam {
  float gyro_var;
  float gyro_bias_var;
  float acc_stdev;
  float mag_stdev;
};

struct Fusion {
    Quat x0;
    struct Vec3 x1;

    struct Mat33 P[2][2];
    struct Mat33 GQGt[2][2];

    struct Mat33 Phi0[2];
    struct Vec3 Ba, Bm;
    uint32_t mInitState;
    float mGyroRate;
    struct Vec3 mData[3];
    size_t mCount[3];
    uint32_t flags;

    float  fake_mag_decimation;
    struct FusionParam param;
};

enum FusionFlagBits {
    FUSION_USE_MAG      = 1 << 0,
    FUSION_USE_GYRO     = 1 << 1,
    FUSION_REINITIALIZE = 1 << 2,
};

void initFusion(struct Fusion *fusion, uint32_t flags);

void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT);
int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT);
int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m);

void fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude);
void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias);
void fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R);
int fusionHasEstimate(const struct Fusion *fusion);

#ifdef __cplusplus
}
#endif

#endif  // FUSION_H_