From 6ae0b5ec5b823058e188ff5d70c19990251739e2 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 8 Jun 2024 17:42:26 +1000
Subject: [PATCH] AP_HAL_SITL: log number of times sim paused on serial0 buffer

SITL pauses the simulation if we do not have a minimum amount of space in its out queue.

Log the number of times we do this.
---
 libraries/AP_HAL_SITL/HAL_SITL_Class.cpp  | 5 +++++
 libraries/AP_HAL_SITL/HAL_SITL_Class.h    | 2 ++
 libraries/AP_HAL_SITL/SITL_State.cpp      | 1 +
 libraries/AP_HAL_SITL/SITL_State_common.h | 4 ++++
 4 files changed, 12 insertions(+)

diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index ab4a67dd83..be72972e26 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -206,6 +206,11 @@ bool HAL_SITL::run_in_maintenance_mode() const
 }
 #endif
 
+uint32_t HAL_SITL::get_uart_output_full_queue_count() const
+{
+    return _sitl_state->_serial_0_outqueue_full_count;
+}
+
 void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const
 {
     assert(callbacks);
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h
index 4ddc20b2cb..495405c62b 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h
@@ -41,6 +41,8 @@ public:
     bool run_in_maintenance_mode() const;
 #endif
 
+    uint32_t get_uart_output_full_queue_count() const;
+
 private:
     HALSITL::SITL_State *_sitl_state;
 
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 627b7c6c80..2d49bb606b 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -188,6 +188,7 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
             if (queue_length < 1024) {
                 break;
             }
+            _serial_0_outqueue_full_count++;
             usleep(1000);
         }
     }
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h
index de3d75d657..7bb4c04348 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.h
+++ b/libraries/AP_HAL_SITL/SITL_State_common.h
@@ -247,6 +247,10 @@ public:
     void multicast_state_open(void);
     void multicast_state_send(void);
 
+    // number of times we have paused the simulation for 1ms because
+    // the TCP queue is full:
+    uint32_t _serial_0_outqueue_full_count;
+
 protected:
     enum vehicle_type _vehicle;