diff --git a/libraries/AP_HAL_ESP32/RCInput.cpp b/libraries/AP_HAL_ESP32/RCInput.cpp index 2588f103f7..8e8a3dea6b 100644 --- a/libraries/AP_HAL_ESP32/RCInput.cpp +++ b/libraries/AP_HAL_ESP32/RCInput.cpp @@ -12,14 +12,24 @@ * You should have received a copy of the GNU General Public License along * with this program. If not, see . */ -#include + #include #include "AP_HAL_ESP32.h" #include "RCInput.h" +#include + +#include + +#ifndef HAL_NO_UARTDRIVER +#include +#endif + using namespace ESP32; +extern const AP_HAL::HAL& hal; + void RCInput::init() { if (_init) { @@ -31,10 +41,26 @@ void RCInput::init() #ifdef HAL_ESP32_RCIN sig_reader.init(); + 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_ESP32_RCIN + if (!enable) { + sig_reader.disable(); + } +#endif +} + bool RCInput::new_input() { if (!_init) { @@ -89,23 +115,29 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len) void RCInput::_timer_tick(void) { -#if AP_RCPROTOCOL_ENABLED if (!_init) { return; } +#ifndef HAL_NO_UARTDRIVER + const char *rc_protocol = nullptr; + RCSource source = last_source; +#endif + +#if AP_RCPROTOCOL_ENABLED AP_RCProtocol &rcprot = AP::RC(); #ifdef HAL_ESP32_RCIN - uint32_t width_s0, width_s1; - while (sig_reader.read(width_s0, width_s1)) { - rcprot.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); + } } - -#ifndef HAL_NO_UARTDRIVER - const char *rc_protocol = nullptr; #endif +#endif // AP_RCPROTOCOL_ENABLED + +#if AP_RCPROTOCOL_ENABLED if (rcprot.new_input()) { WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = AP_HAL::micros(); @@ -113,18 +145,36 @@ void RCInput::_timer_tick(void) _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS); rcprot.read(_rc_values, _num_channels); _rssi = rcprot.get_RSSI(); + _rx_link_quality = rcprot.get_rx_link_quality(); #ifndef HAL_NO_UARTDRIVER rc_protocol = rcprot.protocol_name(); + source = rcprot.using_uart() ? RCSource::RCPROT_BYTES : RCSource::RCPROT_PULSES; + // printf("RCInput: decoding %s", last_protocol); #endif } +#endif // AP_RCPROTOCOL_ENABLED #ifndef HAL_NO_UARTDRIVER - if (rc_protocol && rc_protocol != last_protocol) { + if (rc_protocol && (rc_protocol != last_protocol || source != last_source)) { last_protocol = rc_protocol; - GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol); + last_source = source; + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s(%u)", last_protocol, unsigned(source)); } #endif -#endif -#endif // AP_RCPROTOCOL_ENABLED + // note, we rely on the vehicle code checking new_input() + // and a timeout for the last valid input to handle failsafe +} + +/* + start a bind operation, if supported + */ +bool RCInput::rc_bind(int dsmMode) +{ +#if AP_RCPROTOCOL_ENABLED + // ask AP_RCProtocol to start a bind + AP::RC().start_bind(); +#endif + + return true; } diff --git a/libraries/AP_HAL_ESP32/RCInput.h b/libraries/AP_HAL_ESP32/RCInput.h index 370753d07f..7e8e885b1b 100644 --- a/libraries/AP_HAL_ESP32/RCInput.h +++ b/libraries/AP_HAL_ESP32/RCInput.h @@ -17,9 +17,10 @@ #include "AP_HAL_ESP32.h" #include "Semaphores.h" -#include "RmtSigReader.h" + #include +#include "RmtSigReader.h" #ifndef RC_INPUT_MAX_CHANNELS #define RC_INPUT_MAX_CHANNELS 18 @@ -33,21 +34,42 @@ public: uint8_t num_channels() override; uint16_t read(uint8_t ch) override; uint8_t read(uint16_t* periods, uint8_t len) override; - void _timer_tick(void); - const char *protocol() const override - { - return last_protocol; + /* 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; } + int16_t get_rx_link_quality(void) override { + return _rx_link_quality; + } + const char *protocol() const override { return last_protocol; } + + void _timer_tick(void); + bool rc_bind(int dsmMode) override; + private: uint16_t _rc_values[RC_INPUT_MAX_CHANNELS] = {0}; + uint64_t _last_read; uint8_t _num_channels; Semaphore rcin_mutex; int16_t _rssi = -1; + int16_t _rx_link_quality = -1; uint32_t _rcin_timestamp_last_signal; bool _init; const char *last_protocol; + enum class RCSource { + NONE = 0, + IOMCU = 1, + RCPROT_PULSES = 2, + RCPROT_BYTES = 3, + } last_source; + + bool pulse_input_enabled; + ESP32::RmtSigReader sig_reader; };