Revision 1:81a146dffd7a, committed 2017-06-27

Comitter:

roger5641

Date:

Tue Jun 27 07:37:44 2017 +0000

Parent:

0:56bfa7bd6f71

Commit message:

read_sensorFusion;

Changed in this revision

diff -r 56bfa7bd6f71 -r 81a146dffd7a main.cpp
--- a/main.cpp	Thu Oct 13 05:36:58 2016 +0000
+++ b/main.cpp	Tue Jun 27 07:37:44 2017 +0000
@@ -2,6 +2,9 @@
 #include "LSM9DS0.h"
 #include <math.h>
 
+#define pi  3.1415926
+#define g   9.81
+
 // timer 1
 #define LOOPTIME  1000                         // 1 ms
 unsigned long lastMilli = 0;
@@ -31,13 +34,21 @@
 int sensor_flag = 0;                 // sensor initial flag
 int sensor_count = 0;
 
-// sensor gain
+/*******************************Sensor***************************************/ 
 #define Gyro_gain_x   0.00113356568421052631578947368421        
 #define Gyro_gain_y   0.00113356568421052631578947368421           
 #define Gyro_gain_z   0.00113356568421052631578947368421  
-#define Acce_gain_x   -0.0019768904308522832538352805428  //-9.81/(max-zero)                
-#define Acce_gain_y   -0.00210816564158896459269414388347                 
-#define Acce_gain_z   -0.00237364674122154818035038507811 
+#define Acce_gain_x   -0.00165873343713498511154419826254  //-9.81/(max-zero)                
+#define Acce_gain_y   -0.00217691902504394899561081096519                 
+#define Acce_gain_z   -0.00236798520156680310444239205579 
+ 
+/*******************************Sensor***************************************/ 
+#define Gyro_gain_x   0.00113356568421052631578947368421        
+#define Gyro_gain_y   0.00113356568421052631578947368421           
+#define Gyro_gain_z   0.00113356568421052631578947368421  
+#define Acce_gain_x   -0.00165873343713498511154419826254  //-9.81/(max-zero)                
+#define Acce_gain_y   -0.00217691902504394899561081096519                 
+#define Acce_gain_z   -0.00236798520156680310444239205579 
  
 // 宣告從sensor讀到的值要存入處理的變數
 float Gyro_axis_data[3];       // X, Y, Z axis
@@ -50,7 +61,10 @@
 float Acce_axis_data_f[3];
 float Acce_axis_data_f_old[3];
 float Acce_scale[3];           // scale = (data - zero) * gain
-float Acce_axis_zero[3] = {-802.8320503,-460.282926875,4.717775229};
+float Acce_axis_zero[3] = {-840.873762375,-314.556930675,0.50990098975};
+//-840.45049505  -395.32178215  4143.272277
+//-841.2970297   4191.811881    1.3762376235
+//5073.277228    -233.7920792   -0.356435644
 
 // final sensor output value
 float axm = 0;    
@@ -59,7 +73,6 @@
 float u1 = 0;     
 float u2 = 0;     
 float u3 = 0;
-
 // new defined direction
 float ax = 0;
 float ay = 0;
@@ -67,7 +80,6 @@
 float w1 = 0;
 float w2 = 0;
 float w3 = 0;
- 
 // estimated state variables
 float gs1_hat = 0;
 float gs1_hat_old = 0;
@@ -80,47 +92,77 @@
 float gs1_hat_n = 0;
 float gs2_hat_n = 0;
 float gs3_hat_n = 0;
+//general variables
+float gs1_hat_g = 0;
+float gs2_hat_g = 0;
+float gs3_hat_g = 0;
+
 // bandwidth
 float alpha = 6.28;     // 1Hz
-
-/********************************************************************************/
-//Variable(s) state 
-float            gama  =   0.0;      //Roll Angle
-float         gama_old =   0.0;
-float           gama_f =   0.0;
-float       gama_f_old =   0.0;
-float           Igama  =   0.0;      //
-float          Igama_f =   0.0;
-float      Igama_f_old =   0.0;
-float           dgama  =   0.0;      //Roll Angle
-float        dgama_old =   0.0;
-float          dgama_f =   0.0;
-float      dgama_f_old =   0.0;
+float psi = 0.0;          // pitch angle
  
-float            roll  =   0.0;      //Roll Angle
-float         roll_old =   0.0;
-float           roll_f =   0.0;
-float       roll_f_old =   0.0;
-float           Iroll  =   0.0;      //
-float          Iroll_f =   0.0;
-float      Iroll_f_old =   0.0;
-float           droll  =   0.0;      //Roll Angle
-float        droll_old =   0.0;
-float          droll_f =   0.0;
-float      droll_f_old =   0.0;
- 
-float            pitch  =   0.0;      //Roll Angle
-float         pitch_old =   0.0;
-float           pitch_f =   0.0;
-float       pitch_f_old =   0.0;
-float           Ipitch  =   0.0;      //
-float          Ipitch_f =   0.0;
-float      Ipitch_f_old =   0.0;
-float           dpitch  =   0.0;      //Roll Angle
-float        dpitch_old =   0.0;
-float          dpitch_f =   0.0;
-float      dpitch_f_old =   0.0; 
- 
+//Sensor Fusion 
+//****************theta Angle****************************// 
+float theta = 0.0;        // theta angle
+float theta_old = 0.0;
+float Itheta = 0.0;
+float Itheta_old = 0.0;
+float dtheta = 0.0;
+float dtheta_old = 0.0;
+float dtheta_f = 0.0;
+float dtheta_f_old = 0.0;
+
+//****************phi Angle****************************// 
+float phi = 0.0;        // theta angle
+float phi_old = 0.0;
+float Iphi = 0.0;
+float Iphi_old = 0.0;
+float dphi = 0.0;
+float dphi_old = 0.0;
+float dphi_f = 0.0;
+float dphi_f_old = 0.0;
+
+//****************roll Angle****************************// 
+float roll = 0.0;      
+float roll_old = 0.0;
+float roll_f = 0.0;
+float roll_f_old = 0.0;
+float Iroll = 0.0;      //
+float Iroll_f = 0.0;
+float Iroll_f_old = 0.0;
+float droll = 0.0;      //roll Angle
+float droll_old = 0.0;
+float droll_f = 0.0;
+float droll_f_old = 0.0;
+
+//float roll_1 = 0.0;
+//****************roll Angle without theta ****************************// 
+float gama = 0.0;      //Roll Angle
+float gama_old = 0.0;
+float gama_f = 0.0;
+float gama_f_old = 0.0;
+float Igama  = 0.0;      
+float Igama_f = 0.0;
+float Igama_f_old = 0.0;
+float dgama  = 0.0;      
+float dgama_old = 0.0;
+float dgama_f = 0.0;
+float dgama_f_old = 0.0;
+
+//****************pitch Angle****************************// 
+float pitch = 0.0;      //pitch Angle
+float pitch_old = 0.0;
+float pitch_f = 0.0;
+float pitch_f_old = 0.0;
+float Ipitch = 0.0;      //
+float Ipitch_f = 0.0;
+float Ipitch_f_old = 0.0;
+float dpitch = 0.0;      //pitch Angle
+float dpitch_old = 0.0;
+float dpitch_f = 0.0;
+float dpitch_f_old = 0.0;
+float pitch_0 = 0.0;  //
+
 int main()
 {
     t.start();
@@ -277,8 +319,8 @@
     //                mag_odr mODR = M_ODR_100);
 }
 
-void sensor_fusion(void){    
-    // sensor fusion
+void sensor_fusion(void)
+{    
     gs1_hat = lpf(ax + w3*ay/alpha - w2*az/alpha , gs1_hat_old , alpha);
     gs1_hat_old = gs1_hat;
     gs2_hat = lpf(-w3*ax/alpha + ay + w1*az/alpha , gs2_hat_old , alpha);
@@ -287,35 +329,48 @@
     gs3_hat_old = gs3_hat;
     
     n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
-    gs1_hat_n = (gs1_hat / n) * 9.81;
-    gs2_hat_n = (gs2_hat / n) * 9.81;
-    gs3_hat_n = (gs3_hat / n) * 9.81;
+    gs1_hat_n = (gs1_hat / n) * g;
+    gs2_hat_n = (gs2_hat / n) * g;
+    gs3_hat_n = (gs3_hat / n) * g;
     
-    pitch = asin(gs1_hat_n/9.81);
-    roll = atan(gs2_hat_n / gs3_hat_n);
+    gs1_hat_g = cos(theta)*gs1_hat_n-sin(theta)*gs2_hat_n;
+    gs2_hat_g = cos(phi)*sin(theta)*gs1_hat_n + cos(phi)*cos(theta)*gs2_hat_n - sin(phi)*gs3_hat_n;
+    gs3_hat_g = sin(phi)*sin(theta)*gs1_hat_n + sin(phi)*cos(theta)*gs2_hat_n - cos(phi)*gs3_hat_n;
+    
+    pitch = asin(gs1_hat_g/g);
+    roll = atan(gs2_hat_g/gs3_hat_g);
+    gama = atan(gs2_hat_n / gs3_hat_n);//+0.0157;
+    pitch_0 = asin(-gs1_hat_n/g);
     
     pitch_f = lpf(pitch, pitch_f_old, 1.0);
     pitch_f_old = pitch_f;
-    Ipitch = Ipitch + pitch_f*0.01;
+    Ipitch = Ipitch + pitch_f*T;
     Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0);
     Ipitch_f_old = Ipitch_f;
-    dpitch = (pitch - dpitch_old)/0.01;
-    dpitch_old = pitch;
-    
+    dpitch = (pitch - dpitch_old)/T;
+    dpitch_old = pitch;    
     dpitch_f = lpf(dpitch, dpitch_f_old, 1.0);
     dpitch_f_old = dpitch_f;
     
     roll_f = lpf(roll, roll_f_old, 1.0);
     roll_f_old = roll_f;
-    Iroll = Iroll + roll_f*0.01;
+    Iroll = Iroll + roll_f*T;
     Iroll_f = lpf(Iroll, Iroll_f_old, 18.0);
     Iroll_f_old = Iroll_f;
-    droll = (roll - droll_old)/0.01;
-    droll_old = roll;
+    droll = (roll - droll_old)/T;
+    droll_old = roll;   
+    droll_f = lpf(droll, droll_f_old, 1.0);
+    droll_f_old = droll_f;
     
-    droll_f = lpf(droll, droll_f_old, 1.0);
-    droll_f_old = droll_f;    
-    
+    gama_f = lpf(gama, gama_f_old, 1.0);
+    gama_f_old = gama_f;
+    Igama = Igama + gama_f*T;
+    Igama_f = lpf(Igama, Igama_f_old, 18.0);
+    Igama_f_old = Igama_f;
+    dgama = (gama - dgama_old)/T;
+    dgama_old = gama;   
+    dgama_f = lpf(dgama, dgama_f_old, 1.0);
+    dgama_f_old = dgama_f;
       
 }