Fixed RC input timing jitter in IOMCU

libraries/AP_HAL/RCInput.h

@@ -49,4 +49,8 @@ class AP_HAL::RCInput {
 
     /* execute receiver bind */
     virtual bool rc_bind(int dsmMode) { return false; }
+
+    /* enable or disable pulse input for RC input. This is used to
+       reduce load when we are decoding R/C via a UART */
+    virtual void pulse_input_enable(bool enable) { }
 };

libraries/AP_HAL_ChibiOS/RCInput.cpp

@@ -43,27 +43,42 @@ void RCInput::init()
 #if HAL_USE_ICU == TRUE
     //attach timer channel on which the signal will be received
     sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL);
+    pulse_input_enabled = true;
 #endif
 
 #if HAL_USE_EICU == TRUE
     sig_reader.init(&RCININT_EICU_TIMER, RCININT_EICU_CHANNEL);
+    pulse_input_enabled = true;
 #endif
 
     _init = true;
 }
 
+/*
+  enable or disable pulse input for RC input. This is used to reduce
+  load when we are decoding R/C via a UART
+*/
+void RCInput::pulse_input_enable(bool enable)
+{
+    pulse_input_enabled = enable;
+#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE
+    if (!enable) {
+        sig_reader.disable();
+    }
+#endif
+}
+
 bool RCInput::new_input()
 {
     if (!_init) {
         return false;
     }
-    if (!rcin_mutex.take_nonblocking()) {
-        return false;
+    bool valid;
+    {
+        WITH_SEMAPHORE(rcin_mutex);
+        valid = _rcin_timestamp_last_signal != _last_read;
+        _last_read = _rcin_timestamp_last_signal;
     }
-    bool valid = _rcin_timestamp_last_signal != _last_read;
-
-    _last_read = _rcin_timestamp_last_signal;
-    rcin_mutex.give();
 
 #if HAL_RCINPUT_WITH_AP_RADIO
     if (!_radio_init) {
@@ -90,9 +105,11 @@ uint16_t RCInput::read(uint8_t channel)
     if (!_init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, _num_channels))) {
         return 0;
     }
-    rcin_mutex.take_blocking();
-    uint16_t v = _rc_values[channel];
-    rcin_mutex.give();
+    uint16_t v;
+    {
+        WITH_SEMAPHORE(rcin_mutex);
+        v = _rc_values[channel];
+    }
 #if HAL_RCINPUT_WITH_AP_RADIO
     if (radio && channel == 0) {
         // hook to allow for update of radio on main thread, for mavlink sends
@@ -111,9 +128,16 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len)
     if (len > RC_INPUT_MAX_CHANNELS) {
         len = RC_INPUT_MAX_CHANNELS;
     }
-    for (uint8_t i = 0; i < len; i++){
-        periods[i] = read(i);
+    {
+        WITH_SEMAPHORE(rcin_mutex);
+        memcpy(periods, _rc_values, len*sizeof(periods[0]));
     }
+#if HAL_RCINPUT_WITH_AP_RADIO
+    if (radio) {
+        // hook to allow for update of radio on main thread, for mavlink sends
+        radio->update();
+    }
+#endif
     return len;
 }
 
@@ -122,69 +146,71 @@ void RCInput::_timer_tick(void)
     if (!_init) {
         return;
     }
-
 #ifndef HAL_NO_UARTDRIVER
     const char *rc_protocol = nullptr;
 #endif
 
 #ifndef HAL_BUILD_AP_PERIPH
+    AP_RCProtocol &rcprot = AP::RC();
+
 #if HAL_USE_ICU == TRUE
-    const uint32_t *p;
-    uint32_t n;
-    while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) {
-        AP::RC().process_pulse_list(p, n*2, sig_reader.need_swap);
-        sig_reader.sigbuf.advance(n);
+    if (pulse_input_enabled) {
+        const uint32_t *p;
+        uint32_t n;
+        while ((p = (const uint32_t *)sig_reader.sigbuf.readptr(n)) != nullptr) {
+            rcprot.process_pulse_list(p, n*2, sig_reader.need_swap);
+            sig_reader.sigbuf.advance(n);
+        }
     }
 #endif
 
 #if HAL_USE_EICU == TRUE
-    uint32_t width_s0, width_s1;
-    while(sig_reader.read(width_s0, width_s1)) {
-        AP::RC().process_pulse(width_s0, width_s1);
+    if (pulse_input_enabled) {
+        uint32_t width_s0, width_s1;
+        while(sig_reader.read(width_s0, width_s1)) {
+            rcprot.process_pulse(width_s0, width_s1);
+        }
     }
 #endif
 
-    if (AP::RC().new_input()) {
-        rcin_mutex.take_blocking();
+    if (rcprot.new_input()) {
+        WITH_SEMAPHORE(rcin_mutex);
         _rcin_timestamp_last_signal = AP_HAL::micros();
-        _num_channels = AP::RC().num_channels();
+        _num_channels = rcprot.num_channels();
         _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
-        for (uint8_t i=0; i<_num_channels; i++) {
-            _rc_values[i] = AP::RC().read(i);
-        }
-        _rssi = AP::RC().get_RSSI();
-        rcin_mutex.give();
+        rcprot.read(_rc_values, _num_channels);
+        _rssi = rcprot.get_RSSI();
 #ifndef HAL_NO_UARTDRIVER
-        rc_protocol = AP::RC().protocol_name();
+        rc_protocol = rcprot.protocol_name();
 #endif
     }
 #endif // HAL_BUILD_AP_PERIPH
 
 #if HAL_RCINPUT_WITH_AP_RADIO
     if (radio && radio->last_recv_us() != last_radio_us) {
         last_radio_us = radio->last_recv_us();
-        rcin_mutex.take_blocking();
+        WITH_SEMAPHORE(rcin_mutex);
         _rcin_timestamp_last_signal = last_radio_us;
         _num_channels = radio->num_channels();
         _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
         for (uint8_t i=0; i<_num_channels; i++) {
             _rc_values[i] = radio->read(i);
         }
-        rcin_mutex.give();
     }
 #endif
 
 #if HAL_WITH_IO_MCU
-    rcin_mutex.take_blocking();
-    if (AP_BoardConfig::io_enabled() &&
-        iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
-        _rcin_timestamp_last_signal = last_iomcu_us;
+    {
+        WITH_SEMAPHORE(rcin_mutex);
+        if (AP_BoardConfig::io_enabled() &&
+            iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
+            _rcin_timestamp_last_signal = last_iomcu_us;
 #ifndef HAL_NO_UARTDRIVER
-        rc_protocol = iomcu.get_rc_protocol();
-        _rssi = iomcu.get_RSSI();
+            rc_protocol = iomcu.get_rc_protocol();
+            _rssi = iomcu.get_RSSI();
 #endif
+        }
     }
-    rcin_mutex.give();
 #endif
 
 #ifndef HAL_NO_UARTDRIVER
@@ -204,11 +230,12 @@ void RCInput::_timer_tick(void)
 bool RCInput::rc_bind(int dsmMode)
 {
 #if HAL_WITH_IO_MCU
-    rcin_mutex.take_blocking();
-    if (AP_BoardConfig::io_enabled()) {
-        iomcu.bind_dsm(dsmMode);
+    {
+        WITH_SEMAPHORE(rcin_mutex);
+        if (AP_BoardConfig::io_enabled()) {
+            iomcu.bind_dsm(dsmMode);
+        }
     }
-    rcin_mutex.give();
 #endif
 
 #ifndef HAL_BUILD_AP_PERIPH

libraries/AP_HAL_ChibiOS/RCInput.h

@@ -45,6 +45,10 @@ class ChibiOS::RCInput : public AP_HAL::RCInput {
     uint16_t read(uint8_t ch) override;
     uint8_t read(uint16_t* periods, uint8_t len) override;
 
+    /* enable or disable pulse input for RC input. This is used to
+       reduce load when we are decoding R/C via a UART */
+    void pulse_input_enable(bool enable) override;
+
     int16_t get_rssi(void) override {
         return _rssi;
     }
@@ -64,6 +68,7 @@ class ChibiOS::RCInput : public AP_HAL::RCInput {
     uint32_t _rcin_timestamp_last_signal;
     bool _init;
     const char *last_protocol;
+    bool pulse_input_enabled;
 
 #if HAL_RCINPUT_WITH_AP_RADIO
     bool _radio_init;

libraries/AP_HAL_ChibiOS/SoftSigReader.cpp

@@ -91,6 +91,12 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
     return true;
 }
 
+void SoftSigReader::disable(void)
+{
+    icuStopCapture(_icu_drv);
+    dmaStreamDisable(dma);
+}
+
 void SoftSigReader::_irq_handler(void* self, uint32_t flags)
 {
     SoftSigReader* sig_reader = (SoftSigReader*)self;

libraries/AP_HAL_ChibiOS/SoftSigReader.h

@@ -36,6 +36,7 @@ class ChibiOS::SoftSigReader {
     friend class ChibiOS::RCInput;
 public:
     bool attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan, uint8_t dma_stream, uint32_t dma_channel);
+    void disable(void);
 
 private:
     uint32_t *signal;

libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp

@@ -90,6 +90,11 @@ void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
     eicuEnable(_icu_drv);
 }
 
+void SoftSigReaderInt::disable(void)
+{
+    eicuDisable(_icu_drv);
+}
+
 void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel)
 {
     eicuchannel_t channel = get_pair_channel(aux_channel);

libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h

@@ -42,6 +42,7 @@ class ChibiOS::SoftSigReaderInt {
 
     void init(EICUDriver* icu_drv, eicuchannel_t chan);
     bool read(uint32_t &widths0, uint32_t &widths1);
+    void disable(void);
 private:
     // singleton
     static SoftSigReaderInt *_singleton;

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -301,9 +301,7 @@ void AP_IOMCU_FW::rcin_update()
     if (hal.rcin->new_input()) {
         rc_input.count = hal.rcin->num_channels();
         rc_input.flags_rc_ok = true;
-        for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS; i++) {
-            rc_input.pwm[i] = hal.rcin->read(i);
-        }
+        hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS);
         rc_last_input_ms = last_ms;
         rc_input.rc_protocol = (uint16_t)AP::RC().protocol_detected();
         rc_input.rssi = AP::RC().get_RSSI();
@@ -316,6 +314,7 @@ void AP_IOMCU_FW::rcin_update()
     }
     if (update_default_rate) {
         hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
+        update_default_rate = false;
     }
 
     bool old_override = override_active;

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -26,6 +26,8 @@
 #include "AP_RCProtocol_FPort.h"
 #include <AP_Math/AP_Math.h>
 
+extern const AP_HAL::HAL& hal;
+
 void AP_RCProtocol::init()
 {
     backend[AP_RCProtocol::PPM] = new AP_RCProtocol_PPMSum(*this);
@@ -150,6 +152,9 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
                 memset(_good_frames, 0, sizeof(_good_frames));
                 _last_input_ms = now;
                 _detected_with_bytes = true;
+
+                // stop decoding pulses to save CPU
+                hal.rcin->pulse_input_enable(false);
                 break;
             }
         }
@@ -257,6 +262,13 @@ uint16_t AP_RCProtocol::read(uint8_t chan)
     return 0;
 }
 
+void AP_RCProtocol::read(uint16_t *pwm, uint8_t n)
+{
+    if (_detected_protocol != AP_RCProtocol::NONE) {
+        backend[_detected_protocol]->read(pwm, n);
+    }
+}
+
 int16_t AP_RCProtocol::get_RSSI(void) const
 {
     if (_detected_protocol != AP_RCProtocol::NONE) {

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -62,6 +62,7 @@ class AP_RCProtocol {
 
     uint8_t num_channels();
     uint16_t read(uint8_t chan);
+    void read(uint16_t *pwm, uint8_t n);
     bool new_input();
     void start_bind(void);
     int16_t get_RSSI(void) const;

libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp

@@ -46,6 +46,14 @@ uint16_t AP_RCProtocol_Backend::read(uint8_t chan)
     return _pwm_values[chan];
 }
 
+void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
+{
+    if (n >= MAX_RCIN_CHANNELS) {
+        n = MAX_RCIN_CHANNELS;
+    }
+    memcpy(pwm, _pwm_values, n*sizeof(pwm[0]));
+}
+
 /*
   provide input from a backend
  */

libraries/AP_RCProtocol/AP_RCProtocol_Backend.h

@@ -28,6 +28,7 @@ class AP_RCProtocol_Backend {
     virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {}
     virtual void process_byte(uint8_t byte, uint32_t baudrate) {}
     uint16_t read(uint8_t chan);
+    void read(uint16_t *pwm, uint8_t n);
     bool new_input();
     uint8_t num_channels();