Roger Weng
/
read_sensorFusion_data_BT
pure sensor fusion
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
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; }