Diff: Imu.cpp

Revision:

75:215dd63032d6

Parent:

74:3b5470050420

--- a/Imu.cpp	Wed Mar 20 04:53:47 2019 +0000
+++ b/Imu.cpp	Fri Mar 22 00:59:32 2019 +0000
@@ -11,13 +11,14 @@
     static  UCHAR   buf[RCV_BUF_SIZE]={0};
     static  int     idxBufByG=0;
     static bool flg = false;
+    static UINT32   buf_over_count = 0;
+    static UINT32   unmatch_count = 0;
+    static UINT32   ang_over_range_count = 0;
 
-    float   tmpYaw;
-    int32_t tmpGyroZ;
-    
     if(rxChar != LF){
         if(idxBufByG >= RCV_BUF_SIZE - 1){
             flg = true;//オーバー時のデータは使わない
+            buf_over_count++;
         }
         else{
             buf[idxBufByG] = rxChar;//バッファに詰めて
@@ -30,16 +31,26 @@
             string                 str((char*)buf);
             std::vector<string>    list = f_Split(str,HT);//水平タブで切ってベクタにする
             
-            if(list.size() == 2){
-                //ヨー角
-                tmpYaw= (float)atof(list.at(0).c_str());
-                if (!isnan(tmpYaw) && (-180.0 < tmpYaw && 180.0 > tmpYaw)){
-                    yaw   = wrapAroungGuard(tmpYaw);
+            if(list.size() == 4){
+                float   tmpYaw1,tmpYaw2;
+                int32_t tmpGyroZ1,tmpGyroZ2;
+                //ヨー角,Z軸角速度
+                tmpYaw1 = (float)atof(list.at(0).c_str());
+                tmpGyroZ1 = (int32_t)( strtol( list.at(1).c_str() , NULL , 16) );
+                tmpYaw2 = (float)atof(list.at(2).c_str());
+                tmpGyroZ2 = (int32_t)( strtol( list.at(3).c_str() , NULL , 16) );
+                //通信時の化け防止 2つのデータがそれぞれ同じであることを確認する
+                if (tmpYaw1 == tmpYaw2 && tmpGyroZ1 == tmpGyroZ2){
+                    //Nanチェックと範囲外の値が入ってないかチェック
+                    if (!isnan(tmpYaw1) && (-180.0 < tmpYaw1 && 180.0 > tmpYaw1)){
+                        yaw   = wrapAroungGuard(tmpYaw1);
+                    }
+                    else{ang_over_range_count++;}
+                    if (!isnan(tmpGyroZ1)){
+                        gyroZ = tmpGyroZ1;
+                    }
                 }
-                tmpGyroZ = (int32_t)( strtol( list.at(1).c_str() , NULL , 16) );//Z軸角速度   
-                if (!isnan(tmpYaw)){
-                    gyroZ = tmpGyroZ;
-                }
+                else {unmatch_count++;}
             }
         }
         //バッファクリア
@@ -49,6 +60,10 @@
         flg = false;
         led4 = !led4;
     }
+    if (gf_Print.d1.bf.err){
+        sp.printf("IMU NG COUNT buf: %u, unmatch: %u, angNG: %u\n",buf_over_count, unmatch_count, ang_over_range_count);
+        gf_Print.d1.bf.err=false;
+    }
 }
 //----------------------------------------------------------------
 //ラップアラウンド防止 180度からマイナスへ飛ばないようにする