Diff: SensorFusion.cpp

Revision:

0:bf9bf4b7625f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorFusion.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,113 @@
+#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
\ No newline at end of file