YNU_MPU
Dependencies: FatFileSystem SDFileSystem mbed
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 }
Generated on Fri Jul 15 2022 07:50:28 by 1.7.2