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
Revision 2:307500c991dd, committed 2012-04-21
- Comitter:
- higedura
- Date:
- Sat Apr 21 14:07:27 2012 +0000
- Parent:
- 1:8048a8bcde59
- Commit message:
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Apr 15 11:55:19 2012 +0000
+++ b/main.cpp Sat Apr 21 14:07:27 2012 +0000
@@ -70,7 +70,7 @@
int navi_loop = 10;
int xbee_loop = 100;
- double dt_wait = 0.00064;
+ double dt_wait = 0.00043;
//double dt_wait = 0.01;
double dt = 0.01;
double t = 0;
@@ -81,7 +81,7 @@
// Calibration routine
double calib_acc[N] = {0};
double calib_gyro[N] = {0};
- double compass_basis_rad = 0;
+ //double compass_basis_rad = 0;
// Getting data
double acc[N] = {0, 0, 1};
@@ -89,15 +89,15 @@
double d_acc[N] = {0};
double gyro_deg[N] = {0};
double gyro_rad[N] = {0};
- int mag[N] = {0};
+ //int mag[N] = {0};
// Measurement algorithm
- double angle_acc[2] = {0};
+ //double angle_acc[2] = {0};
double angle_deg[N] = {0};
double angle_rad[N] = {0};
double zLWMA[N] = {0};
- double compass_rad = 0;
- double compass_deg = 0;
+ //double compass_rad = 0;
+ //double compass_deg = 0;
// Gravity z
for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
@@ -116,7 +116,15 @@
{ 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=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}};
+*/
// 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},
@@ -125,34 +133,61 @@
{ 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=50, q4=50, q5=1)
+ double Kr[M][L] = {{ 0, 5.774, 0, 4.309, 0.408},
+ {-5, 2.887,-3.732, 2.155,-0.408},
+ {-5,-2.887,-3.732,-2.155, 0.408},
+ { 0,-5.774, 0,-4.309,-0.408},
+ { 5,-2.887, 3.732,-2.155, 0.408},
+ { 5, 2.887, 3.732, 2.155,-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},
+/* 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}};
+*/
+ // Gain (q1=300, q2=300, q3=100, q4=100, q5=1)
+/* double Kr[M][L] = {{ 0, 10, 0, 6.052, 0.408},
+ {-8.66, 5,-5.242, 3.026,-0.408},
+ {-8.66, -5,-5.242,-3.026, 0.408},
+ { 0,-10, 0,-6.052,-0.408},
+ { 8.66, -5, 5.242,-3.026, 0.408},
+ { 8.66, 5, 5.242, 3.026,-0.408}};
+*/
+ // Gain (q1=400, q2=400, q3=100, q4=100, q5=1)
+/* double Kr[M][L] = {{ 0, 11.547, 0, 6.094, 0.408},
+ {-10, 5.774,-5.278, 3.047,-0.408},
+ {-10, -5.774,-5.278,-3.047, 0.408},
+ { 0,-11.547, 0,-6.094,-0.408},
+ { 10, -5.774, 5.278,-3.047, 0.408},
+ { 10, 5.774, 5.278, 3.047,-0.408}};
*/
+ // Gain (q1=500, q2=500, q3=50, q4=50, q5=1)
+/* double Kr[M][L] = {{ 0, 12.910, 0, 4.574, 0.408},
+ {-11.18, 6.455,-3.962, 2.287,-0.408},
+ {-11.18, -6.455,-3.962,-2.287, 0.408},
+ { 0,-12.910, 0,-4.574,-0.408},
+ { 11.18, -6.455, 3.962,-2.287, 0.408},
+ { 11.18, 6.455, 3.962, 2.287,-0.408}};
+*/
+ // Gain (q1=500, q2=500, q3=100, q4=100, q5=1)
+/* double Kr[M][L] = {{ 0, 12.910, 0, 6.131, 0.408},
+ {-11.18, 6.455,-5.31, 3.066,-0.408},
+ {-11.18, -6.455,-5.31,-3.066, 0.408},
+ { 0,-12.910, 0,-6.131,-0.408},
+ { 11.18, -6.455, 5.31,-3.066, 0.408},
+ { 11.18, 6.455, 5.31, 3.066,-0.408}};
+*/
+ double Ccom_r = 0.2;
+ double Ccom_p = 0.2;
+ double Ccom_y = 0.1;
+
double motor[M] = {0};
- double motor_attitude[M] = {0}; // Pulth width by the attitude control
- double thrust[M];
+ double motor_control[M] = {0}; // Pulth width generated by the transmitter
+ double motor_attitude[M] = {0}; // Pulth width generated by the attitude control
// Coefficient of transfering Force to Puls
double Cfp = 2*pow(10.0, -7);
@@ -231,7 +266,7 @@
p_calib = calib();
for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
- compass_basis_rad = *p_calib;
+ //compass_basis_rad = *p_calib;
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
@@ -253,7 +288,9 @@
command_buf2[i] = (int)command_buf1[i]-48;
if( command_buf2[i]==-16 ){ command_buf2[i]=0; }
}
- for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; }
+ //for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; }
+ command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001;
+ for( int i=1;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]-0.0005; }
navi_flag = 0;
navi_loop = 0;
@@ -282,7 +319,7 @@
gyro_deg[1] = gyro.getGyroY();
gyro_deg[2] = gyro.getGyroZ();
*/
- for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; }
+ //for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; }
// Low pass filter for acc
//if( -0.05<acc[0] && acc[0]<0.05 ){ acc[0]=0; }
@@ -341,7 +378,14 @@
for( int j=0;j<L;j++ ){ motor_attitude[i] += -Kr[i][j]*state[j]; }
}
- for( int i=0;i<M;i++ ){ motor[i] = command[0]+motor_attitude[i]*Cfp; }
+ motor_control[0] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]+1*Ccom_y*command[3];
+ motor_control[1] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3];
+ motor_control[2] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3];
+ motor_control[3] = command[0]+0*Ccom_r*command[1]+2*Ccom_p*command[2]-1*Ccom_y*command[3];
+ motor_control[4] = command[0]-1*Ccom_r*command[1]-1*Ccom_p*command[2]+1*Ccom_y*command[3];
+ motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3];
+
+ for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; }
//for( int i=0;i<M;i++ ){ motor[i] = command[0]; }
// pulsewidth 0.0011~0.0019 (0.001~0.002?)
@@ -353,7 +397,7 @@
ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]);
- for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; }
+ //for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; }
//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);