File content as of revision 0:e493567c21ac:

#include "SENSOR_FUSION.h"
#include "math.h"

int conv_init;

float axm =0;
float aym =0;
float azm =0;
float u1 = 0;
float u2= 0;
float u3= 0;
float mx= 0;
float my= 0;
float mz= 0;

float axm_f= 0;
float axm_f_old= 0;
float u3aym_f= 0;
float u3aym_f_old= 0;
float u2azm_f= 0;
float u2azm_f_old= 0;

float aym_f= 0;
float aym_f_old= 0;
float u3axm_f= 0;
float u3axm_f_old= 0;
float u1azm_f= 0;
float u1azm_f_old= 0;

float u2axm_f= 0;
float u2axm_f_old= 0;
float u1aym_f= 0;
float u1aym_f_old= 0;
float azm_f= 0;
float azm_f_old= 0;


float x1_hat = 0;
float x2_hat= 0;
float x3_hat= 0;
float cospitch= 0;
float pitch_angle= 0;
float roll_angle = 0;

float omega_phi= 0;

float lpf(float in, float out_old, float alpha, float TS) {
    float out;

    out = (1. - alpha * TS) * out_old + alpha * TS*in;
    return out;
}

void pitch_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_2,float alpha,float TS)
{
    axm_f = lpf(a_xm,axm_f_old,alpha,TS);
    axm_f_old = axm_f;
    u3aym_f = lpf(u_3*a_ym,u3aym_f_old,alpha, TS);
    u3aym_f_old = u3aym_f;
    u2azm_f = lpf(u_2*a_zm,u2azm_f_old,alpha, TS);
    u2azm_f_old = u2azm_f;    
    
    x1_hat = axm_f + u3aym_f/alpha - u2azm_f/alpha;
    cospitch = sqrt(1-(x1_hat*0.1020*x1_hat*0.1020));
    pitch_angle = asin(x1_hat*0.1020);
}

void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha, float TS)
{
    aym_f = lpf(a_ym,aym_f_old,alpha, TS);
    aym_f_old = aym_f;
    u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha, TS);
    u3axm_f_old = u3axm_f;
    u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha, TS);
    u1azm_f_old = u1azm_f;    
    
    x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha;
 
}

void x3_hat_estimat(float a_xm,float a_ym,float a_zm,float u_2,float u_1,float alpha, float TS)
{
    u2axm_f = lpf(u_2*a_xm,u2axm_f_old,alpha, TS);
    u2axm_f_old = u2axm_f;
    u1aym_f = lpf(u_1*a_ym,u1aym_f_old,alpha, TS);
    u1aym_f_old = u1aym_f;
    azm_f = lpf(a_zm,azm_f_old,alpha, TS);
    azm_f_old = azm_f;    
    
    x3_hat = u2axm_f/alpha - u1aym_f/alpha + azm_f;
    
    roll_angle = asin(x3_hat*(-0.1012)/cospitch);
    omega_phi = u3;
    
}