Kyohei Shinomoto
add homotopy
Fork of
YNU_MPUd2h
by 
hige dura
Diff: main.cpp
- Revision:
- 4:037fab837823
- Parent:
- 3:17236ad0ff1e
- Child:
- 5:192835ac6106
--- a/main.cpp Mon Jan 07 08:58:34 2013 +0000
+++ b/main.cpp Tue Jan 08 12:22:43 2013 +0000
@@ -34,6 +34,8 @@
//pc.baud(921600);
mavc.baud(115200);
+ int flag = 0;
+
char buf_char;
unsigned int buf_hex[30] = {0};
unsigned int buf_dec[10] = {0};
@@ -58,39 +60,41 @@
// ******************** HOSHINO *********************
float time = 0; //time
- float xs = 0;
- float ys = 0;
+ float xis = 0;
+ float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
- float psi0 = 0;
- float xf = 200; //syuutandaunrenji
- float h0 = 200;
- float hf = 10;
+ float drf = 800; //syuutandaunrenji
+ float ht0 = 200;
+ float htf = 10;
float gm0 = 20*pi/180;
float gmf = 5*pi/180;
- float b0 = h0;
+ float b0 = ht0;
float b1 = tan(gm0);
- float b2 = (3*(hf-h0)-xf*(2*tan(gm0)+tan(gmf)))/(xf*xf);
- float b3 = -(2*(hf-h0)-xf*(tan(gm0)+tan(gmf)))/(xf*xf*xf);
- float hr = 0;
- float x = 0;
+ 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 htc = 0;
+ float x = 0; //down range
float ua = 10; //initial horizontal velosity
- float xs1 = 0;
- float ys1 = 0;
- float xr = 0;
- float yr = 0;
+ float xis1 = 0;
+ float ets1 = 0;
+ float xir = 0;
+ float etr = 0;
float drc = 0;
float dr = 0;
- float dt = 0.1;
- float psi = 1;
+ float dt = 0.1;
+ float psr = 0;
+ float htr = 0;
+ float avp = 0;
+ float avq = 0;
+ float avr = 0;
float s = 0;
//homotopy
- float drf = 800.0;
float rps0 = 90.0;
- float rxi0 = -600.0;
- float ret0 = -200.0;
+ float rxi0 = -600.0; //terminal xi
+ float ret0 = -200.0; //terminal et
float ps0 = rps0*dtr;
float xi0 = rxi0/drf;
float et0 = ret0/drf;
@@ -98,33 +102,35 @@
float ueps = 0.000001;
float ier = 0.0;
float sum = 0.0;
- float ph0[mh] = {0.0};
- float phi[mh] = {0.0};
- float fn[mh] = {0.0};
- float wk[mh] = {0.0};
+ float ph0[mh] = {0};
+ float phi[mh] = {0};
+ float fn[mh] = {0};
+ float wk[mh] = {0};
int iwk[mh] = {0};
- float dph1[mh] = {0.0};
- float dph2[mh] = {0.0};
- float dph3[mh] = {0.0};
- float dph4[mh] = {0.0};
- float fph[mh][mh] = {0.0};
- float fpi[mh][mh] = {0.0};
- float x1[4][mt] = {0.0};
- float rl[4][mt] = {0.0};
+ float dph1[mh] = {0};
+ float dph2[mh] = {0};
+ float dph3[mh] = {0};
+ float dph4[mh] = {0};
+ float fph[mh][mh] = {0};
+ 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 t = 0.0;
- float phf[mh]={0.0};
+ float phf[mh]={0};
//********************* HOSHINO *********************
+ //********************* OHTSU ***********************
+ //********************* OHTSU ***********************
+
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
- while(1){
- //for(int j=0;j<1800;j++){ flag tukuru // for 2 minuts
+ while(flag<120){ //flag tukuru // for 2 minuts
// ******************** sequence No.1 ********************
buf_char = mavc.getc();
@@ -158,34 +164,38 @@
// ******************** initialization *******************
if(t == 0){
- x0 = GPS[0];
- y0 = GPS[1];
- psi0= azi;
+ x0 = GPS[0];
+ y0 = GPS[1];
+ ps0 = azi;
}
- xs = GPS[0] - x0;
- ys = GPS[1] - y0;
- psis= azi - psi0;
+ xis = GPS[0] - x0;
+ ets = GPS[1] - y0;
+ psr = azi - ps0;
+ htr = alt;
+ avp = gyro[0];
+ avq = gyro[1];
+ avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
dr = dr + ua * dt;
- xr = xr + ua * dt * cos(psi);
- yr = yr + ua * dt * sin(psi);
+ xir = xir + ua * dt * cos(psr);
+ etr = etr + ua * dt * sin(psr);
- if (xs != xs1 || fabs(xs-xs1)<50) {
- s = (xs-xs1)*(xs-xs1)+(ys-ys1)*(ys-ys1);
+ if (xis != xis1 || fabs(xis-xis1)<50) {
+ s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
ua = 0.5*(ua+sqrt(s));
drc = drc+ua;
dr= drc;
- xr=xs;
- yr=ys;
+ xir=xis;
+ etr=ets;
}
- xs1=xs;
- ys1=ys;
+ xis1=xis;
+ ets1=ets;
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
- hr = b0 + b1*x + b2*x*x + b3*x*x*x;
+ htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr;
/*
c = drp-dr[i];
@@ -200,7 +210,7 @@
etc=c0*et[i+1]+c1*et[i];
break;
}
- */
+ *///kurikaesituika
// ******************** guidance law WO homo *************
@@ -211,7 +221,9 @@
// !!! HOSHINO !!! fprintf add guidance, velocity and control
fprintf(fp, "%7.4f %7.4f %7.4f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]);
-
+
+ flag++;
+
}
time = time + 0.1;