yal kaiyo
for hige
Fork of
YNU_MPU_1
by 
yal kaiyo
Diff: main.cpp
- Revision:
- 13:b444c425e194
- Parent:
- 12:33085c983354
- Child:
- 14:feea7e2a51b8
--- a/main.cpp Fri Jan 11 04:12:00 2013 +0000
+++ b/main.cpp Sun Jan 13 09:13:17 2013 +0000
@@ -36,7 +36,7 @@
int flag_lc = 0;
int flag_cm = 0;
- int loop = 1200; // for 2 minuts -> loop = 1200
+ int loop = 100; // for 2 minuts -> loop = 1200
char buf_char;
unsigned int buf_hex_rx[30] = {0};
@@ -68,49 +68,59 @@
// ******************** HOSHINO *********************
float time = 0; //time
+ float flag_int= 0;
+ //initial condition
+ float drf = 100; //syuutandaunrenji
+ float ht0 = 0;
+ float gm0 = 20*pi/180;
+ float ua = 10; //initial horizontal velosity
+ //terminal condition
+ float htf = 5;
+ float gmf = 5*pi/180;
+
float xis = 0;
float ets = 0;
float psis = 0;
float x0 = 0;
- float y0 = 0;
- float drf = 100; //syuutandaunrenji
- float ht0 = 50;
- float htf = 5;
- float gm0 = 20*pi/180;
- float gmf = 5*pi/180;
- float b0 = ht0;
- float b1 = tan(gm0);
- float b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
- float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
+ float y0 = 0;
+ float h0 = 0;
+ float b0 = 0;
+ float b1 = 0;
+ float b2 = 0;
+ float b3 = 0;
float htc = 0;
float x = 0; //down range
- float ua = 10; //initial horizontal velosity
float xis1 = 0;
float ets1 = 0;
float xir = 0;
float etr = 0;
float drc = 0;
- float drp = 0;
+ float drp = 0;
float dt = 0.1;
float psr = 0;
float htr = 0;
- float avp = 0;
- float avq = 0;
- float avr = 0;
- float s = 0;
- float c=0;
- float c0=0;
- float c1=0;
- float dr[mt]={0};
- float time1=0;
- float kp=0;
- float kq=0;
- float kr=0;
+ float avp = 0;
+ float avq = 0;
+ float avr = 0;
+ float s = 0;
+ float c = 0;
+ float c0 = 0;
+ float c1 = 0;
+ float dr[mt] = {0};
+ float time1 = 0;
+ float kp = 0;
+ float kq = 0;
+ float kr = 0;
+ float psc = 0;
+ float etc = 0;
+ float xic = 0;
//homotopy
+ //initial condition
float rps0 = 90.0;
float rxi0 = -60.0; //terminal xi
float ret0 = -20.0; //terminal et
+
float ps0 = rps0*dtr;
float xi0 = rxi0/drf;
float et0 = ret0/drf;
@@ -131,20 +141,18 @@
float fpi[mh][mh] = {0};
float x1[4][mt] = {0};
float rl[4][mt] = {0};
- float qmax=0.0;
- float rdet =1.0;
- float idet =0.0;
+ float qmax=0;
+ float rdet =1;
+ float idet =0;
float t = 0.0;
float phf[mh]={0};
- float ap[mt]={0.0};
- float ps[mt]={0.0};
- float xi[mt]={0.0};
- float et[mt]={0.0};
- float xi1[mt]={0.0};
- float et1[mt]={0.0};
- float psc;
- float etc;
- float xic;
+ float ap[mt]={0};
+ float ps[mt]={0};
+ float xi[mt]={0};
+ float et[mt]={0};
+ float xi1[mt]={0};
+ float et1[mt]={0};
+
//********************* HOSHINO *********************
//********************* OHTSU ***********************
@@ -1093,16 +1101,21 @@
for(int i=0;i<mt;i++){
dr[i]=i*drf/md;
}
- if(t == 0){
+ if(flag_int == 0){
x0 = GPS[0];
y0 = GPS[1];
ps0 = azi;
ht0 = alt;
+ b0 = ht0;
+ b1 = tan(gm0);
+ b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
+ b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
+ flag_int=1;
}
xis = (GPS[0] - x0)*cos(pi/2-ps0)-(GPS[1] - y0)*sin(pi/2-ps0);
ets = (GPS[1] - y0)*sin(pi/2-ps0)+(GPS[0] - x0)*cos(pi/2-ps0);
- psr = pi/2 -(azi - ps0);
- htr = 50+(alt-ht0);
+ psr = rps0*dtr -(azi - ps0);
+ htr = alt;
avp = gyro[0];
avq = gyro[1];
avr = gyro[2];
@@ -1126,6 +1139,12 @@
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
+
+ /*
+ b0 = ht0
+ b1 = (hf-ht0)/drf
+ */
+
htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
for(int i=0;i<mt;i++){
@@ -1156,7 +1175,7 @@
avpc=1/tt1*(gkd*pse+gkp*ete);//
*/
- avpc=kp*hte;
+ //avpc=kp*hte;
//vertical
/*
@@ -1168,14 +1187,14 @@
//avqc=(-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte)))*pi/180;
//hte00=hte;
- avqc=kq*(pse+ete);
+ //avqc=kq*(pse+ete);
// dummy output
- /*
+
avpc = avpc+0.001*pi;
avqc = 0;
avrc = avrc-0.001*pi;
- */
+
com[0] = avpc;
com[1] = avqc;
com[2] = avrc;