Revision 75:215dd63032d6, committed 2019-03-22

Comitter:

YutakaTakagi

Date:

Fri Mar 22 00:59:32 2019 +0000

Parent:

74:3b5470050420

Commit message:

add yaw and gyroZ check for New Arduino Code

Changed in this revision

diff -r 3b5470050420 -r 215dd63032d6 Imu.cpp
--- 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度からマイナスへ飛ばないようにする

diff -r 3b5470050420 -r 215dd63032d6 globalFlags.cpp
--- a/globalFlags.cpp	Wed Mar 20 04:53:47 2019 +0000
+++ b/globalFlags.cpp	Fri Mar 22 00:59:32 2019 +0000
@@ -69,7 +69,7 @@
     
     gf_BlinkLED = false;
 
-    gf_EnbImuRead = false;
+    gf_EnbImuRead = true;
     
 //    gf_Print.flg = 0;
 //    gf_Mon.flg = 0;

diff -r 3b5470050420 -r 215dd63032d6 globalFlags.h
--- a/globalFlags.h	Wed Mar 20 04:53:47 2019 +0000
+++ b/globalFlags.h	Fri Mar 22 00:59:32 2019 +0000
@@ -81,6 +81,7 @@
             bool    d    : 1;//!< D制御ゲイン
             bool    v    : 1;//!< PID2(torgue)角速度feedback制御用ゲイン
             bool    stat : 1;//!< ステート表示
+            bool    err  : 1;//!< errorカウント表示
         }bf;
     }d1;
     union{

diff -r 3b5470050420 -r 215dd63032d6 hbCommand.cpp
--- a/hbCommand.cpp	Wed Mar 20 04:53:47 2019 +0000
+++ b/hbCommand.cpp	Fri Mar 22 00:59:32 2019 +0000
@@ -53,6 +53,7 @@
     else if (strcmp(g_CmdBuf , "rg"  )==0 ){gf_Print.d1.bf.gy=true;}//リードジャイロ  
     else if (strcmp(g_CmdBuf , "ry"  )==0 ){gf_Print.d1.bf.yaw=true;}//リード姿勢角ヨー
     else if (strcmp(g_CmdBuf , "rimu")==0 ){gf_Print.d1.bf.yaw=true;gf_Print.d1.bf.gy=true;}//リードIMU
+    else if (strcmp(g_CmdBuf , "rerr")==0 ){gf_Print.d1.bf.err=true;}//リードエラーカウント
     else if (strcmp(g_CmdBuf , "rs"  )==0 ){gf_Print.d2.bf.sw=true;}//リードスイッチ
     else if (strcmp(g_CmdBuf , "rfb" )==0 ){gf_Print.d1.bf.fb=true;}//PID制御フィードバック結果
     else if (strcmp(g_CmdBuf , "rp"  )==0 ){gf_Print.d1.bf.pp=true;gf_Print.d1.bf.p=true;gf_Print.d1.bf.i=true;gf_Print.d1.bf.d=true;}//パラメータ全表示