From ac0d851e6811b4f8c678ed9d6458f1ac1d80ac7c Mon Sep 17 00:00:00 2001
From: Atsushi Watanabe <atsushi.w@ieee.org>
Date: Tue, 7 May 2019 15:42:00 +0900
Subject: [PATCH] Ignore single-shot hall signal error (#59)

---
 tfrog-motordriver/controlPWM.c      | 24 +++++++++++++++++++++---
 tfrog-motordriver/controlVelocity.h |  1 +
 tfrog-motordriver/main.c            |  6 ++++++
 3 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/tfrog-motordriver/controlPWM.c b/tfrog-motordriver/controlPWM.c
index f297196..61fb704 100644
--- a/tfrog-motordriver/controlPWM.c
+++ b/tfrog-motordriver/controlPWM.c
@@ -394,8 +394,11 @@ void FIQ_PWMPeriod()
         //if( halldiff == 3 || halldiff >= 5 ) printf( "ENC error: %x->%x\n\r", _hall[i], hall[i] );
         // ホール素子信号が全相1、全相0のとき
         // ホース素子信号が2ビット以上変化したときはエラー
-        if (driver_state.error.hall[i] < 128)
+
+        // Skip next one error to avoid counting another edge of this error.
+        if (driver_state.error.hall[i] < 12)
           driver_state.error.hall[i] += 12;
+
         if (driver_state.error.hall[i] > 12)
         {
           // エラー検出後、1周以内に再度エラーがあれば停止
@@ -538,8 +541,23 @@ void FIQ_PWMPeriod()
         // In worst case, initial encoder origin can have offset of motor_param[i].enc_rev/12.
         if (_abs(err) > motor_param[i].enc_rev / 6)
         {
-          motor[i].error_state |= ERROR_HALL_ENC;
-          printf("PWM:enc-hall err (%d)\n\r", err);
+          // Skip next one error to avoid counting another edge of this error.
+          if (driver_state.error.hallenc[i] < 12)
+            driver_state.error.hallenc[i] += 12;
+
+          if (driver_state.error.hallenc[i] > 12)
+          {
+            // Enter error stop mode if another error occurs within one revolution
+            motor[i].error_state |= ERROR_HALL_ENC;
+            printf("PWM:enc-hall err (%d)\n\r", err);
+          }
+          // Don't apply erroneous absolute angle
+          continue;
+        }
+        else
+        {
+          if (driver_state.error.hallenc[i] > 0)
+            driver_state.error.hallenc[i]--;
         }
       }
 
diff --git a/tfrog-motordriver/controlVelocity.h b/tfrog-motordriver/controlVelocity.h
index 585a9a1..8badde7 100644
--- a/tfrog-motordriver/controlVelocity.h
+++ b/tfrog-motordriver/controlVelocity.h
@@ -139,6 +139,7 @@ typedef struct _DriverState
   {
     unsigned char low_voltage;
     unsigned char hall[2];
+    unsigned char hallenc[2];
   } error;
   enum
   {
diff --git a/tfrog-motordriver/main.c b/tfrog-motordriver/main.c
index 9a36655..b9f6d42 100644
--- a/tfrog-motordriver/main.c
+++ b/tfrog-motordriver/main.c
@@ -593,6 +593,8 @@ int main()
   driver_state.error.low_voltage = 0;
   driver_state.error.hall[0] = 0;
   driver_state.error.hall[1] = 0;
+  driver_state.error.hallenc[0] = 0;
+  driver_state.error.hallenc[1] = 0;
 
   printf("Velocity Control init\n\r");
   // Configure velocity control loop
@@ -704,6 +706,8 @@ int main()
   driver_state.error.low_voltage = 0;
   driver_state.error.hall[0] = 0;
   driver_state.error.hall[1] = 0;
+  driver_state.error.hallenc[0] = 0;
+  driver_state.error.hallenc[1] = 0;
   driver_state.ifmode = 0;
   driver_state.watchdog = 0;
   // Driver loop
@@ -761,6 +765,8 @@ int main()
       }
       driver_state.error.hall[0] = 0;
       driver_state.error.hall[1] = 0;
+      driver_state.error.hallenc[0] = 0;
+      driver_state.error.hallenc[1] = 0;
       motor[0].error_state |= ERROR_WATCHDOG;
       motor[1].error_state |= ERROR_WATCHDOG;
       printf("Watchdog - init parameters\n\r");