Diff: main.cpp

Revision:

0:9aa9be24636a

Child:

1:ca296cfa603b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jan 06 06:37:09 2013 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "SDFileSystem.h"
+
+#define pi      3.1416
+#define data    6
+
+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() {
+    
+    pc.baud(921600);
+    mavc.baud(115200);
+    
+    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
+    test[0] =   0x10;   test[1] =   0x20;      test[2] =   0x30;
+    test[3] =   0x40;   test[4] =   0x50;      test[5] =   0x60;
+    test[6] =   0x70;   test[7] =   0x80;      test[8] =   0x90;
+    
+    //mkdir("/sd/mydir", 0777);
+    
+    FILE *fp = fopen("/sd/sdtest.txt", "w");
+    if(fp == NULL) {
+        error("Could not open file for write\n");
+    }
+        
+    //pc.printf("\r\n\r\nYNU MPU\r\n\r\n");
+    
+    //while(1){
+    while(stop_sd==0){
+    //for(int j=0;j<1800;j++){      flag tukuru     // for 2 minuts
+        
+        // ******************** 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];  }
+        // !!! switch bun ni suru
+        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 ********************
+            
+            // ******************** control law ********************
+            // transrating dec to hex
+            //com
+            // ******************** control law ********************
+            
+            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]);
+            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]);
+                        
+            // !!! fclose(fp); 
+            
+        }
+             
+    }
+    
+    fclose(fp); 
+    led1    =   1;      led2    =   1;      led3    =   1;      led4    =   1;
+    
+}