yal kaiyo
for hige
Fork of
YNU_MPU_1
by 
yal kaiyo
Diff: main.cpp
- Revision:
- 17:3cd1afc27ca7
- Parent:
- 16:a33fe7d99a0f
diff -r a33fe7d99a0f -r 3cd1afc27ca7 main.cpp
--- a/main.cpp Wed Jan 16 13:04:38 2013 +0000
+++ b/main.cpp Sun Jan 20 13:38:22 2013 +0000
@@ -36,8 +36,8 @@
int flag_lc = 0;
int flag_cm = 0;
- int loop = 100; // for 2 minuts -> loop = 1200
-
+ int loop = 1200; // for 2 minuts -> loop = 1200
+
char buf_char;
unsigned int buf_hex_rx[30] = {0};
unsigned int buf_dec_rx[10] = {0};
@@ -69,14 +69,16 @@
// ******************** HOSHINO *********************
float time = 0; //time
float flag_int= 0;
+ float flag_safe= 0;
+ float flag_homo= 0;
//initial condition
float drf = 100; //syuutandaunrenji
float ht0 = 0;
float gm0 = 20*pi/180;
float ua = 1; //initial horizontal velosity
//terminal condition
- float htf = 5;
- float gmf = 5*pi/180;
+ float htf = 2;
+ float gmf = -20*pi/180;
float xis = 0;
float ets = 0;
@@ -89,7 +91,7 @@
float b2 = 0;
float b3 = 0;
float htc = 0;
- float x = 0; //down range
+ // float x = 0; //down range
float xis1 = 0;
float ets1 = 0;
float xir = 0;
@@ -115,7 +117,7 @@
float etc = 0;
float xic = 0;
float apc = 0;
-
+
//homotopy
//initial condition
float rps0 = 0.0;
@@ -127,8 +129,8 @@
float et0 = ret0/drf;
float eps = 0.00000001;
float ueps = 0.000001;
- float ier = 0.0;
- float sum = 0.0;
+ float ier = 0;
+ float sum = 0;
float ph0[mh] = {0};
float phi[mh] = {0};
float fn[mh] = {0};
@@ -145,15 +147,19 @@
float qmax=0;
float rdet =1;
float idet =0;
- float t = 0.0;
+ float t = 0;
float phf[mh]={0};
- float ap[mt]={0};
- float ps[mt]={0};
- float xi[mt]={0};
- float et[mt]={0};
- float xi1[mt]={0};
- float et1[mt]={0};
-
+ float ap[mt]={0, 2.718, -1.679, -4.821, -2.386, 2.22154, 0};//1-3, 2-2
+ //
+ float ps[mt]={1.099, 1.319, 1.371, 0.670, -0.103, -0.171, 0};//1-3, 2-2
+ //
+ float xi[mt]={0, 10.28, 17.25, 30.60, 54.83, 80.37, 100};//1-3, 2-2
+ //
+ float et[mt]={0, 23.12, 46.73, 67.32, 74.76, 72.87, 70};//1-3, 2-2
+ //
+ float xi1[mt]={0};
+ float et1[mt]={0};
+
//********************* HOSHINO *********************
//********************* OHTSU ***********************
@@ -163,15 +169,15 @@
float wgt = 0.76;
float rho = 1.2255;
- float tt1 = 0.8;
- float omg1 = 1.0;
- float zet1 = 0.7;
- float tt2 = 0.37;
- float omg2 = 2.6;
+ // float tt1 = 0.8;
+ float omg1 = 1.2;
+ float zet1 = 0.5;
+ float tt2 = 2.0;
+ float omg2 = 1.0;
float zet2 = 0.7;
- float gmr = 20;
- float vmr = 16;
+ float gmr = -20;
+ float vmr = 20;
float pse = 0;
float phir = 0;
float ete = 0;
@@ -199,6 +205,7 @@
float avpc = 0;
float avqc = 0;
float avrc = 0;
+
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
@@ -213,12 +220,49 @@
// // flag_lc==1 is debug mode. skip this while loop. //
// ////////// for flight mode, flag_lc is 0! ///////////
// /////////////////////////////////////////////////////
- while(flag_lc==1){
+ while(flag_lc==1){ // if you want to use @LC, 1 -> 2
buf_char = mavc.getc();
buf_hex_rx[0] = (unsigned int)buf_char;
for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){
- // ******************** waiting @LC ********************
+ // ******************** waiting @LC ********************
+
+ ///////////////////
+ flag_lc = 1; // end of @LC flag
+
+ }
+ }
+
+ while(flag_cm<loop){
+
+ // ******************** sequence No.1 ********************
+ buf_char = mavc.getc();
+ buf_hex_rx[0] = (unsigned int)buf_char;
+ for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
+ if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x43 && buf_hex_rx[27]==0x4D && buf_hex_rx[26]==0x0D){
+ // ******************** sequence No.1 ********************
+
+ // ******************** sequence No.2 ********************
+ // send to mavc
+ mavc.putc(H);
+ for(int i=0;i<data;i++){ mavc.putc(D[i]); }
+ // ******************** sequence No.2 ********************
+
+ // ******************** sequence No.3 ********************
+ // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
+ // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
+ // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
+ // transrating hex to dec
+ for(int i=0;i<10;i++){ buf_dec_rx[i] = buf_hex_rx[24-i*2]*256+buf_hex_rx[23-i*2]; }
+ for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec_rx[i]-32768)/65535*20*9.8; }
+ for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec_rx[i+3]-32768)/65535*600/180*pi; }
+ azi = (double)buf_dec_rx[6]/65535*2*pi;
+ alt = ((double)buf_dec_rx[7]-32768)*0.1;
+ for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec_rx[i+8]-32768)*0.1; }
+ // ******************** sequence No.3 ********************
+
+ // ***************** horizontal guidance *****************//1-4, 2-3
+ if(flag_homo == 0 ) {
// ******************** homotopy ********************
@@ -439,7 +483,7 @@
}
}
- //////function
+ /////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
@@ -1075,40 +1119,13 @@
led1 = 1;
wait(1.0);
- ///////////////////
- flag_lc = 1; // end of homotopy flag
+ /////////////
+ flag_homo =1;
+ }
// ******************** homotopy ********************
- }
- }
-
- while(flag_cm<loop){
-
- // ******************** sequence No.1 ********************
- buf_char = mavc.getc();
- buf_hex_rx[0] = (unsigned int)buf_char;
- for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
- if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x43 && buf_hex_rx[27]==0x4D && buf_hex_rx[26]==0x0D){
- // ******************** sequence No.1 ********************
+ // ***************** horizontal guidance *****************
- // ******************** sequence No.2 ********************
- // send to mavc
- mavc.putc(H);
- for(int i=0;i<data;i++){ mavc.putc(D[i]); }
- // ******************** sequence No.2 ********************
-
- // ******************** sequence No.3 ********************
- // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
- // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
- // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
- // transrating hex to dec
- for(int i=0;i<10;i++){ buf_dec_rx[i] = buf_hex_rx[24-i*2]*256+buf_hex_rx[23-i*2]; }
- for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec_rx[i]-32768)/65535*20*9.8; }
- for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec_rx[i+3]-32768)/65535*600/180*pi; }
- azi = (double)buf_dec_rx[6]/65535*2*pi;
- alt = ((double)buf_dec_rx[7]-32768)*0.1;
- for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec_rx[i+8]-32768)*0.1; }
- // ******************** sequence No.3 ********************
// ******************** initialization *******************
for(int i=0;i<mt;i++){
@@ -1119,6 +1136,12 @@
y0 = GPS[1];
ps0 = azi;
ht0 = alt;
+ if (alt-50<2){
+ htf=2;
+ }
+ else{
+ htf = alt-50;
+ }
b0 = ht0;
b1 = tan(gm0);
b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
@@ -1133,7 +1156,7 @@
avq = gyro[1];
avr = gyro[2];
// ******************** initialization *******************
-
+
// ******************** interpolation ********************
if (fabs(xis- xis1)>1 && fabs(xis-xis1)<50) {
@@ -1154,15 +1177,16 @@
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
-
- /*
+
+ /* //1-2,1-3,1-4
b0 = ht0
b1 = (hf-ht0)/drf
+ htc = b0 + b1*drp
*/
- /*
+
htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
- for(int i=0;i<mt;i++){
+ for(int i=0;i<mt;i++){ //1-3,1-4,2-2,2-3
c = drp-dr[i];
c1= dr[i+1]-drp;
@@ -1176,16 +1200,17 @@
break;
}
}
- */
+ /*
htc = ht0;
xic = 0;
etc = 0;
+ */
// ******************** guidance law WO homo *************
// ******************** control law **********************
- xie=xic-xir;
- pse=(psc-psr)*dtr;
- ete=etc-etr;
+ pse=(psr-psc);
+ xie=cos(psc)*(xir-xic)+sin(psc)*(etr-etc);
+ ete=-sin(psc)*(xir-xic)+cos(psc)*(etr-etc);
hte=htc-htr;
//horizontal
@@ -1217,12 +1242,15 @@
com[1] = avqc;
com[2] = avrc;
- if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
+ //safemode
+
+ if(fabs(hte) > 12 || fabs(xie) > 16 || fabs(ete) >16 || flag_safe==1){
com[0] = 0;
com[1] = 0;
com[2] = 0;
+ flag_safe = 1;
}
-
+
// transrating dec to hex
for(int i=0;i<3;i++){
buf_dec_tx[i] = 19660.5/pi*com[i]+32768;