Diff: SENSOR_FUSION.cpp

Revision:

0:e493567c21ac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SENSOR_FUSION.cpp	Thu Apr 28 09:08:53 2016 +0000
@@ -0,0 +1,95 @@
+#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;
+    
+}
\ No newline at end of file