File content as of revision 0:bf9bf4b7625f:

#include "SensorFusion.h"
#include "SystemConstant.h"
#include "math.h"

///Gyro and Acce data
float axm = 0.0f;
float aym = 0.0f;
float azm = 0.0f;
float u1 = 0.0f;
float u2 = 0.0f;
float u3 = 0.0f;
float mx = 0.0f;
float my = 0.0f;
float mz = 0.0f;
float Ac[3] = {0.0f, 0.0f, 0.0f};

float axm_f = 0.0f;
float axm_f_old = 0.0f;
float u3aym_f = 0.0f;
float u3aym_f_old = 0.0f;
float u2azm_f = 0.0f;
float u2azm_f_old = 0.0f;

float aym_f = 0.0f;
float aym_f_old = 0.0f;
float u3axm_f = 0.0f;
float u3axm_f_old = 0.0f;
float u1azm_f = 0.0f;
float u1azm_f_old = 0.0f;

float azm_f = 0.0f;
float azm_f_old = 0.0f;
float u2axm_f = 0.0f;
float u2axm_f_old = 0.0f;
float u1aym_f = 0.0f;
float u1aym_f_old = 0.0f;

float x1_hat = 0.0f;
float x2_hat = 0.0f;
float x3_hat = 0.0f;
float sinroll = 0.0f;
float cosroll = 0.0f;
float roll_angle = 0.0f;
float roll_angle_old = 0.0f;
float droll_angle= 0.0f;
float droll_angle_old= 0.0f;
float sinpitch = 0.0f;
float cospitch = 0.0f;
float yaw_ref = 0.0f;
float yaw_angle = 0.0f;
float yaw_angle_old = 0.0f;
float dyaw_angle = 0.0f;
float dyaw_angle_old = 0.0f;
// ***************************************************************************************************************************************************************************
//                                                                              Low-Pass Filter
float lpf(float input,float input_old,float frequency)
{
    float output = 0;
    
    //y[n] = a*y[n-1] + (1 - a)*x[n]
    output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
    
    return output;
}// Low-Pass Filter

// ***************************************************************************************************************************************************************************
//                                                                              x2_hat_estimat with finding roll angle
void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
{
    // x2_hat estimation
    aym_f = lpf(a_ym,aym_f_old,alpha);
    aym_f_old = aym_f;
    u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha); // w3axm
    u3axm_f_old = u3axm_f;
    u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha); // w1azm
    u1azm_f_old = u1azm_f;
    
    x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha; //THe Low-Pass Filter(alpha/(s+alpha)) have already been done  
      
   // Finding the roll angle and giving the limit for it
    sinroll = x2_hat*(-0.1020); // This is for finding the roll angle, because x2_hat = -gsin(roll_angle). This is from paper Observer-Based Sensor Fusion.
    if(sinroll >= 1.0f)
    {
        sinroll = 1.0;
        cosroll = 0.0;
    }
    else if(sinroll <= -1.0f)
    {
        sinroll = -1.0;
        cosroll = 0.0;
    }
    else cosroll = sqrt(1-(sinroll*sinroll));
    roll_angle = asin(sinroll);///By 312 Rotation
//    roll_angle = lpf(roll_angle, roll_angle_old, 3);
//    roll_angle_old = roll_angle;
}// roll_fusion

// ***************************************************************************************************************************************************************************
//                                                                              absolute
float absolute(float value)
{
    if(value < 0)return -value;
    else return value;
}// absolute

// ***************************************************************************************************************************************************************************
//                                                                              Reset_data
void Reset_data(void)
{
    axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0;
    aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0;
    azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0;
}// Reset_data