Diff: TVDCTRL.cpp

Revision:

41:0c53acd31247

Parent:

40:8e33c60c6590

Child:

42:3ab09d0e3071

--- a/TVDCTRL.cpp	Thu Oct 26 07:58:00 2017 +0000
+++ b/TVDCTRL.cpp	Fri Oct 27 09:11:31 2017 +0000
@@ -216,7 +216,7 @@
         //ブレーキオーバーライドチェック
         if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25))   //Brake-ON and APS > 25%
             errCounter.brakeOverRide++;
-        if(tmpApsP < APS_OVERRIDE05)   //Brake-ON and APS < 5%
+        if(tmpApsP < APS_OVERRIDE05)   //APS < 5%
             errCounter.brakeOverRide=0;
 
         //センサ値取得
@@ -237,7 +237,6 @@
 volatile int gRightWheelPulseCounter = 0, gLeftWheelPulseCounter = 0;
 //パルス入力までの時間
 volatile int gRightWheelPulse_dT2 = 0, gLeftWheelPulse_dT2 = 0;
-
 volatile float gRightWheelRPS = 0, gLeftWheelRPS = 0;
 
 volatile bool pulseTimeISRFlag = false;
@@ -273,7 +272,8 @@
     int rwpCounter = gRightWheelPulseCounter;
     static int rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;  //初期設定は無限時間前にパルス入力があったと仮定
     int rwp_dT2 = gRightWheelPulse_dT2;
-    int currentTime = wheelPulseTimer.read_us();
+    static bool preInputState = false;
+    static int rwpStopCounter = 0;                //タイヤが止まっている間カウントアップ
 
     //パルス数クリア
     gRightWheelPulseCounter = 0;
@@ -282,29 +282,41 @@
     gRightWheelPulse_dT2 = 0;
     //次回計測周期までのパルス時間計測開始
     wheelPulseTimer.reset();
-    
-    //計測周期内にパルス入力なければ
-    if(rwpCounter == 0) {
+
+    //前回パルス入力がない場合
+    if(preInputState == false) {
         //以前のdT1に計測周期の時間を積算
         rwp_dT1 = rwp_dT1 + TIRE_MEAS_CYCLE_US;
         if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US)
-            rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;      //overflow防止
-        //(dT2は0であるはず)
-    } else {
-        //パルス入力あれば直前のパルス入力からの経過時間取得
-        rwp_dT2 = currentTime - rwp_dT2;
+            rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;  //overflow防止
+    }
+
+    //パルス入力あれば直前のパルス入力からの経過時間取得
+    if(rwpCounter != 0) {
+        rwp_dT2 = TIRE_MEAS_CYCLE_US - rwp_dT2;
     }
-    
-    //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
-    if(rwpCounter == 0)
-        gRightWheelRPS = 1.0f / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f) / WHEEL_PULSE_NUM;
-    else{
-        gRightWheelRPS = (float)rwpCounter / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f) / WHEEL_PULSE_NUM;
-//    printf("%d %d\r\n", rwp_dT1, rwp_dT2);
+
+    //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定
+    if(rwpCounter == 0) {
+        if(rwpStopCounter < 100) {
+            rwpStopCounter++;
+        } else {
+            gRightWheelRPS = 1.0f / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f) / WHEEL_PULSE_NUM;
         }
+    } else {
+        //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
+        gRightWheelRPS = ((float)rwpCounter / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f)) / WHEEL_PULSE_NUM;
+        rwpStopCounter = 0;
+    }
+
     //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
     if(rwpCounter != 0)
         rwp_dT1 = rwp_dT2;
+
+    if(rwpCounter == 0)
+        preInputState = false;
+    else
+        preInputState = true;
 }
 
 void getPulseCounterISR(void)
@@ -578,8 +590,10 @@
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
-    
-    printf("%05.2f\r\n",gRightWheelRPS*60.0f);
+
+//    printf("%04d\r\n",gRightWheelPulseCounter);
+    printf("%f\r\n",gRightWheelRPS*60.0f);
+//    printf("%09d %09d %09d\r\n",rwp_dT1, rwp_dT2, rwpCounter);
 
     if(isRedyToDrive && isBrakeOn())
         readyToDriveFlag = 0;
@@ -667,6 +681,9 @@
     rightMotorPulse.fall(&countRightMotorPulseISR);
     leftMotorPulse.fall(&countLeftMotorPulseISR);
     rightWheelPulse1.fall(&countRightWheelPulseISR);
+//    rightWheelPulse2.fall(&countRightWheelPulseISR);
+//    rightWheelPulse1.rise(&countRightWheelPulseISR);
+//    rightWheelPulse2.rise(&countRightWheelPulseISR);
     leftWheelPulse1.fall(&countLeftWheelPulseISR);
 
     ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)