yal kaiyo
for hige
Fork of
YNU_MPU_1
by 
yal kaiyo
main.cpp
- Committer:
- kenkenpapurika
- Date:
- 2013-01-10
- Revision:
- 7:05a718fdef74
- Parent:
- 6:3792b68b1fc9
- Child:
- 8:de86cf9ccb89
File content as of revision 7:05a718fdef74:
#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};
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;
//********************* HOSHINO *********************
//********************* OHTSU ***********************
//teigi
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 gmr=20;
float vmr=0;
float pse;
float ete;
float hte;
float hte00;
float dhdt;
float v2;
float gkp;
float gkd;
float avpc;
float avqc;
//********************* OHTSU ***********************
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
// ******************** waiting @LC ********************
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){
// ******************** 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
// ******************** 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
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);//
//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));//
hte00=hte;
// ******************** 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;
}