for hige

Fork of YNU_MPU_1 by yal kaiyo

main.cpp

Committer:
yal_kaiyo
Date:
2013-01-14
Revision:
15:571b34671309
Parent:
14:feea7e2a51b8
Child:
16:a33fe7d99a0f

File content as of revision 15:571b34671309:

#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    =   100;       // 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   flag_int=   0;
    //initial condition
    float   drf     =   100;    //syuutandaunrenji
    float   ht0     =   0;
    float   gm0     =   20*pi/180;
    float   ua      =   10; //initial horizontal velosity
    //terminal condition
    float   htf     =   5;    
    float   gmf     =   5*pi/180;
     
    float   xis     =   0;
    float   ets     =   0;
    float   psis    =   0;
    float   x0      =   0;
    float   y0      =   0; 
    float   h0      =   0;    
    float   b0      =   0;
    float   b1      =   0;
    float   b2      =   0;
    float   b3      =   0;
    float   htc     =   0;
    float   x       =   0;  //down range
    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;
    float   psc     =   0;
    float   etc     =   0;
    float   xic     =   0;
    float   apc     =   0;
    
    //homotopy
    //initial condition
    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;
    float rdet =1;
    float idet =0;
    float t = 0.0;
    float phf[mh]={0};
    float   ap[mt]={0};
    float   ps[mt]={0};
    float   xi[mt]={0};
    float   et[mt]={0};
    float   xi1[mt]={0};
    float   et1[mt]={0};

    //********************* 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   phir    =   0;
    float   ete     =   0;   
    float   hte     =   0;
    float   xie     =   0;
    
    float   hte00   =   0;
    float   dhdt    =   0; 
    float   phic00  =   0;
    float   dclda   =   0.084;

    float   v2      =   0;
    float   a22     =   0;
    float   a23     =   0;
    float   a32     =   0;
    float   b21     =   0;
    float   aaa     =   0;
    
    float   gkp     =   0;
    float   gkd     =   0;
    float   ck1     =   0;
    float   ck2     =   0;
    float   phff    =   0;
    float   phic    =   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==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]==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(flag_int == 0){
            x0   =   GPS[0];
            y0   =   GPS[1];
            ps0  =   azi;
            ht0  =   alt;
            b0      =   ht0;
            b1      =   tan(gm0);
            b2      =   (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf); 
            b3      =   -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
            flag_int=1;            
            }
            xis  =   (GPS[0]  -   x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
            ets  =   -(GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1]  -   y0)*sin(pi/2-ps0);
            psr  =   rps0*dtr   -(azi     -   ps0);
            htr  =   alt;
            avp  =   gyro[0];
            avq  =   gyro[1];
            avr  =   gyro[2];
            // ******************** initialization *******************
            
            // ******************** interpolation ********************
            //xir ni hennkannseneba

            //filter henko

            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 + ua * dt * cos(psr);
                etr=ets + ua * dt * sin(psr);
                xis1=xis;
                ets1=ets;
            }
            else{
                drp = drp + ua * dt;
                xir = xir + ua * dt * cos(psr);
                etr = etr + ua * dt * sin(psr);
            }
            // ******************** interpolation ********************
            
            // ******************** guidance law WO homo *************

            /*
            b0      =   ht0
            b1      =   (hf-ht0)/drf
            */
            /*
            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;
             }
            }
            */
            htc =   ht0;
            xic =   0;
            etc =   0;
            // ******************** guidance law WO homo *************
            
            // ******************** control law **********************
            xie=xic-xir;
            pse=(psc-psr)*dtr;
            ete=etc-etr;
            hte=htc-htr;
            
            //horizontal
             v2=vmr*vmr;
            
            gkp=omg1*omg1/grv;
            gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
            phff=atan(apc*v2/grv);
            phic=-gkp*ete-gkd*pse-phff;
            avpc=(phic-phic00)/dt;
            phic00=phic;
            
            
            
            //vertical
            
            a22=grv*sin(gmr*dtr)/vmr;
            a23=0;
            a32=vmr*cos(gmr*dtr);
            b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
            aaa=a32*b21;
            ck1=(a22+2*zet2*omg2)/aaa;
            ck2=(a23*a32+omg2*omg2)/aaa;
            
            
            
            
            dhdt=(hte-hte00)/dt;
            
            
            // to translate deg/s to rad/s, add *pi/180. TORATANI 
            avqc=(ck1*dhdt+ck2*hte)/tt2;
            hte00=hte;
            
            

            
            com[0]  =   avpc;
            com[1]  =   avqc;
            com[2]  =   avrc;
            
            if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
            com[0]  =   0;
            com[1]  =   0;
            com[2]  =   0;
            }
            
            // 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;
    
}