File content as of revision 13:51ef67cd4fd6:

#include "SensorFusion.h"
#include "SPI_9dSensor.h"
#include "RobotBicycleConst.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 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;

///magnetometer's data
float mx_f = 0.0f;
float mx_f_old = 0.0f;
float u3my_f = 0.0f;
float u3my_f_old = 0.0f;
float u2mz_f = 0.0f;
float u2mz_f_old = 0.0f;

float my_f = 0.0f;
float my_f_old = 0.0f;
float u3mx_f = 0.0f;
float u3mx_f_old = 0.0f;
float u1mz_f = 0.0f;
float u1mz_f_old = 0.0f;

float mz_f = 0.0f;
float mz_f_old = 0.0f;
float u2mx_f = 0.0f;
float u2mx_f_old = 0.0f;
float u1my_f = 0.0f;
float u1my_f_old = 0.0f;

float mx1_hat = 0.0f;
float mx2_hat = 0.0f;
float mx3_hat = 0.0f;
float m_sinyaw = 0.0f;
float m_cosyaw = 0.0f;
float m_yaw_angle = 0.0f;

void get_9axis_data(unsigned char speed_state)
{
    Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2];
    if(speed_state == 3) Ac[1] = (-1.0) * v_high * Gyro_scale[2];
    else if(speed_state == 2) Ac[1] = (-1.0) * v_low * Gyro_scale[2];
    else Ac[1] = 0.0;///(-1.0) * Vx * Gyro_scale[2];///
    Ac[2] = 0.0;

    axm = Acce_scale[0] - Ac[0];
    aym = Acce_scale[1] - Ac[1];
    azm = Acce_scale[2];
    u1  = Gyro_scale[0];
    u2  = Gyro_scale[1];
    u3  = Gyro_scale[2];
    mx  = Magn_scale[0];
    my  = Magn_scale[1];
    mz  = Magn_scale[2]*(-1);
    B_total = sqrt(mx*mx+my*my+mz*mz);
}

float lpf(float input,float input_old,float frequency)
{
    float output = 0;
    output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
    return output;
}

void x1_hat_estimat(float a_xm,float a_ym,float a_zm,float u_3,float u_2,float alpha)
{
    axm_f = lpf(a_xm,axm_f_old,alpha);
    axm_f_old = axm_f;
    u3aym_f = lpf(u_3*a_ym,u3aym_f_old,alpha);
    u3aym_f_old = u3aym_f;
    u2azm_f = lpf(u_2*a_zm,u2azm_f_old,alpha);
    u2azm_f_old = u2azm_f;

    x1_hat = axm_f + u3aym_f/alpha - u2azm_f/alpha;
}

void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
{
    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);
    u3axm_f_old = u3axm_f;
    u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha);
    u1azm_f_old = u1azm_f;

    x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha;
    sinroll = x2_hat*(-0.1020);
    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
}

void x3_hat_estimat(float a_xm,float a_ym,float a_zm,float u_2,float u_1,float alpha)
{
    u2axm_f = lpf(u_2*a_xm,u2axm_f_old,alpha);
    u2axm_f_old = u2axm_f;
    u1aym_f = lpf(u_1*a_ym,u1aym_f_old,alpha);
    u1aym_f_old = u1aym_f;
    azm_f = lpf(a_zm,azm_f_old,alpha);
    azm_f_old = azm_f;

    x3_hat = u2axm_f/alpha - u1aym_f/alpha + azm_f;
}

void m_x1_hat(float m_x, float m_y,float m_z,float u_3,float u_2,float alpha)
{
    mx_f = lpf(m_x,mx_f_old,alpha);
    mx_f_old = mx_f;
    u3my_f = lpf(u_3*m_y,u3my_f_old,alpha);
    u3my_f_old = u3my_f;
    u2mz_f = lpf(u_2*m_z,u2mz_f_old,alpha);
    u2mz_f_old = u2mz_f;

    mx1_hat = (mx_f + u3my_f/alpha - u2mz_f/alpha)/B_total;
}

void m_x2_hat(float m_x,float m_y,float m_z,float u_3,float u_1,float alpha)
{
    my_f = lpf(m_y,my_f_old,alpha);
    my_f_old = my_f;
    u3mx_f = lpf(u_3*m_x,u3mx_f_old,alpha);
    u3mx_f_old = u3mx_f;
    u1mz_f = lpf(u_1*m_z,u1mz_f_old,alpha);
    u1mz_f_old = u1mz_f;

    mx2_hat = (my_f - u3mx_f/alpha + u1mz_f/alpha)/B_total;
}

void m_x3_hat(float m_x,float m_y,float m_z,float u_2,float u_1,float alpha)
{
    u2mx_f = lpf(u_2*m_x,u2mx_f_old,alpha);
    u2mx_f_old = u2mx_f;
    u1my_f = lpf(u_1*m_y,u1my_f_old,alpha);
    u1my_f_old = u1my_f;
    mz_f = lpf(m_z,mz_f_old,alpha);
    mz_f_old = mz_f;

    mx3_hat = (u2mx_f/alpha - u1my_f/alpha + mz_f)/B_total;
}

void m_yaw_fusion(void)
{
    float delta, delta_1, delta_2;

    delta = Determinant((B_x*cospitch+B_y*sinpitch*sinroll), (B_y*cospitch-B_x*sinpitch*sinroll), (B_y*cosroll), (-B_x*cosroll));
    delta_1 = Determinant((mx1_hat+B_z*sinpitch*cosroll), (B_y*cospitch-B_x*sinpitch*sinroll), (mx2_hat-B_z*sinroll), (-B_x*cosroll));
    delta_2 = Determinant((B_x*cospitch+B_y*sinpitch*sinroll), (mx1_hat+B_z*sinpitch*cosroll), (B_y*cosroll), (mx2_hat-B_z*sinroll));
    ///B_y = 0
//    delta = Determinant((B_x*cospitch), (-B_x*sinpitch*sinroll), (0.0), (-B_x*cosroll));
//    delta_1 = Determinant((mx1_hat+B_z*sinpitch*cosroll), (-B_x*sinpitch*sinroll), (mx2_hat-B_z*sinroll), (-B_x*cosroll));
//    delta_2 = Determinant((B_x*cospitch), (mx1_hat+B_z*sinpitch*cosroll), (0.0), (mx2_hat-B_z*sinroll));

    m_cosyaw = delta_1/delta;
    m_sinyaw = delta_2/delta;
}

float Determinant(float x11, float x12, float x21, float x22)
{
    return (x11*x22 - x12*x21);
}

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

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;
}