From 1b031c488593c407483f9db685270b2aa7fd8b3d Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 18 May 2024 21:10:16 -0500 Subject: [PATCH] UARTDriver: fix panic when using simulated latency This would sometimes happen when initiating a connection when using high simulated latency values when the queue would suddenly flush to the buffer. --- libraries/AP_HAL_SITL/UARTDriver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 4aa316804b2682..b9058f7d101136 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -268,7 +268,9 @@ void UARTDriver::_flush(void) size_t UARTDriver::_write(const uint8_t *buffer, size_t size) { const auto _txspace = txspace(); - if (_txspace < size) { + if (_txspace < size && latencyQueueWrite.empty()) { + // If we're using a latecy queue, we don't actually care about txspace + // at this point, as we're not going to write to the device yet. size = _txspace; } if (size <= 0) {