Diff: SensorFusion.cpp

Revision:

0:830ddddc129f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorFusion.cpp	Thu Jun 09 09:46:38 2016 +0000
@@ -0,0 +1,229 @@
+#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;
+}