yal kaiyo
/
130110_YNU_MPU_1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
YNU_MPU_0108
by 
hige dura
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;
+ }
}