Diff: main.cpp

Revision:

4:6f167145f4b0

Parent:

3:32843000531e

--- a/main.cpp	Fri Jan 14 13:31:58 2022 +0000
+++ b/main.cpp	Fri Jan 14 15:09:03 2022 +0000
@@ -35,9 +35,9 @@
 //=========================================================
 //IMUの変数
 float IMU_BUFFER = 50;//慣性センサをsdに記録する前にためておくバッファの大きさ
-float time_i[7];//慣性センサの計測時刻
-float gyro_val[7][3];//角速度の値
-float acc_val[7][3];//加速度の値
+float time_i[20];//慣性センサの計測時刻
+float gyro_val[20][3];//角速度の値
+float acc_val[20][3];//加速度の値
 
 int imu_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
 //=========================================================
@@ -46,6 +46,10 @@
 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である
 float velN_float,velE_float,velD_float; // NED座標系に置ける速度
 
+float time_g[20];//gnssの計測時刻
+float gnss_val[20][6];//gnss受信機から得た値
+
+int gnss_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
 //=========================================================
 //UBXデータを処理したかどうかのフラグ
 int flag_posllh,flag_velned;
@@ -327,56 +331,39 @@
     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軸の加速度の算出   
+    
+    imu_counter++;
     //=========================================
     
-    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++;
-    }
-    
     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((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);
+            time_g[gnss_counter] = processing_timer.read_us();
+            gnss_val[gnss_counter][0] = latitude;
+            gnss_val[gnss_counter][1] = longitude;
+            gnss_val[gnss_counter][2] = height_float;
+            gnss_val[gnss_counter][3] = velN_float;
+            gnss_val[gnss_counter][4] = velE_float;
+            gnss_val[gnss_counter][5] = 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);
             
-            
-            /*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;
+            
+            gnss_counter++;
+            
         }else{}//if((flag_posllh==1)&&(flag_velned==1)){   
     }else{}//if(((logging_counter+1)%20)==0){
     
@@ -448,6 +435,7 @@
     flag_velned=0;
     
     imu_counter = 0;
+    gnss_counter = 0;
     logging_counter = 0;
     
     //-------------------------------------------  
@@ -458,7 +446,16 @@
     
     processing_timer.start();//timer starts
     
-    while(1) {     
+    while(1) {
+        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;
+        
+        for (int i = 0; i < gnss_counter; i++){
+            fprintf(fp,"%f,%f,%f,%f,%f,%f,%f\r\n",time_g[i],gnss_val[i][0],gnss_val[i][1],gnss_val[i][2],gnss_val[i][3],gnss_val[i][4],gnss_val[i][5]);
+        }
+        gnss_counter = 0;
         }//while
         
 }
\ No newline at end of file