diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 97d5d772253..d4d7827a2b7 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -36,6 +36,8 @@ #include "crsf_telemetry.h" #include +#include + using namespace time_literals; constexpr char const *RCInput::RC_SCAN_STRING[]; @@ -397,15 +399,13 @@ void RCInput::Run() unsigned frame_drops = 0; - int newBytes = 0; - // TODO: needs work (poll _rcs_fd) // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100); // then update priority to SCHED_PRIORITY_FAST_DRIVER // read all available data from the serial RC input UART // read all available data from the serial RC input UART - newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); if (newBytes > 0) { _bytes_rx += newBytes; @@ -419,6 +419,10 @@ void RCInput::Run() sbus_config(_rcs_fd, board_rc_singlewire(_device)); rc_io_invert(true); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -453,6 +457,10 @@ void RCInput::Run() // Configure serial port for DSM dsm_config(_rcs_fd); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -486,6 +494,10 @@ void RCInput::Run() // Configure serial port for DSM dsm_config(_rcs_fd); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -533,6 +545,10 @@ void RCInput::Run() // Configure serial port for DSM dsm_config(_rcs_fd); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -607,6 +623,10 @@ void RCInput::Run() // Configure serial port for CRSF crsf_config(_rcs_fd); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -651,6 +671,10 @@ void RCInput::Run() // Configure serial port for GHST ghst_config(_rcs_fd); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 652bd72e00f..e74ea08b9d2 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -153,7 +153,8 @@ private: int _rcs_fd{-1}; char _device[20] {}; ///< device / serial port path - uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; + static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; uint16_t _raw_rc_count{};