diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index cba6b87ee9..ddb6d889c6 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -56,7 +56,13 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) } // otherwise scan all protocols - for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { + rcprotocol_t last_protocol = AP_RCProtocol::NONE; +#ifdef HAL_RCIN_PULSE_PPM_ONLY + // only uses pulses for PPM on this board, using process_byte() for + // other protocols + last_protocol = (rcprotocol_t)1; +#endif + for (uint8_t i = 0; i < last_protocol; i++) { if (backend[i] != nullptr) { backend[i]->process_pulse(width_s0, width_s1); if (backend[i]->new_input()) { @@ -69,7 +75,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1) } } -void AP_RCProtocol::process_byte(uint8_t byte) +void AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate) { if (_detected_protocol != AP_RCProtocol::NONE && !_detected_with_bytes) { // we're using pulse inputs, discard bytes @@ -78,7 +84,7 @@ void AP_RCProtocol::process_byte(uint8_t byte) uint32_t now = AP_HAL::millis(); // first try current protocol if (_detected_protocol != AP_RCProtocol::NONE && now - _last_input_ms < 200) { - backend[_detected_protocol]->process_byte(byte); + backend[_detected_protocol]->process_byte(byte, baudrate); if (backend[_detected_protocol]->new_input()) { _new_input = true; _last_input_ms = AP_HAL::millis(); @@ -89,7 +95,7 @@ void AP_RCProtocol::process_byte(uint8_t byte) // otherwise scan all protocols for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) { if (backend[i] != nullptr) { - backend[i]->process_byte(byte); + backend[i]->process_byte(byte, baudrate); if (backend[i]->new_input()) { _new_input = true; _detected_protocol = (enum AP_RCProtocol::rcprotocol_t)i; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 89fbaa6a71..11acedebd7 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -44,7 +44,7 @@ public: return _valid_serial_prot; } void process_pulse(uint32_t width_s0, uint32_t width_s1); - void process_byte(uint8_t byte); + void process_byte(uint8_t byte, uint32_t baudrate); enum rcprotocol_t protocol_detected() { return _detected_protocol; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 17f5570689..ae0ef6889a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -25,7 +25,7 @@ class AP_RCProtocol_Backend { public: AP_RCProtocol_Backend(AP_RCProtocol &_frontend); virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {} - virtual void process_byte(uint8_t byte) {} + virtual void process_byte(uint8_t byte, uint32_t baudrate) {} uint16_t read(uint8_t chan); bool new_input(); uint8_t num_channels(); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 9d7555bebf..9ecd37fb7d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -442,8 +442,11 @@ void AP_RCProtocol_DSM::update(void) } // support byte input -void AP_RCProtocol_DSM::process_byte(uint8_t b) +void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) { + if (baudrate != 115200) { + return; + } uint32_t now = AP_HAL::millis(); if (now - byte_input.last_byte_ms > 3 || byte_input.ofs == sizeof(byte_input.buf)) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index faad1dadea..b78902b16a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -23,7 +23,7 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { public: AP_RCProtocol_DSM(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; - void process_byte(uint8_t byte) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; void start_bind(void) override; void update(void) override; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp index 11f20e7f26..bb4726129e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.cpp @@ -280,3 +280,30 @@ void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1) reset: memset(&sbus_state, 0, sizeof(sbus_state)); } + +// support byte input +void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate) +{ + if (baudrate != 100000) { + return; + } + uint32_t now = AP_HAL::millis(); + if (now - byte_input.last_byte_ms > 2 || + byte_input.ofs == sizeof(byte_input.buf)) { + byte_input.ofs = 0; + } + byte_input.last_byte_ms = now; + byte_input.buf[byte_input.ofs++] = b; + if (byte_input.ofs == sizeof(byte_input.buf)) { + uint16_t values[SBUS_INPUT_CHANNELS]; + uint16_t num_values=0; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + if (sbus_decode(byte_input.buf, values, &num_values, + &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) && + num_values >= MIN_RCIN_CHANNELS) { + add_input(num_values, values, false); + } + byte_input.ofs = 0; + } +} diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h index 0a78eac8fd..7f23203114 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SBUS.h @@ -23,6 +23,7 @@ class AP_RCProtocol_SBUS : public AP_RCProtocol_Backend { public: AP_RCProtocol_SBUS(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; private: bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values); @@ -30,4 +31,10 @@ private: uint16_t bytes[25]; // including start bit, parity and stop bits uint16_t bit_ofs; } sbus_state; -}; \ No newline at end of file + + struct { + uint8_t buf[25]; + uint8_t ofs; + uint32_t last_byte_ms; + } byte_input; +}; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp index 17c693eff6..febdefe3b9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.cpp @@ -90,7 +90,7 @@ void AP_RCProtocol_SRXL::process_pulse(uint32_t width_s0, uint32_t width_s1) break; } byte = ((v>>1) & 0xFF); - process_byte(byte); + process_byte(byte, 115200); } memset(&srxl_state, 0, sizeof(srxl_state)); } @@ -249,7 +249,7 @@ int AP_RCProtocol_SRXL::srxl_channels_get_v5(uint16_t max_values, uint8_t *num_v return 0; } -void AP_RCProtocol_SRXL::process_byte(uint8_t byte) +void AP_RCProtocol_SRXL::process_byte(uint8_t byte, uint32_t baudrate) { uint64_t timestamp_us = AP_HAL::micros64(); /*----------------------------------------distinguish different srxl variants at the beginning of each frame---------------------------------------------- */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h index 08ed832945..00a1848262 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL.h @@ -39,7 +39,7 @@ class AP_RCProtocol_SRXL : public AP_RCProtocol_Backend { public: AP_RCProtocol_SRXL(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; - void process_byte(uint8_t byte) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; private: static uint16_t srxl_crc16(uint16_t crc, uint8_t new_byte); int srxl_channels_get_v1v2(uint16_t max_values, uint8_t *num_values, uint16_t *values, bool *failsafe_state); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp index bb11cce6dc..7abed65105 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.cpp @@ -118,7 +118,7 @@ void AP_RCProtocol_ST24::process_pulse(uint32_t width_s0, uint32_t width_s1) break; } byte = ((v>>1) & 0xFF); - process_byte(byte); + process_byte(byte, 115200); } memset(&st24_state, 0, sizeof(st24_state)); } @@ -138,9 +138,11 @@ reset: memset(&st24_state, 0, sizeof(st24_state)); } -void AP_RCProtocol_ST24::process_byte(uint8_t byte) +void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate) { - + if (baudrate != 115200) { + return; + } switch (_decode_state) { case ST24_DECODE_STATE_UNSYNCED: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h index 6676125762..96423908f1 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_ST24.h @@ -39,7 +39,7 @@ class AP_RCProtocol_ST24 : public AP_RCProtocol_Backend { public: AP_RCProtocol_ST24(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; - void process_byte(uint8_t byte) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; private: static uint8_t st24_crc8(uint8_t *ptr, uint8_t len); enum ST24_PACKET_TYPE { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp index fef17fa24d..08d4c03b56 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.cpp @@ -125,7 +125,7 @@ void AP_RCProtocol_SUMD::process_pulse(uint32_t width_s0, uint32_t width_s1) break; } byte = ((v>>1) & 0xFF); - process_byte(byte); + process_byte(byte, 115200); } memset(&sumd_state, 0, sizeof(sumd_state)); } @@ -145,8 +145,11 @@ reset: memset(&sumd_state, 0, sizeof(sumd_state)); } -void AP_RCProtocol_SUMD::process_byte(uint8_t byte) +void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate) { + if (baudrate != 115200) { + return; + } switch (_decode_state) { case SUMD_DECODE_STATE_UNSYNCED: #ifdef SUMD_DEBUG diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h index 74905a29b0..3eff2e5cdb 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SUMD.h @@ -24,7 +24,7 @@ class AP_RCProtocol_SUMD : public AP_RCProtocol_Backend { public: AP_RCProtocol_SUMD(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) {} void process_pulse(uint32_t width_s0, uint32_t width_s1) override; - void process_byte(uint8_t byte) override; + void process_byte(uint8_t byte, uint32_t baudrate) override; private: static uint16_t sumd_crc16(uint16_t crc, uint8_t value); static uint8_t sumd_crc8(uint8_t crc, uint8_t value);