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
main.cpp
- Committer:
- higedura
- Date:
- 2013-01-08
- Revision:
- 5:192835ac6106
- Parent:
- 4:037fab837823
- Child:
- 6:3792b68b1fc9
File content as of revision 5:192835ac6106:
#include "mbed.h"
#include "SDFileSystem.h"
#define pi 3.1416
#define data 6
#define zer 0.0
#define hal 0.5
#define one 1.0
#define dtr 0.01745
#define rtd 57.3
//parameter//
#define dt1 0.2 // 1/N
#define hdt 0.1 // 1/2N
#define md 6 //kizami N
#define mt 7 //md+1
#define mx 4
#define mv 8
#define mh 56 //mt*mv
Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
SDFileSystem sd(p5, p6, p7, p8, "sd");
Serial mavc(p9, p10);
DigitalIn stop_sd(p11);
int main() {
// ******************** TORATANI ********************
//pc.baud(921600);
mavc.baud(115200);
int flag_lc = 0;
int flag_cm = 0;
int loop = 1200; // for 2 minuts
char buf_char;
unsigned int buf_hex[30] = {0};
unsigned int buf_dec[10] = {0};
// rx
double acc[3] = {0};
double gyro[3] = {0};
double azi;
double alt;
double GPS[2] = {0};
// tx
double com[3] = {0};
char H;
char D[data];
char test[9];
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
// ******************** TORATANI ********************
// ******************** HOSHINO *********************
float time = 0; //time
float xis = 0;
float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
float drf = 800; //syuutandaunrenji
float ht0 = 200;
float htf = 10;
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 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 avp = 0;
float avq = 0;
float avr = 0;
float s = 0;
//homotopy
float rps0 = 90.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;
float eps = 0.00000001;
float ueps = 0.000001;
float ier = 0.0;
float sum = 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};
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};
//********************* HOSHINO *********************
//********************* OHTSU ***********************
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
// ******************** waiting @LC ********************
while(flag_lc==0){
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){
// ******************** waiting @LC ********************
// ******************** homotopy ********************
flag_lc = 1; // end of homotopy
// ******************** homotopy ********************
}
}
while(flag_cm<loop){
// ******************** 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){
// ******************** 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 ********************
// buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
// |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; }
// ******************** sequence No.3 ********************
// ******************** initialization *******************
if(t == 0){
x0 = GPS[0];
y0 = GPS[1];
ps0 = azi;
}
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;
xir = xir + ua * dt * cos(psr);
etr = etr + ua * dt * sin(psr);
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;
xir=xis;
etr=ets;
}
xis1=xis;
ets1=ets;
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr;
/*
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;
}
*///kurikaesituika
// ******************** guidance law WO homo *************
// ******************** control law **********************
// transrating dec to hex
//com
// ******************** 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]);
flag_cm++;
}
time = time + 0.1;
}
fclose(fp);
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
}