From 73f4a474962bc45a350dc1b81c74187af691fc9d Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sat, 6 Apr 2024 17:11:57 +0100
Subject: [PATCH] AP_HAL_ChibiOS: and `uart_log` method and stats struct

---
 libraries/AP_HAL_ChibiOS/Util.cpp | 31 +++++++++++++++++++++++++++----
 libraries/AP_HAL_ChibiOS/Util.h   | 15 +++++++++++++--
 2 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 8deec9fda1..03a8b3e745 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -684,8 +684,8 @@ void 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;
+    const uint32_t dt_ms = now_ms - sys_uart_stats.last_ms;
+    sys_uart_stats.last_ms = now_ms;
 
     // a header to allow for machine parsers to determine format
     str.printf("UARTV1\n");
@@ -693,15 +693,38 @@ void Util::uart_info(ExpandingString &str)
         auto *uart = hal.serial(i);
         if (uart) {
             str.printf("SERIAL%u ", i);
-            uart->uart_info(str, uart_stats.serial[i], dt_ms);
+            uart->uart_info(str, sys_uart_stats.serial[i], dt_ms);
         }
     }
 #if HAL_WITH_IO_MCU
     str.printf("IOMCU   ");
-    uart_io.uart_info(str, uart_stats.io, dt_ms);
+    uart_io.uart_info(str, sys_uart_stats.io, dt_ms);
 #endif
 }
+
+// Log UART message for each serial port
+#if HAL_LOGGING_ENABLED
+void Util::uart_log()
+{
+    // Calculate time since last call
+    const uint32_t now_ms = AP_HAL::millis();
+    const uint32_t dt_ms = now_ms - log_uart_stats.last_ms;
+    log_uart_stats.last_ms = now_ms;
+
+    // Loop over all numbered ports
+    for (uint8_t i = 0; i < HAL_UART_NUM_SERIAL_PORTS; i++) {
+        auto *uart = hal.serial(i);
+        if (uart) {
+            uart->log_stats(i, log_uart_stats.serial[i], dt_ms);
+        }
+    }
+#if HAL_WITH_IO_MCU
+    // Use magic instance 100 for IOMCU
+    uart_io.log_stats(100, log_uart_stats.io, dt_ms);
 #endif
+}
+#endif // HAL_LOGGING_ENABLED
+#endif // HAL_UART_STATS_ENABLED
 
 // request information on uart I/O
 #if HAL_USE_PWM == TRUE
diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h
index 7b927ca4fd..8e431484a0 100644
--- a/libraries/AP_HAL_ChibiOS/Util.h
+++ b/libraries/AP_HAL_ChibiOS/Util.h
@@ -20,6 +20,7 @@
 #include "AP_HAL_ChibiOS_Namespace.h"
 #include "AP_HAL_ChibiOS.h"
 #include <ch.h>
+#include <AP_Logger/AP_Logger_config.h>
 
 class ExpandingString;
 
@@ -99,7 +100,13 @@ public:
 #if HAL_UART_STATS_ENABLED
     // request information on uart I/O
     void uart_info(ExpandingString &str) override;
+
+#if HAL_LOGGING_ENABLED
+    // Log UART message for each serial port
+    void uart_log() override;
 #endif
+#endif // HAL_UART_STATS_ENABLED
+
 #if HAL_USE_PWM == TRUE
     void timer_info(ExpandingString &str) override;
 #endif
@@ -163,12 +170,16 @@ private:
 #endif
 
 #if HAL_UART_STATS_ENABLED
-    struct {
+    struct uart_stats {
         AP_HAL::UARTDriver::StatsTracker serial[HAL_UART_NUM_SERIAL_PORTS];
 #if HAL_WITH_IO_MCU
         AP_HAL::UARTDriver::StatsTracker io;
 #endif
         uint32_t last_ms;
-    } uart_stats;
+    };
+    uart_stats sys_uart_stats;
+#if HAL_LOGGING_ENABLED
+    uart_stats log_uart_stats;
+#endif
 #endif
 };