yal kaiyo
still homotopy @FB
Fork of
130111_YNU_MPU_4
by 
yal kaiyo
Diff: main.cpp
- Revision:
- 9:a56c340f47c9
- Parent:
- 8:de86cf9ccb89
- Child:
- 10:fe98c33623b6
diff -r de86cf9ccb89 -r a56c340f47c9 main.cpp
--- a/main.cpp Thu Jan 10 14:43:14 2013 +0000
+++ b/main.cpp Thu Jan 10 18:14:02 2013 +0000
@@ -2,7 +2,7 @@
#include "SDFileSystem.h"
#define pi 3.1416
-#define data 12
+#define data 6
#define zer 0.0
#define hal 0.5
@@ -31,12 +31,12 @@
int main() {
// ******************** TORATANI ********************
- //pc.baud(921600);
+ pc.baud(921600);
mavc.baud(115200);
int flag_lc = 0;
int flag_cm = 0;
- int loop = 1200; // for 2 minuts
+ int loop = 100; // for 2 minuts -> loop = 1200
char buf_char;
unsigned int buf_hex_rx[30] = {0};
@@ -52,47 +52,60 @@
double GPS[2] = {0};
// tx
- double com[3] = {0};
- char H;
- char D[data] = {0};
+ float com[3] = {0};
+ unsigned int H;
+ unsigned int D[data] = {0};
H = 0x02; // header of dateset2
+ D[0] = 0x7f; D[1] = 0xff; // p_com
+ D[2] = 0x7f; D[3] = 0xff; // q_com
+ D[4] = 0x7f; D[5] = 0xff; // r_com
+ /*
D[0] = 0x7; D[1] = 0xf; D[2] = 0xf; D[3] = 0xf; // p_com
D[4] = 0x7; D[5] = 0xf; D[6] = 0xf; D[7] = 0xf; // q_com
D[8] = 0x7; D[9] = 0xf; D[10] = 0xf; D[11] = 0xf; // r_com
+ */
// ******************** TORATANI ********************
// ******************** HOSHINO *********************
- float time = 0; //time
+ float time = 0; //time
float xis = 0;
- float ets = 0;
+ float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
- float drf = 800; //syuutandaunrenji
- float ht0 = 200;
- float htf = 10;
+ 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 htc = 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 dr = 0;
- float dt = 0.1;
- float psr = 0;
- float htr = 0;
+ 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 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;
//homotopy
float rps0 = 90.0;
@@ -131,14 +144,15 @@
float et1[mt]={0.0};
float psc;
float etc;
+ float xic;
//********************* HOSHINO *********************
//********************* OHTSU ***********************
//teigi
- float grv = 9.80665;
+ float grv = 9.807;
float swg = 0.04695;
float wgt = 0.76;
- float rho = 1.22547328574415;
+ float rho = 1.2255;
float tt1 = 0.8;
float omg1 = 1.0;
@@ -171,7 +185,8 @@
error("Could not open file for write\n");
}
- // ******************** waiting @LC ********************
+ // **************** waiting @LC or @FB ****************
+ // * @LC:0x40 0x4C 0x43 0x0D, @FB:0x40 0x46 0x42 0x0D *
// /////////////////////////////////////////////////////
// ////////////////////// NOTICE! //////////////////////
// // flag_lc==1 is debug mode. skip this while loop. //
@@ -181,7 +196,7 @@
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]==0x4C && buf_hex_rx[27]==0x43 && buf_hex_rx[26]==0x0D){
+ if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){
// ******************** waiting @LC ********************
// ******************** homotopy ********************
@@ -1040,7 +1055,7 @@
wait(1.0);
///////////////////
- flag_lc = 1; // end of homotopy
+ flag_lc = 1; // end of homotopy flag
// ******************** homotopy ********************
}
@@ -1075,53 +1090,58 @@
// ******************** sequence No.3 ********************
// ******************** initialization *******************
- if(t == 0){
- x0 = GPS[0];
- y0 = GPS[1];
- ps0 = azi;
+ for(int i=0;i<mt;i++){
+ dr[i]=i*drf/md;
}
- xis = GPS[0] - x0;
- ets = GPS[1] - y0;
- psr = azi - ps0;
- htr = alt;
+ if(t == 0){
+ x0 = GPS[0];
+ y0 = GPS[1];
+ ps0 = azi;
+ ht0 = alt;
+ }
+ ets = (GPS[0] - x0)*cos(ps0);
+ xis = (GPS[1] - y0)*sin(ps0);
+ psr = pi/2 -(azi - ps0);
+ htr = 50+(alt-ht0);
avp = gyro[0];
avq = gyro[1];
avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
- dr = dr + ua * dt;
+ drp = drp + ua * dt;
xir = xir + ua * dt * cos(psr);
etr = etr + ua * dt * sin(psr);
- if (xis != xis1 || fabs(xis-xis1)<50) {
+ 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;
+ ua = 0.5*(ua+sqrt(s)/(time-time1));
+ drc = drc+ua*(time-time1);
+ drp= drc;
xir=xis;
etr=ets;
+ xis1=xis;
+ ets1=ets;
}
- xis1=xis;
- ets1=ets;
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
- htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr;
+ htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
- /*
+ for(int i=0;i<mt;i++){
c = drp-dr[i];
c1= dr[i+1]-drp;
if(c >=0 && c1 >= 0){
- c0=c/(dr[i+1]-dr[i]);
- c1=c1/(dr[i+1]-dr[i]);
- psc=c0*ps[i+1]+c1*ps[i];
- xic=c0*xi[i+1]+c1*xi[i];
- etc=c0*et[i+1]+c1*et[i];
- break;
+ c0=c/(dr[i+1]-dr[i]);
+ c1=c1/(dr[i+1]-dr[i]);
+ psc=c0*ps[i+1]+c1*ps[i];
+ xic=c0*xi[i+1]+c1*xi[i];
+ etc=c0*et[i+1]+c1*et[i];
+ break;
}
- *///kurikaesituika
+ }
+
// ******************** guidance law WO homo *************
// ******************** control law **********************
@@ -1130,49 +1150,51 @@
hte=htc-htr;
//horizontal
+ /*
gkp=omg1*omg1/grv;
gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
avpc=1/tt1*(gkd*pse+gkp*ete);//
+ */
+
+ //avpc=kp*hte;
//vertical
+ /*
v2=vmr*vmr;
- dhdt=(hte-hte00)/dt;
+ dhdt=(hte-hte00)/dt
+ */
// to translate deg/s to rad/s, add *pi/180. TORATANI
- 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=(-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);
+
+ // dummy output
+ avpc = avpc+0.001*pi;
+ avqc = 0;
+ avrc = avrc-0.001*pi;
+
+ com[0] = avpc;
+ com[1] = avqc;
+ com[2] = avrc;
// transrating dec to hex
for(int i=0;i<3;i++){
buf_dec_tx[i] = 19660.5/pi*com[i]+32768;
- // pc.printf("%d ", buf_dec_tx[i]);
-
- buf_hex_tx[i][0] = buf_dec_tx[i]/4096;
- buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][0]*4096;
- buf_hex_tx[i][1] = buf_dec_tx[i]/256;
- buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][1]*256;
- buf_hex_tx[i][2] = buf_dec_tx[i]/16;
- buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][2]*16;
- buf_hex_tx[i][3] = buf_dec_tx[i];
+ buf_hex_tx[i][0] = buf_dec_tx[i]/256;
+ buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][0]*256;
+ buf_hex_tx[i][1] = buf_dec_tx[i];
+
+ for(int j=0;j<2;j++){ D[j+2*i] = buf_hex_tx[i][j]; }
- // pc.printf("%d %d %d %d\r\n", buf_hex_tx[i][0], buf_hex_tx[i][1], buf_hex_tx[i][2], buf_hex_tx[i][3]);
-
- for(int j=0;j<4;j++){
- if(buf_hex_tx[i][j]<10){
- buf_hex_tx[i][j] = buf_hex_tx[i][j]+48;
- D[j+4*i] = (char)buf_hex_tx[i][j];
- }else{
- buf_hex_tx[i][j] = buf_hex_tx[i][j]+55;
- D[j+4*i] = (char)buf_hex_tx[i][j];
- }
- }
}
// ******************** control law **********************
-
+
// !!! 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]);
-
+ fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n",time, gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]);
+
time = time + 0.1;
flag_cm++;