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;
    
}