hige dura
/
dotHR_EKF
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: FatFileSystem mbed
Diff: main.cpp
- Revision:
- 1:8048a8bcde59
- Parent:
- 0:813b917360ac
- Child:
- 2:307500c991dd
--- a/main.cpp Mon Apr 02 12:22:30 2012 +0000
+++ b/main.cpp Sun Apr 15 11:55:19 2012 +0000
@@ -16,7 +16,6 @@
ITG3200 gyro(p9, p10);
HMC5883L compass(p9, p10);
Serial xbee(p13, p14);
-AnalogIn alt_in(p19);
DigitalIn stop(p20);
PwmOut ESC1(p21);
PwmOut ESC2(p22);
@@ -64,14 +63,14 @@
while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; }
}
- int correct_period = 1000;
+ int correct_period = 100;
int navi_period = 10;
int xbee_period = 100;
- int correct_loop = -2000;
+ int correct_loop = 0;
int navi_loop = 10;
int xbee_loop = 100;
- double dt_wait = 0.0019;
+ double dt_wait = 0.00064;
//double dt_wait = 0.01;
double dt = 0.01;
double t = 0;
@@ -110,33 +109,53 @@
// Optimal regulator
double state[L];
- // Gain
- double Kr[M][L] = {{ 0, 5.774, 0, 2.288, 0.408},
- { -5, 2.887,-1.982, 1.144,-0.408},
- { -5,-2.887,-1.982,-1.144, 0.408},
- { 0,-5.774, 0,-2.288,-0.408},
- { 5,-2.887, 1.982,-1.144, 0.408},
- { 5, 2.887, 1.982, 1.144,-0.408}};
+ // Gain (q1=10, q2=10, q3=10, q4=10, q5=1)
+/* double Kr[M][L] = {{ 0, 1.826, 0, 1.984, 0.408},
+ {-1.581, 0.913,-1.718, 0.992,-0.408},
+ {-1.581,-0.913,-1.718,-0.992, 0.408},
+ { 0,-1.826, 0,-1.984,-0.408},
+ { 1.581,-0.913, 1.718,-0.992, 0.408},
+ { 1.581, 0.913, 1.718, 0.992,-0.408}};
+*/
+ // Gain (q1=100, q2=100, q3=10, q4=10, q5=1)
+/* double Kr[M][L] = {{ 0, 5.774, 0, 2.288, 0.408},
+ {-5, 2.887,-1.982, 1.144,-0.408},
+ {-5,-2.887,-1.982,-1.144, 0.408},
+ { 0,-5.774, 0,-2.288,-0.408},
+ { 5,-2.887, 1.982,-1.144, 0.408},
+ { 5, 2.887, 1.982, 1.144,-0.408}};
+*/
+ // Gain (q1=1000, q2=1000, q3=10, q4=10, q5=1)
+/* double Kr[M][L] = {{ 0, 18.257, 0, 3.058, 0.408},
+ {-15.811, 9.129,-2.65, 1.529,-0.408},
+ {-15.811, -9.129,-2.65,-1.529, 0.408},
+ { 0,-18.257, 0,-3.058,-0.408},
+ { 15.811, -9.129, 2.65,-1.529, 0.408},
+ { 15.811, 9.129, 2.65, 1.529,-0.408}};
+*/
+ // Gain (q1=100, q2=100, q3=100, q4=100, q5=1)
+ double Kr[M][L] = {{ 0, 5.774, 0, 5.936, 0.408},
+ {-5, 2.887,-5.141, 2.968,-0.408},
+ {-5,-2.887,-5.141,-2.968, 0.408},
+ { 0,-5.774, 0,-5.936,-0.408},
+ { 5,-2.887, 5.141,-2.968, 0.408},
+ { 5, 2.887, 5.141, 2.968,-0.408}};
+
+ // Gain (q1=100, q2=100, q3=1, q4=1, q5=1)
+/* double Kr[M][L] = {{ 0, 5.774, 0, 1.496, 0.408},
+ {-5, 2.887,-1.296, 0.748,-0.408},
+ {-5,-2.887,-1.296,-0.748, 0.408},
+ { 0,-5.774, 0,-1.496,-0.408},
+ { 5,-2.887, 1.296,-0.748, 0.408},
+ { 5, 2.887, 1.296, 0.748,-0.408}};
+*/
double motor[M] = {0};
double motor_attitude[M] = {0}; // Pulth width by the attitude control
double thrust[M];
- // Hovering puls width
- //double hover = 0.0015;
-
// Coefficient of transfering Force to Puls
- //double Cfp = 1/(1.4*pow(10.0, 6));
double Cfp = 2*pow(10.0, -7);
- // Coefficient of transfering altitude to Puls
- double Calt = 1/(1.4*pow(10.0, 5));
-
- // Keeping altitude
- double alt;
- double alt_r = 1000; // Altitude reference 1m
- double alt_keep;
- double Kp_alt = 1;
- double sensor_h = 300; // Hight of sensor
// Navigation
char command_buf1[chan_buf] = {0};
@@ -218,8 +237,8 @@
//pc.printf("Starting IMU unit\n\r");
xbee.printf("Starting IMU unit\n\r");
- //pc.printf(" time phi the P Q R accX accY accZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
- xbee.printf(" time phi the P Q R accX accY accZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
+ //pc.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
+ xbee.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
//while(1){
while( stop==0 ){
@@ -246,7 +265,6 @@
// Updating accelerometer and compass
accelerometer.getOutput(bit_acc);
compass.readData(get_mag);
- alt = alt_in/0.000074;
for( int i=0;i<N;i++ ){ acc0[i] = acc[i]; }
@@ -258,7 +276,12 @@
gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0);
gyro_deg[1] = (-(gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); // Modify the differencial of the sensor axis
gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
-
+/*
+ for( int i=0;i<N;i++ ){ acc[i] = (int16_t)bit_acc[i]; }
+ gyro_deg[0] = gyro.getGyroX();
+ gyro_deg[1] = gyro.getGyroY();
+ gyro_deg[2] = gyro.getGyroZ();
+*/
for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; }
// Low pass filter for acc
@@ -308,11 +331,7 @@
}
correct_loop++;
- // Caribrating EKF
for( int i=0;i<N;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; }
-
- alt_keep = -Kp_alt*(alt-alt_r-sensor_h)*Calt;
- //alt_keep = 0;
state[0] = angle_deg[0]; state[1] = angle_deg[1];
state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2];
@@ -323,6 +342,7 @@
}
for( int i=0;i<M;i++ ){ motor[i] = command[0]+motor_attitude[i]*Cfp; }
+ //for( int i=0;i<M;i++ ){ motor[i] = command[0]; }
// pulsewidth 0.0011~0.0019 (0.001~0.002?)
for( int i=0;i<M;i++ ){
@@ -337,9 +357,10 @@
//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
//pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
- fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
+ //fprintf(fp,"%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %6.3f\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], command[0]*1000);
+ fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
if( xbee_loop>=xbee_period ){
- xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.1f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], alt, motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
+ xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
xbee_loop = 0;
}
xbee_loop++;
@@ -360,7 +381,7 @@
// P = F * P * F' + G * Q * G';
//double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
- double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
+ double Q[N][N] = { {5198, 0, 0}, {0, 5518, 0}, {0, 0, 5722} };
double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
@@ -407,7 +428,8 @@
// P = P - K * H * P
//double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
- double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
+ //double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
+ double R[N][N] = { {272528, 0, 0}, {0, 295812, 0}, {0, 0, 908451} };
double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};