Diff: main.cpp

Revision:

3:32843000531e

Parent:

2:940851e53dde

Child:

4:6f167145f4b0

diff -r 940851e53dde -r 32843000531e main.cpp
--- a/main.cpp	Fri Jan 14 11:06:11 2022 +0000
+++ b/main.cpp	Fri Jan 14 13:31:58 2022 +0000
@@ -1,26 +1,46 @@
 //=========================================================
-//GNSS logger with ublox-NEO7M 
+//IMU/GNSS logger with ublox-NEO7M 
 //MPU board:  mbed LPC1768
 //GNSS module: ublox-NEO7M
-//2021/04/26  A.Toda
+//2022/01/14  A.Toda
 //========================================================
 #include "mbed.h"
+#include "MG354PDH0.h"
 #include "SDFileSystem.h"
 //=========================================================
 //Port Setting
-Serial pc(USBTX, USBRX); // tx, rx
-SPI spi(p11, p12, p13);  // mosi, miso, sclk
+Serial pc(USBTX, USBRX); // tx, rx            // PCとの通信用シリアルポート
+Serial   epson_imu(p9, p10);                  // IMU通信用シリアルポート
+SPI spi(p11, p12, p13);  // mosi, miso, sclk  SPIを使用するセンサのバス
 DigitalOut CS(p14);      // NEO-7MのCSピン
-DigitalIn log_switch(p15);      // 
+
+DigitalIn log_switch(p15); //記録終了用のスイッチ
+
+AnalogIn thermopile_input_1(p20); //アナログ電圧の読み取り
+AnalogIn thermopile_input_2(p19); //アナログ電圧の読み取り
 
-//SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
+DigitalOut myled_1(LED1);
+DigitalOut myled_2(LED2);
+DigitalOut myled_3(LED3);
+DigitalOut myled_4(LED4);
+
+//ライブラリとポートの対応づけ
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs
+MG354PDH0        imu(&epson_imu);                     // IMU
 
 //ファイルポインタ
 FILE *fp;
 FILE *im;
 
 //=========================================================
+//IMUの変数
+float IMU_BUFFER = 50;//慣性センサをsdに記録する前にためておくバッファの大きさ
+float time_i[7];//慣性センサの計測時刻
+float gyro_val[7][3];//角速度の値
+float acc_val[7][3];//加速度の値
+
+int imu_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
+//=========================================================
 //受信したメッセージから抽出したい情報
 float latitude,longitude,height_float;  //緯度、経度、高度
 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である
@@ -36,7 +56,7 @@
 
 //=========================================================
 //処理時間
-int processed_time,processed_time_before,processed_time_after,measurement_time_g;
+int processed_time,processed_time_before,processed_time_after,measurement_time_g,measurement_time_i;
 
 //=========================================================
 //Ticker
@@ -45,7 +65,11 @@
 
 //=========================================================
 //Logging variables
-float imu_mesurement_freq = 200.0; //Hz
+
+//Logging counter 100Hzの割り込みで何回目であるかを0から99でカウント
+int logging_counter;
+
+float imu_mesurement_freq = 100.0; //Hz
 float gnss_mesurement_freq = 5.0;  //theta_update_freq;
 
 float imu_interval = 1.0f/imu_mesurement_freq;   //sec
@@ -280,7 +304,7 @@
    
    /*processGPSの処理時間の表示*/
    //pc.printf("processed_time_after(us)=%d;",(processed_time_after));
-   pc.printf("processed_time(us)=%d\r\n",(processed_time));
+   //pc.printf("processed_time(us)=%d\r\n",(processed_time));
    //pc.printf("%d,%d\r\n",processed_time_after,processed_time);
    
 }
@@ -297,61 +321,123 @@
 
 void ublox_logging()
 {
-    //detach the rotary imu mesurement
-    timer1.detach();
     
-    processGPS();
-    processGPS();
-    processGPS();
+    //============慣性センサのロギング============
+    time_i[imu_counter] = processing_timer.read_us();//慣性センサの計測時刻
+    gyro_val[imu_counter][0]=imu.read_angular_rate_x();//X軸周りの角速度の算出
+    gyro_val[imu_counter][1]=imu.read_angular_rate_y();//Y軸周りの角速度の算出
+    gyro_val[imu_counter][2]=imu.read_angular_rate_z();//Z軸周りの角速度の算出
+        
+    acc_val[imu_counter][0]=imu.read_acceleration_x();//X軸の加速度の算出
+    acc_val[imu_counter][1]=imu.read_acceleration_y();//Y軸の加速度の算出
+    acc_val[imu_counter][2]=imu.read_acceleration_z();//Z軸の加速度の算出   
+    //=========================================
+    
+    if(((imu_counter+1)%5)==0){
+        for (int i = 0; i < imu_counter; i++){
+            fprintf(im,"%f,%f,%f,%f,%f,%f,%f\r\n",time_i[i],gyro_val[i][0],gyro_val[i][1],gyro_val[i][2],acc_val[i][0],acc_val[i][1],acc_val[i][2]);
+        }
+        imu_counter = 0;
+    }else{
+        imu_counter++;
+    }
     
-    measurement_time_g = processing_timer.read_us();
-    //pc.printf("%d\r\n",measurement_time_g);// captureing prossing time
+    if(((logging_counter+1)%20)==0){
+        //detach the rotary imu mesurement
+     
+        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(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)){
+            fprintf(fp, "%d,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float);
+            pc.printf("U\r\n");
+            /*Teratermでロギングする用の表示*/
+            //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);
             
-    //位置と速度情報を読み取った場合
-    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("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);
-        
+            
+            /*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{}//if((flag_posllh==1)&&(flag_velned==1)){   
+    }else{}//if(((logging_counter+1)%20)==0){
+    
+    
+    if((logging_counter+1)>=100){
+        logging_counter = 0;
+    }else{
+        logging_counter++;
+    }
+    
+    if(log_switch==1){
+        logging_status=1;
+    }else if(log_switch==0){
+        logging_status=0;
+        fclose(fp);
+        fclose(im);
+        pc.printf("FC\r\n");
+        timer1.detach();
         
-        /*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;
-            
+        //ロガーの動作状態を見るためのLED
+        //ロガーが記録を終了した表示をする
+        myled_1 = 0;
+        myled_2 = 1;
+        
     }else{}
     
-    //attach a timer for imu mesurement (400 Hz)
-    timer1.attach(&imu_mesurement, imu_interval);
+    
+    
 }
 
 
 /*--------------------------------------------*/
 int main() {
     
+    //power on wait 800ms form IMU
+    wait(1.0);
+    
+    //IMU initialize
+    imu.power_on_sequence1();//IMUが動作可能かどうかの確認
+    imu.power_on_sequence2();//IMUが動作可能かどうかの確認
+    imu.UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
+    imu.move_to_sampling_mode();//サンプリングモードへの移行
+    
     //UART initialization
     pc.baud(460800); //115.2 kbps
     
     spi.frequency(1000000);
     
     mkdir("/sd/mydir",0777);//SDファイル作成
-    fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外で行う
-    //im = fopen("/sd/mydir/imu.txt", "a");
+    fp = fopen("/sd/mydir/gps.csv", "a");//最初のSDopen時間かかるのでwhile外で行う
+    im = fopen("/sd/mydir/imu.csv", "a");
     
     if(fp == NULL) {
        error("Could not open file for write\n");
        }else{}
     
+    //ロガーの動作状態を見るためのLED
+    //初期状態ではロガーが記録中の表示をする
+    myled_1 = 1;
+    myled_2 = 0;
+    
     pc.printf("FO\r\n");//file open
     logging_status=1;
     
@@ -361,31 +447,18 @@
     flag_posllh=0;
     flag_velned=0;
     
+    imu_counter = 0;
+    logging_counter = 0;
+    
     //-------------------------------------------  
     //Timer
     //-------------------------------------------  
-    //timer1: imu mesurement, 400 Hz
-    timer1.attach(&imu_mesurement, imu_interval);
-    //timer2: GNSS mesurement, 5 Hz
-    timer2.attach(&ublox_logging, gnss_interval);
+    //timer1: imu mesurement, 100 Hz
+    timer1.attach(&ublox_logging, imu_interval);
     
     processing_timer.start();//timer starts
     
-    while(1) {
-        pc.printf("T2D\r\n");
-        timer2.detach();
+    while(1) {     
+        }//while
         
-        if(logging_status==0){
-            fclose(fp);
-            pc.printf("FC\r\n");
-            timer2.detach();
-            timer1.detach();
-            break;
-        }else if(logging_status==1){}
-        
-        timer2.attach(&ublox_logging, gnss_interval);
-        wait(0.8);
-        //        
-        }//while
-  
 }
\ No newline at end of file