From 3716f5513d307e4731972819d095e8315dd8bf2d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 13:39:20 +1100 Subject: [PATCH] AP_HAL_SITL: process inbound data in outqueue-length delay loop this is the loop which ensures the amount of data sent to the mavlink client (usually Python) is limited - if we don't do this then we lose vast amounts of data when running at large speedups. By attempting to process inbound data we may realise that the TCP connection has been dropped, and in that case we will start to listen for another connection. This allows you to terminate the sim_vehicle.py MAVProxy and have it automagically restart (when running under GDB). This is very useful for testing MAVProxy patches with SITL; it's a different workflow to opening an output and connecting a new version of MAVProxy to that outout. --- libraries/AP_HAL_SITL/SITL_State.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index af55acaa1f..4850e40b02 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -185,12 +185,14 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) // conditions. if (speedup > 1 && hal.scheduler->in_main_thread()) { while (true) { - const int queue_length = ((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length(); + HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0); + const int queue_length = uart->get_system_outqueue_length(); // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length); if (queue_length < 1024) { break; } _serial_0_outqueue_full_count++; + uart->handle_reading_from_device_to_readbuffer(); usleep(1000); } }