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
main.cpp
- Committer:
- yal_kaiyo
- Date:
- 2013-01-11
- Revision:
- 12:33085c983354
- Parent:
- 11:5a744949ba5a
- Child:
- 13:b444c425e194
File content as of revision 12:33085c983354:
#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 -> loop = 1200
char buf_char;
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};
double gyro[3] = {0};
double azi;
double alt;
double GPS[2] = {0};
// tx
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 xis = 0;
float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
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 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 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;
float rxi0 = -60.0; //terminal xi
float ret0 = -20.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};
float ap[mt]={0.0};
float ps[mt]={0.0};
float xi[mt]={0.0};
float et[mt]={0.0};
float xi1[mt]={0.0};
float et1[mt]={0.0};
float psc;
float etc;
float xic;
//********************* HOSHINO *********************
//********************* OHTSU ***********************
//teigi
float grv = 9.807;
float swg = 0.04695;
float wgt = 0.76;
float rho = 1.2255;
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 = 0;
float ete = 0;
float hte = 0;
float hte00 = 0;
float dhdt = 0;
float v2 = 0;
float gkp = 0;
float gkd = 0;
float avpc = 0;
float avqc = 0;
float avrc = 0;
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
// **************** 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. //
// ////////// for flight mode, flag_lc is 0! ///////////
// /////////////////////////////////////////////////////
while(flag_lc==0){
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]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){
// ******************** waiting @LC ********************
// ******************** homotopy ********************
////SYOKIKATEIKAI////
for(int it=0;it<mt;it++){
double tm=dt1*it;
int i0=mv*it;
ph0[i0]=zer;
if(fabs(ps0)<=0.0000000001){
ph0[i0+1]=(ps0+1.0)*tm;
}
else{
ph0[i0+1] = ps0*tm;
}
ph0[i0+2]= xi0*tm;
ph0[i0+3]= et0*tm;
ph0[i0+4]=-hal;
ph0[i0+5]=-hal;
ph0[i0+6]=-hal;
ph0[i0+7]=-hal;
}
for(int it=0;it<mh;it++){
phi[it]=ph0[it];
}
for(int it=0;it<mh;it++){
phf[it]=ph0[it];
}
//rungekutta
for(int ir=0;ir<mt;ir++){
//k1
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
//if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph1[jh]=sum;
}
for(int it=0;it<mh;it++){
phi[it]= phf[it]+hal*dt1*dph1[it];
}
//k2
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph2[jh]=sum;
}
//k3
for(int it=0;it<mh;it++){
phi[it]= phf[it]+hal*dt1*dph2[it];
}
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph3[jh]=sum;
}
//k4
for(int it=0;it<mh;it++){
phi[it]= phf[it]+dt1*dph3[it];
}
////SABUNSHIKI//////
for(int it=0;it<mt;it++){
int i0=mv*it;
x1[0][it]=phi[i0+0];
x1[1][it]=phi[i0+1];
x1[2][it]=phi[i0+2];
x1[3][it]=phi[i0+3];
rl[0][it]=phi[i0+4];
rl[1][it]=phi[i0+5];
rl[2][it]=phi[i0+6];
rl[3][it]=phi[i0+7];
}
fn[0]=x1[0][0];
fn[1]=x1[1][0];
fn[2]=x1[2][0];
fn[3]=x1[3][0];
for(int it=0;it<md;it++){
int i0=mx+mv*it;
int it1=it+1;
double bkp = hal*(x1[0][it]+x1[0][it1]);
double bps = hal*(x1[1][it]+x1[1][it1]);
double br1 = hal*(rl[0][it]+rl[0][it1]);
double br2 = hal*(rl[1][it]+rl[1][it1]);
double br3 = hal*(rl[2][it]+rl[2][it1]);
double br4 = hal*(rl[3][it]+rl[3][it1]);
fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
fn[i0+6] = rl[2][it1]-rl[2][it];
fn[i0+7] = rl[3][it1]-rl[3][it];
}
fn[mh-4]=x1[0][mt-1];
fn[mh-3]=x1[1][mt-1]-ps0;
fn[mh-2]=x1[2][mt-1]-xi0;
fn[mh-1]=x1[3][mt-1]-et0;
////HENBIBUN////////////
//// I 0 //////
//// A B 0 ////
//// 0 A B 0 //
///////////////
////////////I 0
//I
fph[0][0]=one;
fph[1][1]=one;
fph[2][2]=one;
fph[3][3]=one;
//A
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*iu;
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=-one;
}
fph[i0+1][j0] = hdt;
fph[i0+2][j0+1] =-hdt*sps;
fph[i0+3][j0+1] = hdt*cps;
fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
fph[i0][j0+4] = hdt;
fph[i0+4][j0+5] =-hdt;
fph[i0+5][j0+6] = hdt*sps;
fph[i0+5][j0+7] =-hdt*cps;
}
//B
//j0=j0+mv;
for(int iu=0;iu<md;iu++){
int i0=mx+mv*iu;
int j0=mv*(iu+1);
int j1=j0+mv;
double bps=hal*(phi[j0+1]+phi[j1+1]);
double br3=hal*(phi[j0+6]+phi[j1+6]);
double br4=hal*(phi[j0+7]+phi[j1+7]);
double cps=cos(bps);
double sps=sin(bps);
for(int iv=0;iv<mv;iv++){
fph[i0+iv][j0+iv]=one;
}
fph[i0+1][j0]=hdt;
fph[i0+2][j0+1]=-hdt*sps;
fph[i0+3][j0+1]=hdt*cps;
fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
fph[i0][j0+4]=hdt;
fph[i0+4][j0+5]=-hdt;
fph[i0+5][j0+6]=hdt*sps;
fph[i0+5][j0+7]=-hdt*cps;
}
//I
fph[mh-4][mh-8]=one;
fph[mh-3][mh-7]=one;
fph[mh-2][mh-6]=one;
fph[mh-1][mh-5]=one;
//GYAKUGYOURETSU
for(int i=0;i<mh;i++){
for(int j=0;j<mh;j++){
fpi[i][j]=fph[i][j];
}
}
for(int j=0;j<mh;j++) {
for(int i=0;i<mh;i++) {
if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
}
}
for(int i=0;i<mh;i++) {
double q=wk[i];
// if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
wk[i]=1.0/q;
if(q > qmax){qmax=q;}
}
if(eps < 0){eps=qmax*mh*ueps;}
rdet =1.0;
idet =0.0;
for(int k=0;k<mh;k++) {
double amax = 0.0;
int kmax = k;
for(int i=k;i<mh;i++) {
if(fabs(fpi[i][k])*wk[i] <= amax){}
else{
amax=fabs(fpi[i][k])*wk[i];
kmax=i;
}
}
// if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
rdet=rdet*fpi[kmax][k];
if(k != kmax){rdet=-rdet;}
double adet =fabs(rdet);
while(adet > 1.0){
adet = adet*0.0625;
idet = idet + 4;
}
if(adet >= 0.0625){}
else{
adet=adet*16.0;
idet=idet-4;
}
if(rdet < 0.0){adet=-adet;}
rdet=adet;
iwk[k]=kmax;
amax=-1.0/fpi[kmax][k];
t=fpi[kmax][k];
fpi[kmax][k]=fpi[k][k];
fpi[k][k]=t;
for(int j=0;j<mh;j++) {
if(j == k){}
else{
t=fpi[kmax][j]*amax;
fpi[kmax][j]=fpi[k][j];
if(fabs(t) <= 0){}
else{
for(int i=0;i<mh;i++) {
fpi[i][j]=fpi[i][j]+t*fpi[i][k];
}
}
fpi[k][j]=-t;
}
}
for(int i=0;i<mh;i++) {
fpi[i][k]=fpi[i][k]*amax;
}
fpi[k][k]=-amax;
}
for(int l=0;l<mh;l++) {
int k=mh-l-1;
if(k == iwk[k]){}
else{
int kmax=iwk[k];
for(int i=0;i<mh;i++) {
t=fpi[i][k];
fpi[i][k]=fpi[i][kmax];
fpi[i][kmax]=t;
}
}
}
//////function
for(int jh=0;jh<mh;jh++){
sum=zer;
for(int ih=0;ih<mh;ih++){
sum=sum-fpi[jh][ih]*fn[ih];
}
dph4[jh]=sum;
}
//saitekikai
for(int it=0;it<mh;it++){
phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0;
}
}
for (int id=0; id< mt; ++id){
int ii = mt-id-1;
int i0 = mv*ii;
ap[id]=phi[i0+0];
ps[id]=phi[i0+1];
xi1[id]=phi[i0+2]*drf;
et1[id]=phi[i0+3]*drf;
xi[id]=xi1[id]-xi1[0];
et[id]=et1[id]-et1[0];
}
led1 = 1;
wait(1.0);
///////////////////
flag_lc = 1; // end of homotopy flag
// ******************** homotopy ********************
}
}
while(flag_cm<loop){
// ******************** sequence No.1 ********************
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]==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]); }
// ******************** 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_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 *******************
for(int i=0;i<mt;i++){
dr[i]=i*drf/md;
}
if(t == 0){
x0 = GPS[0];
y0 = GPS[1];
ps0 = azi;
ht0 = alt;
}
xis = (GPS[0] - x0)*cos(pi/2-ps0)-(GPS[1] - y0)*sin(pi/2-ps0);
ets = (GPS[1] - y0)*sin(pi/2-ps0)+(GPS[0] - x0)*cos(pi/2-ps0);
psr = pi/2 -(azi - ps0);
htr = 50+(alt-ht0);
avp = gyro[0];
avq = gyro[1];
avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
drp = drp + 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)/(time-time1));
drc = drc+ua*(time-time1);
drp= drc;
xir=xis;
etr=ets;
xis1=xis;
ets1=ets;
}
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
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;
}
}
// ******************** guidance law WO homo *************
// ******************** control law **********************
pse=(psc-psr)*dtr;
ete=etc-etr;
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
*/
// 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=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;
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]; }
}
// ******************** control law **********************
// !!! HOSHINO !!! fprintf add guidance, velocity and control
fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %8.5f %8.5f %8.1f %8.1f %8.1f %8.1f %8.1f %8.1f\r\n",time ,gyro[0],avpc, gyro[1],avqc, gyro[2],avrc, psr,psc, htr,htc, xir,xic,etr,etc);
time = time + 0.1;
flag_cm++;
}
}
fclose(fp);
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
}