Diff: main.cpp

Revision:

5:192835ac6106

Parent:

4:037fab837823

Child:

6:3792b68b1fc9

--- a/main.cpp	Tue Jan 08 12:22:43 2013 +0000
+++ b/main.cpp	Tue Jan 08 12:54:47 2013 +0000
@@ -34,7 +34,9 @@
     //pc.baud(921600);
     mavc.baud(115200);
     
-    int flag    =   0;
+    int flag_lc =   0;
+    int flag_cm =   0;
+    int loop    =   1200;       // for 2 minuts
     
     char  buf_char;
     unsigned int    buf_hex[30] =   {0};
@@ -123,6 +125,7 @@
     //********************* HOSHINO *********************
     
     //********************* OHTSU ***********************
+    
     //********************* OHTSU ***********************
     
     FILE *fp = fopen("/sd/sdtest.txt", "w");
@@ -130,13 +133,28 @@
         error("Could not open file for write\n");
     }
     
-    while(flag<120){      //flag tukuru     // for 2 minuts
+    // ******************** waiting @LC ********************
+    while(flag_lc==0){
+        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];  }
+        if(buf_hex[29]==0x40 && buf_hex[28]==0x4C && buf_hex[27]==0x43 && buf_hex[26]==0x0D){
+        // ******************** waiting @LC ********************
+            
+            // ******************** homotopy ********************
+            
+            flag_lc =   1;          // end of homotopy
+            // ******************** homotopy ********************
+            
+        }   
+    }
+    
+    while(flag_cm<loop){
         
         // ******************** 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 ********************
             
@@ -222,7 +240,7 @@
             // !!! HOSHINO !!! fprintf add guidance, velocity and control
             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]);
             
-            flag++;
+            flag_cm++;
             
         }