yal kaiyo / Mbed 2 deprecated 130110_YNU_MPU_1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of YNU_MPU_0108 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 }