main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 
00004 #define pi      3.1416
00005 #define data    6
00006 
00007 Serial pc(USBTX, USBRX);
00008 DigitalOut led1(LED1);
00009 DigitalOut led2(LED2);
00010 DigitalOut led3(LED3);
00011 DigitalOut led4(LED4);
00012 
00013 SDFileSystem sd(p5, p6, p7, p8, "sd");
00014 Serial mavc(p9, p10);
00015 DigitalIn stop_sd(p11);
00016 
00017 int main() {
00018         
00019     // ******************** TORATANI ********************
00020     //pc.baud(921600);
00021     mavc.baud(115200);
00022     
00023     char  buf_char;
00024     unsigned int    buf_hex[30] =   {0};
00025     unsigned int    buf_dec[10] =   {0};
00026     
00027     // rx
00028     double  acc[3]  =   {0};
00029     double  gyro[3] =   {0};
00030     double  azi;
00031     double  alt;
00032     double  GPS[2]  =   {0};
00033     
00034     // tx
00035     double  com[3]  =   {0};
00036     char H;
00037     char D[data];
00038     char test[9];
00039     H       =   0x02;   // header of dateset2
00040     D[0]    =   0xff;   D[1]    =   0xff;       // p_com
00041     D[2]    =   0x7f;   D[3]    =   0xff;       // q_com
00042     D[4]    =   0x00;   D[5]    =   0x00;       // r_com
00043     // ******************** TORATANI ********************
00044     
00045     FILE *fp = fopen("/sd/sdtest.txt", "w");
00046     if(fp == NULL) {
00047         error("Could not open file for write\n");
00048     }
00049     
00050     while(1){
00051     //for(int j=0;j<1800;j++){      flag tukuru     // for 2 minuts
00052         
00053         // ******************** sequence No.1 ********************
00054         buf_char       =   mavc.getc();
00055         buf_hex[0]  =   (unsigned int)buf_char;
00056         for(int i=0;i<29;i++){  buf_hex[29-i]   =   buf_hex[28-i];  }
00057         // !!! switch bun ni suru
00058         if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
00059         // ******************** sequence No.1 ********************
00060             
00061             // ******************** sequence No.2 ********************
00062             // send to mavc
00063             mavc.putc(H);
00064             for(int i=0;i<data;i++){  mavc.putc(D[i]);  }
00065             //for(int i=0;i<9;i++){  mavc.putc(test[i]);  }
00066             // ******************** sequence No.2 ********************
00067             
00068             // ******************** sequence No.3 ********************
00069             // buf |  25  | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 |  9 |  8 |  7 |  6 |  5  |
00070             //     |header|   AccX  |   AccY  |   AccZ  |  GyroX  |  GyroY  |   GyroZ | Azimuth | Altitude|   GPSX  |   GPSY   |
00071             //     |   H  |   D[1]  |   D[2]  |   D[3]  |   D[4]  |   D[5]  |   D[6]  |   D[7]  |   D[8]  |   D[9]  |   D[10]  |
00072             // transrating hex to dec
00073             for(int i=0;i<10;i++){  buf_dec[i]   =   buf_hex[24-i*2]*256+buf_hex[23-i*2];   }
00074             // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
00075             // -> saisyuteki niha matrix ni simasu?
00076             for(int i=0;i<3;i++){   acc[i]  =   ((double)buf_dec[i]-32768)/65535*20*9.8;    }
00077             for(int i=0;i<3;i++){   gyro[i] =   ((double)buf_dec[i+3]-32768)/65535*600/180*pi;  }
00078             azi =   (double)buf_dec[6]/65535*2*pi;
00079             alt =   ((double)buf_dec[7]-32768)*0.1;
00080             for(int i=0;i<2;i++){   GPS[i]  =   ((double)buf_dec[i+8]-32768)*0.1;   }
00081             // ******************** sequence No.3 ********************
00082             
00083             // ******************** control law ********************
00084             // transrating dec to hex
00085             //com
00086             // ******************** control law ********************
00087             
00088             //pc.printf("%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]);
00089             //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]);
00090                         
00091         }
00092              
00093     }
00094     
00095     fclose(fp); 
00096     led1    =   1;      led2    =   1;      led3    =   1;      led4    =   1;
00097     
00098 }