HAL_ChibiOS: use 2 channels in SoftSigReaderInt.cpp

libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp

@@ -24,6 +24,10 @@ extern const AP_HAL::HAL& hal;
 
 #if HAL_USE_EICU == TRUE
 
+#if STM32_EICU_USE_TIM10 || STM32_EICU_USE_TIM11 || STM32_EICU_USE_TIM13 || STM32_EICU_USE_TIM14
+#error "Timers with only one channel are not supported"
+#endif
+
 // singleton instance
 SoftSigReaderInt *SoftSigReaderInt::_instance;
 
@@ -32,68 +36,79 @@ SoftSigReaderInt::SoftSigReaderInt()
     _instance = this;
 }
 
+eicuchannel_t SoftSigReaderInt::get_pair_channel(eicuchannel_t channel)
+{
+    switch (channel) {
+        case EICU_CHANNEL_1:
+            return EICU_CHANNEL_2;
+        case EICU_CHANNEL_2:
+            return EICU_CHANNEL_1;
+        case EICU_CHANNEL_3:
+            return EICU_CHANNEL_4;
+        case EICU_CHANNEL_4:
+            return EICU_CHANNEL_3;
+        case EICU_CHANNEL_ENUM_END:
+            return EICU_CHANNEL_ENUM_END;
+    }
+    return EICU_CHANNEL_ENUM_END;
+}
+
 void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
 {
     last_value = 0;
     _icu_drv = icu_drv;
+    eicuchannel_t aux_chan = get_pair_channel(chan);
     icucfg.dier = 0;
     icucfg.frequency = INPUT_CAPTURE_FREQUENCY;
     for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) {
         icucfg.iccfgp[i]=nullptr;
     }
+    
+    //configure main channel
     icucfg.iccfgp[chan] = &channel_config;
 #ifdef HAL_RCIN_IS_INVERTED
     channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
 #else
     channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
 #endif
-    channel_config.capture_cb = _irq_handler;
+    channel_config.capture_cb = nullptr;
+    
+    //configure aux channel
+    icucfg.iccfgp[aux_chan] = &aux_channel_config;
+#ifdef HAL_RCIN_IS_INVERTED
+    aux_channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
+#else
+    aux_channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
+#endif
+    aux_channel_config.capture_cb = _irq_handler;
+
     eicuStart(_icu_drv, &icucfg);
     //sets input filtering to 4 timer clock
     stm32_timer_set_input_filter(_icu_drv->tim, chan, 2);
+    //sets input for aux_chan 
+    stm32_timer_set_channel_input(_icu_drv->tim, aux_chan, 2);
     eicuEnable(_icu_drv);
 }
 
-inline void invert_polarity(EICUDriver *eicup, eicuchannel_t channel)
+void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel)
 {
-    eicup->tim->CCER ^= STM32_TIM_CCER_CC1P << (channel * 4);
+    eicuchannel_t channel = get_pair_channel(aux_channel);
-}
+    uint16_t value1 = eicup->tim->CCR[channel];
+    uint16_t value2 = eicup->tim->CCR[aux_channel];
     
-//check for CCxOF, if it was set by timer hw, it means we missed 2 or more transitions
+    _instance->sigbuf.push(value1);
-inline bool overcapture_occured(EICUDriver *eicup, eicuchannel_t channel)
+    _instance->sigbuf.push(value2);
-{
-    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);
     
     //check for missed interrupt 
-    if (overcapture_occured(eicup, channel) || wrong_polarity(eicup, channel)) {
+    uint32_t mask = (STM32_TIM_SR_CC1OF << channel) | (STM32_TIM_SR_CC1OF << aux_channel);
+    if ((eicup->tim->SR & mask) != 0) {
         //we have missed some pulses
         //try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)
-        _instance->sigbuf.push(value);
+        _instance->sigbuf.push(value2);
         //second 0 width pulse to keep polarity right
-        _instance->sigbuf.push(value);
+        _instance->sigbuf.push(value2);
+        //reset overcapture mask
+        eicup->tim->SR &= ~mask;
     }
 }
 
@@ -101,7 +116,7 @@ bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1)
 {
     uint16_t p0, p1;
     if (sigbuf.available() >= 2) {
-        if (sigbuf.pop(p0)&&sigbuf.pop(p1)) {
+        if (sigbuf.pop(p0) && sigbuf.pop(p1)) {
             widths0 = uint16_t(p0 - last_value);
             widths1 = uint16_t(p1 - p0);
             last_value = p1;

libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h

@@ -46,9 +46,12 @@ private:
 
     static void _irq_handler(EICUDriver *eicup, eicuchannel_t channel);
     
+    static eicuchannel_t get_pair_channel(eicuchannel_t channel);
+
     ObjectBuffer<uint16_t> sigbuf{MAX_SIGNAL_TRANSITIONS};
     EICUConfig icucfg;
     EICUChannelConfig channel_config;
+    EICUChannelConfig aux_channel_config;
     EICUDriver* _icu_drv = nullptr;
     uint16_t last_value;
 };

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c

@@ -19,6 +19,9 @@
 #include <string.h>
 #include <stm32_dma.h>
 
+/*
+  setup the timer capture digital filter for a channel
+ */
 void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode)
 {
     switch (channel) {
@@ -37,6 +40,39 @@ void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t fil
     }
 }
 
+/*
+  set the input source of a timer channel
+ */    
+void stm32_timer_set_channel_input(stm32_tim_t *tim, uint8_t channel, uint8_t input_source)
+{
+    switch (channel) {
+        case 0:
+            tim->CCER &= ~STM32_TIM_CCER_CC1E;
+            tim->CCMR1 &= ~STM32_TIM_CCMR1_CC1S_MASK;
+            tim->CCMR1 |= STM32_TIM_CCMR1_CC1S(input_source);
+            tim->CCER |= STM32_TIM_CCER_CC1E;
+            break;
+        case 1:
+            tim->CCER &= ~STM32_TIM_CCER_CC2E;
+            tim->CCMR1 &= ~STM32_TIM_CCMR1_CC2S_MASK;
+            tim->CCMR1 |= STM32_TIM_CCMR1_CC2S(input_source);
+            tim->CCER |= STM32_TIM_CCER_CC2E;
+            break;
+        case 2:
+            tim->CCER &= ~STM32_TIM_CCER_CC3E;
+            tim->CCMR2 &= ~STM32_TIM_CCMR2_CC3S_MASK;
+            tim->CCMR2 |= STM32_TIM_CCMR2_CC3S(input_source);
+            tim->CCER |= STM32_TIM_CCER_CC3E;
+            break;
+        case 3:
+            tim->CCER &= ~STM32_TIM_CCER_CC4E;
+            tim->CCMR2 &= ~STM32_TIM_CCMR2_CC4S_MASK;
+            tim->CCMR2 |= STM32_TIM_CCMR2_CC4S(input_source);
+            tim->CCER |= STM32_TIM_CCER_CC4E;
+            break;
+    }
+}
+
 #if CH_DBG_ENABLE_STACK_CHECK == TRUE
 void show_stack_usage(void)
 {

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h

@@ -21,6 +21,7 @@ extern "C" {
 #endif
 
 void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode);
+void stm32_timer_set_channel_input(stm32_tim_t *tim, uint8_t channel, uint8_t input_source);
 
 #if CH_DBG_ENABLE_STACK_CHECK == TRUE
 // print stack usage

libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py

@@ -701,7 +701,6 @@ 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: