From 78e432af0c311af00956e6665e6ccc166f078ca6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Apr 2020 11:50:25 +1100 Subject: [PATCH] AP_HAL_SITL: do not let outbound queue length to grow too far This basically limits our loop rate to whatever is listening on uartA can handle in terms of mavlink traffic. --- libraries/AP_HAL_SITL/SITL_State.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 85448183c9..183642473b 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -212,6 +212,20 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) usleep(1000); } } + // check the outbound TCP queue size. If it is too long then + // MAVProxy/pymavlink take too long to process packets and it ends + // up seeing traffic well into our past and hits time-out + // conditions. + if (sitl_model->get_speedup() > 1) { + while (true) { + const int queue_length = ((HALSITL::UARTDriver*)hal.uartA)->get_system_outqueue_length(); + // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length); + if (queue_length < 1024) { + break; + } + usleep(1000); + } + } } #define streq(a, b) (!strcmp(a, b))