From 7790b1ec713edceaa6d18c7a72fc5f79109495ed Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 10 Feb 2024 15:24:54 +1100
Subject: [PATCH] AP_Periph: map MAV_SEVERITY to DroneCAN debug levels

---
 Tools/AP_Periph/AP_Periph.h |  4 ++-
 Tools/AP_Periph/can.cpp     | 49 +++++++++++++++++++++++++++++++------
 2 files changed, 45 insertions(+), 8 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 424e341c1c8c1..95d32af7936ff 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -119,7 +119,9 @@ void stm32_watchdog_pat();
 extern const app_descriptor_t app_descriptor;
 
 extern "C" {
-void can_printf(const char *fmt, ...) FMT_PRINTF(1,2);
+    void can_vprintf(uint8_t severity, const char *fmt, va_list arg);
+    void can_printf_severity(uint8_t severity, const char *fmt, ...) FMT_PRINTF(2,3);
+    void can_printf(const char *fmt, ...) FMT_PRINTF(1,2);
 }
 
 struct CanardInstance;
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index c985403fd282d..82cca004ee5e2 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1852,23 +1852,42 @@ void AP_Periph_FW::can_update()
 }
 
 // printf to CAN LogMessage for debugging
-void can_printf(const char *fmt, ...)
+void can_vprintf(uint8_t severity, const char *fmt, va_list ap)
 {
+    // map MAVLink levels to CAN levels
+    uint8_t level = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG;
+    switch (severity) {
+    case MAV_SEVERITY_DEBUG:
+        level = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_DEBUG;
+        break;
+    case MAV_SEVERITY_INFO:
+        level = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_INFO;
+        break;
+    case MAV_SEVERITY_NOTICE:
+    case MAV_SEVERITY_WARNING:
+        level = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_WARNING;
+        break;
+    case MAV_SEVERITY_ERROR:
+    case MAV_SEVERITY_CRITICAL:
+    case MAV_SEVERITY_ALERT:
+    case MAV_SEVERITY_EMERGENCY:
+        level = UAVCAN_PROTOCOL_DEBUG_LOGLEVEL_ERROR;
+        break;
+    }
+
 #if HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF
     const uint8_t packet_count_max = 4; // how many packets we're willing to break up an over-sized string into
     const uint8_t packet_data_max = 90; // max single debug string length = sizeof(uavcan_protocol_debug_LogMessage.text.data)
     uint8_t buffer_data[packet_count_max*packet_data_max] {};
 
-    va_list ap;
-    va_start(ap, fmt);
     // strip off any negative return errors by treating result as 0
     uint32_t char_count = MAX(vsnprintf((char*)buffer_data, sizeof(buffer_data), fmt, ap), 0);
-    va_end(ap);
 
     // send multiple uavcan_protocol_debug_LogMessage packets if the fmt string is too long.
     uint16_t buffer_offset = 0;
     for (uint8_t i=0; i<packet_count_max && char_count > 0; i++) {
         uavcan_protocol_debug_LogMessage pkt {};
+        pkt.level.value = level;
         pkt.text.len = MIN(char_count, sizeof(pkt.text.data));
         char_count -= pkt.text.len;
 
@@ -1888,10 +1907,8 @@ void can_printf(const char *fmt, ...)
 #else
     uavcan_protocol_debug_LogMessage pkt {};
     uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
-    va_list ap;
-    va_start(ap, fmt);
     uint32_t n = vsnprintf((char*)pkt.text.data, sizeof(pkt.text.data), fmt, ap);
-    va_end(ap);
+    pkt.level.value = level;
     pkt.text.len = MIN(n, sizeof(pkt.text.data));
 
     uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, !periph.canfdout());
@@ -1904,3 +1921,21 @@ void can_printf(const char *fmt, ...)
 
 #endif
 }
+
+// printf to CAN LogMessage for debugging, with severity
+void can_printf_severity(uint8_t severity, const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(severity, fmt, ap);
+    va_end(ap);
+}
+
+// printf to CAN LogMessage for debugging, with DEBUG level
+void can_printf(const char *fmt, ...)
+{
+    va_list ap;
+    va_start(ap, fmt);
+    can_vprintf(MAV_SEVERITY_DEBUG, fmt, ap);
+    va_end(ap);
+}