diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index d0fedce547915..da4ae589df661 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -299,6 +299,7 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) _rx_initialised = false; _readbuf.set_size_best(rxS); } + _rts_threshold = _readbuf.get_size() / 16U; bool clear_buffers = false; if (b != 0) { @@ -1414,10 +1415,10 @@ __RAMFUNC__ void UARTDriver::update_rts_line(void) return; } uint16_t space = _readbuf.space(); - if (_rts_is_active && space < 16) { + if (_rts_is_active && space < _rts_threshold) { _rts_is_active = false; palSetLine(arts_line); - } else if (!_rts_is_active && space > 32) { + } else if (!_rts_is_active && space > _rts_threshold+16) { _rts_is_active = true; palClearLine(arts_line); } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index e50230d427ae6..f21c1d7aa8be4 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -182,6 +182,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { #endif ByteBuffer _readbuf{0}; ByteBuffer _writebuf{0}; + uint32_t _rts_threshold; HAL_Semaphore _write_mutex; #ifndef HAL_UART_NODMA const stm32_dma_stream_t* rxdma;