Diff: main.cpp

Revision:

1:8757d12d193b

Parent:

0:91d5f51f73e2

Child:

2:9216162a9e17

--- a/main.cpp	Mon Apr 26 16:15:02 2021 +0000
+++ b/main.cpp	Wed Apr 28 06:59:04 2021 +0000
@@ -5,12 +5,20 @@
 //2021/04/26  A.Toda
 //========================================================
 #include "mbed.h"
-
+#include "SDFileSystem.h"
 //=========================================================
 //Port Setting
 Serial pc(USBTX, USBRX); // tx, rx
 SPI spi(p11, p12, p13);  // mosi, miso, sclk
 DigitalOut CS(p15);      // NEO-7MのCSピン
+DigitalIn log_switch(p16);      // 
+
+//SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
+SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs
+
+//ファイルポインタ
+FILE *fp;
+FILE *im;
 
 //=========================================================
 //受信したメッセージから抽出したい情報
@@ -28,7 +36,7 @@
 
 //=========================================================
 //処理時間
-int processed_time,processed_time_before,processed_time_after;
+int processed_time,processed_time_before,processed_time_after,measurement_time_g;
 
 //=========================================================
 //Ticker
@@ -37,12 +45,13 @@
 
 //=========================================================
 //Logging variables
-float imu_mesurement_freq = 400; //Hz
-float gnss_mesurement_freq = 5;  //theta_update_freq;
+float imu_mesurement_freq = 200.0; //Hz
+float gnss_mesurement_freq = 5.0;  //theta_update_freq;
 
 float imu_interval = 1.0f/imu_mesurement_freq;   //sec
 float gnss_interval = 1.0f/gnss_mesurement_freq; //sec
 
+int logging_status;
 //=========================================================
 //Header char
 const unsigned char UBX_HEADER[]        = { 0xB5, 0x62 };
@@ -227,20 +236,27 @@
              longitude=ubxMessage.navPosllh.lon/10000000.0f;
              height_float=float(ubxMessage.navPosllh.height);
              
+             //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
+       
+        
              flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる
+             //pc.printf("flag_posllh=%d\r\n",flag_posllh);
+             
               }
           else if(currentMsgType==MT_NAV_VELNED){
               velN_float=float(ubxMessage.navVelned.velN);
               velE_float=float(ubxMessage.navVelned.velE);
               velD_float=float(ubxMessage.navVelned.velD);
               
+              //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
               flag_velned=1;//速度情報を読み取った合図としてフラグを立てる
+              //pc.printf("flag_velned=%d\r\n",flag_velned);
               
               }
           else if(currentMsgType==MT_NAV_STATUS){
               
               }
-          
+          else{}
           //return currentMsgType; 
         }
       }
@@ -262,40 +278,60 @@
    processed_time_after = processing_timer.read_us();// captureing prossing time
    processed_time=processed_time_after-processed_time_before;
    
+   /*processGPSの処理時間の表示*/
+   //pc.printf("processed_time_after(us)=%d;",(processed_time_after));
+   //pc.printf("processed_time(us)=%d\r\n",(processed_time));
+   //pc.printf("%d,%d\r\n",processed_time_after,processed_time);
+   
 }
 
 void imu_mesurement()
 {
+    
+    if(log_switch==1){
+        logging_status=1;
+    }else if(log_switch==0){
+        logging_status=0;
+    }else{}
+    
 }
 
 void ublox_logging()
 {
     //detach the rotary imu mesurement
     timer1.detach();
-
-    for(int message_number=0; message_number<3;message_number++){
-        processGPS();
-    }
+    
+    processGPS();
+    processGPS();
+    processGPS();
+    
+    measurement_time_g = processing_timer.read_us();
+    //pc.printf("%d\r\n",measurement_time_g);// captureing prossing time
         
+    if(logging_status==1){
+        fprintf(fp, "%d,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float);
+    }else if(logging_status==0){}
+            
     //位置と速度情報を読み取った場合
     if((flag_posllh==1)&&(flag_velned==1)){
         /*Teratermでロギングする用の表示*/
-        pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float);
+        //pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float);
         /*計測ではなくデバッグ用の表示*/
         //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
         //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
-        //pc.printf("processed_time(us)=%d\r\n",processed_time);
-            
+        
+        
         /*processGPSの処理時間の表示*/
         //pc.printf("processed_time_before(us)=%d\r\n",(processed_time_before));
         //pc.printf("processed_time_after(us)=%d\r\n",(processed_time_after));
+        //pc.printf("processed_time(us)=%d\r\n",processed_time);    
             
         /*フラグを0に戻す*/
         flag_posllh=0;
         flag_velned=0;
             
     }else{}
-        
+    
     //attach a timer for imu mesurement (400 Hz)
     timer1.attach(&imu_mesurement, imu_interval);
 }
@@ -307,6 +343,21 @@
     //UART initialization
     pc.baud(460800); //115.2 kbps
     
+    spi.frequency(5500000);
+    
+    mkdir("/sd/mydir",0777);//SDファイル作成
+    fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外で行う
+    //im = fopen("/sd/mydir/imu.txt", "a");
+    
+    if(fp == NULL) {
+       error("Could not open file for write\n");
+       }else{}
+    
+    pc.printf("FO\r\n");//file open
+    logging_status=1;
+    
+    wait(0.1);
+    
     //フラグのリセット
     flag_posllh=0;
     flag_velned=0;
@@ -322,7 +373,18 @@
     processing_timer.start();//timer starts
     
     while(1) {
+        pc.printf("T2D\r\n");
+        timer2.detach();
         
+        if(logging_status==0){
+            fclose(fp);
+            pc.printf("FC\r\n");
+            timer2.detach();
+            timer1.detach();
+        }else if(logging_status==1){}
+        
+        timer2.attach(&ublox_logging, gnss_interval);
+        wait(0.8);
         //        
         }//while