diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 9ba8b0130e..7d2b9ddb39 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -829,6 +829,19 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n) if (tx_len == 0) { return; } + + if (_flow_control != FLOW_CONTROL_DISABLE && + sdef.cts_line != 0 && + palReadLine(sdef.cts_line)) { + // we are using hw flow control and the CTS line is high. We + // will hold off trying to transmit until the CTS line goes + // low to indicate the receiver has space. We do this before + // we take the DMA lock to prevent a high CTS line holding a + // DMA channel that may be needed by another device + tx_len = 0; + return; + } + if (!dma_handle->lock_nonblock()) { tx_len = 0; return; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 50c3159126..6c05725c95 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -81,6 +81,7 @@ public: ioline_t tx_line; ioline_t rx_line; ioline_t rts_line; + ioline_t cts_line; int8_t rxinv_gpio; uint8_t rxinv_polarity; int8_t txinv_gpio; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index c09c7e0478..2c196bf77b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1214,6 +1214,7 @@ def write_UART_config(f): tx_line = make_line(dev + '_TX') rx_line = make_line(dev + '_RX') rts_line = make_line(dev + '_RTS') + cts_line = make_line(dev + '_CTS') if rts_line != "0": have_rts_cts = True f.write('#define HAL_HAVE_RTSCTS_SERIAL%u\n' % uart_serial_num[dev]) @@ -1234,10 +1235,10 @@ def write_UART_config(f): "#define HAL_%s_CONFIG { (BaseSequentialStream*) &SD%u, %u, false, " % (dev, n, n)) if mcu_series.startswith("STM32F1"): - f.write("%s, %s, %s, " % (tx_line, rx_line, rts_line)) + f.write("%s, %s, %s, %s, " % (tx_line, rx_line, rts_line, cts_line)) else: - f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, %s, %s, " % - (dev, dev, tx_line, rx_line, rts_line)) + f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, %s, %s, %s, " % + (dev, dev, tx_line, rx_line, rts_line, cts_line)) # add inversion pins, if any f.write("%d, " % get_gpio_bylabel(dev + "_RXINV"))