diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 210cb3b8fe..035c5a1cb9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -26,6 +26,8 @@ #include "AP_RCProtocol_FPort.h" #include +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) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index c150b85bd8..9db14c9dc2 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -62,6 +62,7 @@ public: 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; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index f4b6fb393f..86a8a7315a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/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 */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 41154bdf14..f32137755e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -28,6 +28,7 @@ public: 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();