From 0b95b515bdfb81dabb47a4f5ff6ad19cc19bb3a9 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 15 Mar 2024 14:25:18 +0000
Subject: [PATCH] AP_HAL_SITL: implement uart_info to populate @SYS/uarts.txt

---
 libraries/AP_HAL_SITL/UARTDriver.cpp | 27 ++++++++++++++++++++++++++-
 libraries/AP_HAL_SITL/UARTDriver.h   |  9 +++++++++
 libraries/AP_HAL_SITL/Util.cpp       | 25 +++++++++++++++++++++++++
 libraries/AP_HAL_SITL/Util.h         | 14 ++++++++++++++
 4 files changed, 74 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index 9b7516af69..3c58ea02b0 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -46,6 +46,7 @@
 
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_Filesystem/AP_Filesystem.h>
+#include <AP_Common/ExpandingString.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -227,7 +228,9 @@ uint32_t UARTDriver::txspace(void)
 
 ssize_t UARTDriver::_read(uint8_t *buffer, uint16_t count)
 {
-    return _readbuffer.read(buffer, count);
+    const ssize_t ret = _readbuffer.read(buffer, count);
+    _rx_stats_bytes += ret;
+    return ret;
 }
 
 bool UARTDriver::_discard_input(void)
@@ -284,6 +287,8 @@ size_t UARTDriver::_write(const uint8_t *buffer, size_t size)
         }
 #endif // HAL_BUILD_AP_PERIPH
 
+    // Include lost byte in tx count, we think we sent it even though it was never added to the write buffer
+    _tx_stats_bytes += lost_byte;
 
     const size_t ret = _writebuffer.write(buffer, size - lost_byte) + lost_byte;
     if (_unbuffered_writes) {
@@ -834,6 +839,7 @@ void UARTDriver::handle_writing_from_writebuffer_to_device()
             ssize_t ret = send(_fd, tmpbuf, n, MSG_DONTWAIT);
             if (ret > 0) {
                 _writebuffer.advance(ret);
+                _tx_stats_bytes += ret;
             }
         }
     } else {
@@ -855,6 +861,7 @@ void UARTDriver::handle_writing_from_writebuffer_to_device()
             }
             if (nwritten > 0) {
                 _writebuffer.advance(nwritten);
+                _tx_stats_bytes += nwritten;
             }
         }
     }
@@ -1012,5 +1019,23 @@ uint32_t UARTDriver::bw_in_bytes_per_second() const
     const uint32_t bitrate = _connected ? 10E6 : _uart_baudrate;
     return bitrate/10; // convert bits to bytes minus overhead
 };
+
+#if HAL_UART_STATS_ENABLED
+// request information on uart I/O for @SYS/uarts.txt for this uart
+void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint32_t dt_ms)
+{
+    const uint32_t tx_bytes = stats.tx.update(_tx_stats_bytes);
+    const uint32_t rx_bytes = stats.rx.update(_rx_stats_bytes);
+
+    str.printf("TX=%8u RX=%8u TXBD=%6u RXBD=%6u %s (%s)\n",
+                unsigned(tx_bytes),
+                unsigned(rx_bytes),
+                unsigned((tx_bytes * 10000) / dt_ms),
+                unsigned((rx_bytes * 10000) / dt_ms),
+                _connected ? "connected    " : "not connected",
+                _sitlState->_serial_path[_portNumber]);
+}
+#endif
+
 #endif // CONFIG_HAL_BOARD
 
diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h
index 7d08301fc1..3352826385 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_SITL/UARTDriver.h
@@ -67,6 +67,11 @@ public:
 
     uint32_t get_baud_rate() const override { return _uart_baudrate; }
 
+#if HAL_UART_STATS_ENABLED
+    // request information on uart I/O
+    void uart_info(ExpandingString &str, StatsTracker &stats, const uint32_t dt_ms) override;
+#endif
+
 private:
 
     int _fd;
@@ -143,6 +148,10 @@ protected:
 private:
     void handle_writing_from_writebuffer_to_device();
     void handle_reading_from_device_to_readbuffer();
+
+    // statistics
+    uint32_t _tx_stats_bytes;
+    uint32_t _rx_stats_bytes;
 };
 
 #endif
diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp
index 958f280956..727823b6b8 100644
--- a/libraries/AP_HAL_SITL/Util.cpp
+++ b/libraries/AP_HAL_SITL/Util.cpp
@@ -5,6 +5,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
+#include <AP_Common/ExpandingString.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -191,3 +192,27 @@ bool HALSITL::Util::get_random_vals(uint8_t* data, size_t size)
     close(dev_random);
     return true;
 }
+
+#if HAL_UART_STATS_ENABLED
+// request information on uart I/O
+void HALSITL::Util::uart_info(ExpandingString &str)
+{
+    // Calculate time since last call
+    const uint32_t now_ms = AP_HAL::millis();
+    const uint32_t dt_ms = now_ms - uart_stats.last_ms;
+    uart_stats.last_ms = now_ms;
+
+    // a header to allow for machine parsers to determine format
+    str.printf("UARTV1\n");
+    for (uint8_t i = 0; i < hal.num_serial; i++) {
+        if (i >= ARRAY_SIZE(sitlState->_serial_path)) {
+            continue;
+        }
+        auto *uart = hal.serial(i);
+        if (uart) {
+            str.printf("SERIAL%u ", i);
+            uart->uart_info(str, uart_stats.serial[i], dt_ms);
+        }
+    }
+}
+#endif
diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h
index 192e4406d3..71864bdfcb 100644
--- a/libraries/AP_HAL_SITL/Util.h
+++ b/libraries/AP_HAL_SITL/Util.h
@@ -106,4 +106,18 @@ private:
 
     int saved_argc;
     char *const *saved_argv;
+
+#if HAL_UART_STATS_ENABLED
+    // request information on uart I/O
+    void uart_info(ExpandingString &str) override;
+#endif
+
+private:
+#if HAL_UART_STATS_ENABLED
+    // UART stats tracking helper
+    struct {
+        AP_HAL::UARTDriver::StatsTracker serial[AP_HAL::HAL::num_serial];
+        uint32_t last_ms;
+    } uart_stats;
+#endif
 };