yal kaiyo
/
130114_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
130111_YNU_MPU_4
by 
yal kaiyo
Diff: main.cpp
- Revision:
- 9:a56c340f47c9
- Parent:
- 8:de86cf9ccb89
- Child:
- 10:fe98c33623b6
--- 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++;