yal kaiyo
for hige
Fork of
YNU_MPU_1
by 
yal kaiyo
Diff: main.cpp
- Revision:
- 8:de86cf9ccb89
- Parent:
- 7:05a718fdef74
- Child:
- 9:a56c340f47c9
diff -r 05a718fdef74 -r de86cf9ccb89 main.cpp
--- a/main.cpp Thu Jan 10 05:59:12 2013 +0000
+++ b/main.cpp Thu Jan 10 14:43:14 2013 +0000
@@ -2,7 +2,7 @@
#include "SDFileSystem.h"
#define pi 3.1416
-#define data 6
+#define data 12
#define zer 0.0
#define hal 0.5
@@ -39,8 +39,10 @@
int loop = 1200; // for 2 minuts
char buf_char;
- unsigned int buf_hex[30] = {0};
- unsigned int buf_dec[10] = {0};
+ unsigned int buf_hex_rx[30] = {0};
+ unsigned int buf_dec_rx[10] = {0};
+ unsigned int buf_dec_tx[3] = {0};
+ unsigned int buf_hex_tx[3][4] = {0};
// rx
double acc[3] = {0};
@@ -52,12 +54,11 @@
// tx
double com[3] = {0};
char H;
- char D[data];
- char test[9];
+ char D[data] = {0};
H = 0x02; // header of dateset2
- D[0] = 0xff; D[1] = 0xff; // p_com
- D[2] = 0x7f; D[3] = 0xff; // q_com
- D[4] = 0x00; D[5] = 0x00; // 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 *********************
@@ -134,36 +135,35 @@
//********************* OHTSU ***********************
//teigi
- float grv=9.80665;
- float swg=0.04695;
- float wgt=0.76;
- float rho=1.22547328574415;
-
+ float grv = 9.80665;
+ float swg = 0.04695;
+ float wgt = 0.76;
+ float rho = 1.22547328574415;
- float tt1=0.8;
- float omg1=1.0;
- float zet1=0.7;
- float tt2=0.37;
- float omg2=2.6;
- float zet2=0.7;
+ float tt1 = 0.8;
+ float omg1 = 1.0;
+ float zet1 = 0.7;
+ float tt2 = 0.37;
+ float omg2 = 2.6;
+ float zet2 = 0.7;
- float gmr=20;
- float vmr=0;
- float pse;
- float ete;
- float hte;
+ float gmr = 20;
+ float vmr = 0;
+ float pse = 0;
+ float ete = 0;
+ float hte = 0;
- float hte00;
- float dhdt;
+ float hte00 = 0;
+ float dhdt = 0;
- float v2;
+ float v2 = 0;
- float gkp;
- float gkd;
+ float gkp = 0;
+ float gkd = 0;
- float avpc;
- float avqc;
-
+ float avpc = 0;
+ float avqc = 0;
+ float avrc = 0;
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
@@ -172,11 +172,16 @@
}
// ******************** waiting @LC ********************
+ // /////////////////////////////////////////////////////
+ // ////////////////////// NOTICE! //////////////////////
+ // // flag_lc==1 is debug mode. skip this while loop. //
+ // ////////// for flight mode, flag_lc is 0! ///////////
+ // /////////////////////////////////////////////////////
while(flag_lc==1){
buf_char = mavc.getc();
- buf_hex[0] = (unsigned int)buf_char;
- for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
- if(buf_hex[29]==0x40 && buf_hex[28]==0x4C && buf_hex[27]==0x43 && buf_hex[26]==0x0D){
+ 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){
// ******************** waiting @LC ********************
// ******************** homotopy ********************
@@ -1045,16 +1050,15 @@
// ******************** sequence No.1 ********************
buf_char = mavc.getc();
- buf_hex[0] = (unsigned int)buf_char;
- for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
- if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
+ 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]); }
- //for(int i=0;i<9;i++){ mavc.putc(test[i]); }
// ******************** sequence No.2 ********************
// ******************** sequence No.3 ********************
@@ -1062,21 +1066,19 @@
// |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[i] = buf_hex[24-i*2]*256+buf_hex[23-i*2]; }
- // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
- // -> saisyuteki niha matrix ni simasu?
- for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec[i]-32768)/65535*20*9.8; }
- for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec[i+3]-32768)/65535*600/180*pi; }
- azi = (double)buf_dec[6]/65535*2*pi;
- alt = ((double)buf_dec[7]-32768)*0.1;
- for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec[i+8]-32768)*0.1; }
+ 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 *******************
if(t == 0){
- x0 = GPS[0];
- y0 = GPS[1];
- ps0 = azi;
+ x0 = GPS[0];
+ y0 = GPS[1];
+ ps0 = azi;
}
xis = GPS[0] - x0;
ets = GPS[1] - y0;
@@ -1111,23 +1113,18 @@
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;
+ 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;
}
*///kurikaesituika
-
// ******************** guidance law WO homo *************
// ******************** control law **********************
- // transrating dec to hex
- //com
-
pse=(psc-psr)*dtr;
ete=etc-etr;
hte=htc-htr;
@@ -1140,21 +1137,46 @@
//vertical
v2=vmr*vmr;
dhdt=(hte-hte00)/dt;
-
- avqc=-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte));//
+
+ // 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;
-
+ // 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];
+
+ // 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]);
+ time = time + 0.1;
flag_cm++;
- }
-
- time = time + 0.1;
+ }
}