Kyohei Shinomoto / Mbed 2 deprecated YNU_MPU_0108

add homotopy

Fork of YNU_MPUd2h by hige dura

main.cpp@0:9aa9be24636a, 2013-01-06 (annotated)

Committer:
higedura
Date:
Sun Jan 06 06:37:09 2013 +0000
Revision:
0:9aa9be24636a
Child:
1:ca296cfa603b
YNU_MPU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:9aa9be24636a 1 #include "mbed.h"
higedura 0:9aa9be24636a 2 #include "SDFileSystem.h"
higedura 0:9aa9be24636a 3
higedura 0:9aa9be24636a 4 #define pi 3.1416
higedura 0:9aa9be24636a 5 #define data 6
higedura 0:9aa9be24636a 6
higedura 0:9aa9be24636a 7 Serial pc(USBTX, USBRX);
higedura 0:9aa9be24636a 8 DigitalOut led1(LED1);
higedura 0:9aa9be24636a 9 DigitalOut led2(LED2);
higedura 0:9aa9be24636a 10 DigitalOut led3(LED3);
higedura 0:9aa9be24636a 11 DigitalOut led4(LED4);
higedura 0:9aa9be24636a 12
higedura 0:9aa9be24636a 13 SDFileSystem sd(p5, p6, p7, p8, "sd");
higedura 0:9aa9be24636a 14 Serial mavc(p9, p10);
higedura 0:9aa9be24636a 15 DigitalIn stop_sd(p11);
higedura 0:9aa9be24636a 16
higedura 0:9aa9be24636a 17 int main() {
higedura 0:9aa9be24636a 18
higedura 0:9aa9be24636a 19 pc.baud(921600);
higedura 0:9aa9be24636a 20 mavc.baud(115200);
higedura 0:9aa9be24636a 21
higedura 0:9aa9be24636a 22 char buf_char;
higedura 0:9aa9be24636a 23 unsigned int buf_hex[30] = {0};
higedura 0:9aa9be24636a 24 unsigned int buf_dec[10] = {0};
higedura 0:9aa9be24636a 25
higedura 0:9aa9be24636a 26 // rx
higedura 0:9aa9be24636a 27 double acc[3] = {0};
higedura 0:9aa9be24636a 28 double gyro[3] = {0};
higedura 0:9aa9be24636a 29 double azi;
higedura 0:9aa9be24636a 30 double alt;
higedura 0:9aa9be24636a 31 double GPS[2] = {0};
higedura 0:9aa9be24636a 32
higedura 0:9aa9be24636a 33 // tx
higedura 0:9aa9be24636a 34 double com[3] = {0};
higedura 0:9aa9be24636a 35 char H;
higedura 0:9aa9be24636a 36 char D[data];
higedura 0:9aa9be24636a 37 char test[9];
higedura 0:9aa9be24636a 38 H = 0x02; // header of dateset2
higedura 0:9aa9be24636a 39 D[0] = 0xff; D[1] = 0xff; // p_com
higedura 0:9aa9be24636a 40 D[2] = 0x7f; D[3] = 0xff; // q_com
higedura 0:9aa9be24636a 41 D[4] = 0x00; D[5] = 0x00; // r_com
higedura 0:9aa9be24636a 42 test[0] = 0x10; test[1] = 0x20; test[2] = 0x30;
higedura 0:9aa9be24636a 43 test[3] = 0x40; test[4] = 0x50; test[5] = 0x60;
higedura 0:9aa9be24636a 44 test[6] = 0x70; test[7] = 0x80; test[8] = 0x90;
higedura 0:9aa9be24636a 45
higedura 0:9aa9be24636a 46 //mkdir("/sd/mydir", 0777);
higedura 0:9aa9be24636a 47
higedura 0:9aa9be24636a 48 FILE *fp = fopen("/sd/sdtest.txt", "w");
higedura 0:9aa9be24636a 49 if(fp == NULL) {
higedura 0:9aa9be24636a 50 error("Could not open file for write\n");
higedura 0:9aa9be24636a 51 }
higedura 0:9aa9be24636a 52
higedura 0:9aa9be24636a 53 //pc.printf("\r\n\r\nYNU MPU\r\n\r\n");
higedura 0:9aa9be24636a 54
higedura 0:9aa9be24636a 55 //while(1){
higedura 0:9aa9be24636a 56 while(stop_sd==0){
higedura 0:9aa9be24636a 57 //for(int j=0;j<1800;j++){ flag tukuru // for 2 minuts
higedura 0:9aa9be24636a 58
higedura 0:9aa9be24636a 59 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 60 buf_char = mavc.getc();
higedura 0:9aa9be24636a 61 buf_hex[0] = (unsigned int)buf_char;
higedura 0:9aa9be24636a 62 for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
higedura 0:9aa9be24636a 63 // !!! switch bun ni suru
higedura 0:9aa9be24636a 64 if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
higedura 0:9aa9be24636a 65 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 66
higedura 0:9aa9be24636a 67 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 68 // send to mavc
higedura 0:9aa9be24636a 69 mavc.putc(H);
higedura 0:9aa9be24636a 70 for(int i=0;i<data;i++){ mavc.putc(D[i]); }
higedura 0:9aa9be24636a 71 //for(int i=0;i<9;i++){ mavc.putc(test[i]); }
higedura 0:9aa9be24636a 72 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 73
higedura 0:9aa9be24636a 74 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 75 // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
higedura 0:9aa9be24636a 76 // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
higedura 0:9aa9be24636a 77 // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
higedura 0:9aa9be24636a 78 // transrating hex to dec
higedura 0:9aa9be24636a 79 for(int i=0;i<10;i++){ buf_dec[i] = buf_hex[24-i*2]*256+buf_hex[23-i*2]; }
higedura 0:9aa9be24636a 80 // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
higedura 0:9aa9be24636a 81 // -> saisyuteki niha matrix ni simasu?
higedura 0:9aa9be24636a 82 for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec[i]-32768)/65535*20*9.8; }
higedura 0:9aa9be24636a 83 for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec[i+3]-32768)/65535*600/180*pi; }
higedura 0:9aa9be24636a 84 azi = (double)buf_dec[6]/65535*2*pi;
higedura 0:9aa9be24636a 85 alt = ((double)buf_dec[7]-32768)*0.1;
higedura 0:9aa9be24636a 86 for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec[i+8]-32768)*0.1; }
higedura 0:9aa9be24636a 87 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 88
higedura 0:9aa9be24636a 89 // ******************** control law ********************
higedura 0:9aa9be24636a 90 // transrating dec to hex
higedura 0:9aa9be24636a 91 //com
higedura 0:9aa9be24636a 92 // ******************** control law ********************
higedura 0:9aa9be24636a 93
higedura 0:9aa9be24636a 94 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]);
higedura 0:9aa9be24636a 95 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]);
higedura 0:9aa9be24636a 96
higedura 0:9aa9be24636a 97 // !!! fclose(fp);
higedura 0:9aa9be24636a 98
higedura 0:9aa9be24636a 99 }
higedura 0:9aa9be24636a 100
higedura 0:9aa9be24636a 101 }
higedura 0:9aa9be24636a 102
higedura 0:9aa9be24636a 103 fclose(fp);
higedura 0:9aa9be24636a 104 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:9aa9be24636a 105
higedura 0:9aa9be24636a 106 }