From 73d33356ad4b2bca1b923c724cd5fdadf1cd526f Mon Sep 17 00:00:00 2001
From: Alexander Malishev <malishev@gmail.com>
Date: Sun, 27 May 2018 18:37:43 +0400
Subject: [PATCH] HAL_ChibiOS: detect overcapture in SoftSigReaderInt.cpp

---
 libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp | 41 +++++++++++++++++--
 .../hwdef/scripts/chibios_hwdef.py            |  1 +
 2 files changed, 38 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
index 916dc62230..1b5288afd9 100644
--- a/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
+++ b/libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
@@ -24,9 +24,6 @@ extern const AP_HAL::HAL& hal;
 
 #if HAL_USE_EICU == TRUE
 
-#define eicu_lld_invert_polarity(eicup, channel)                              \
-  (eicup)->tim->CCER ^= ((uint16_t)(STM32_TIM_CCER_CC1P << ((channel) * 4)))
-
 // singleton instance
 SoftSigReaderInt *SoftSigReaderInt::_instance;
 
@@ -57,11 +54,47 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
     eicuEnable(_icu_drv);
 }
 
+inline void invert_polarity(EICUDriver *eicup, eicuchannel_t channel)
+{
+    eicup->tim->CCER ^= STM32_TIM_CCER_CC1P << (channel * 4);
+}
+
+//check for CCxOF, if it was set by timer hw, it means we missed 2 or more transitions
+inline bool overcapture_occured(EICUDriver *eicup, eicuchannel_t channel)
+{
+    bool result = (eicup->tim->SR & (STM32_TIM_SR_CC1OF << channel)) != 0;
+    eicup->tim->SR &= ~(STM32_TIM_SR_CC1OF << channel);
+    return result;
+}
+
+//check for right pin polarity, if it is wrong it means we missed 1, 3, 5 or more transitions
+inline bool wrong_polarity(EICUDriver *eicup, eicuchannel_t channel)
+{
+    bool pin_high = palReadLine(RCININT_PIN);
+    bool waiting_rising = (eicup->tim->CCER & (STM32_TIM_CCER_CC1P << (channel * 4))) == 0;
+    if (pin_high && waiting_rising) {
+        return true;
+    }
+    if (!pin_high && !waiting_rising) {
+        return true;
+    }
+    return false;
+}
+
 void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t channel)
 {
     uint16_t value = eicup->tim->CCR[channel];
+    invert_polarity(eicup, channel);
     _instance->sigbuf.push(value);
-    eicu_lld_invert_polarity(eicup, channel);
+    
+    //check for missed interrupt 
+    if (overcapture_occured(eicup, channel) || wrong_polarity(eicup, channel)) {
+        //we have missed some pulses
+        //try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)        
+        _instance->sigbuf.push(value);
+        //second 0 width pulse to keep polarity right
+        _instance->sigbuf.push(value);
+    }
 }
 
 bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1)
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
index 264f16a75b..200824e0a8 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
@@ -701,6 +701,7 @@ def write_PWM_config(f):
         f.write('#define STM32_EICU_USE_TIM%u TRUE\n' % n)
         f.write('#define RCININT_EICU_TIMER EICUD%u\n' % n)
         f.write('#define RCININT_EICU_CHANNEL EICU_CHANNEL_%u\n' % int(chan_str))
+        f.write('#define RCININT_PIN HAL_GPIO_PIN_TIM%s_CH%s\n' % (timer_str, chan_str))
         f.write('\n')
 
     if alarm is not None: