AP_Periph: can_printf() to send longer strings using multiple packets

Tools/AP_Periph/can.cpp

@@ -81,6 +81,15 @@ extern AP_Periph_FW periph;
  # define Debug(fmt, args ...)
 #endif
 
+#ifndef HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF
+    // When enabled, can_printf() strings longer than the droneCAN max text length (90 chars)
+    // are split into multiple packets instead of truncating the string. This is
+    // especially helpful with HAL_GCS_ENABLED where libraries use the mavlink
+    // send_text() method where we support strings up to 256 chars by splitting them
+    // up into multiple 50 char mavlink packets.
+    #define HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF (BOARD_FLASH_SIZE >= 1024)
+#endif
+
 static struct instance_t {
     uint8_t index;
 
@@ -2570,6 +2579,38 @@ void AP_Periph_FW::can_efi_update(void)
 // printf to CAN LogMessage for debugging
 void can_printf(const char *fmt, ...)
 {
+#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.text.len = MIN(char_count, sizeof(pkt.text.data));
+        char_count -= pkt.text.len;
+
+        memcpy(pkt.text.data, &buffer_data[buffer_offset], pkt.text.len);
+        buffer_offset += pkt.text.len;
+
+        uint8_t buffer_packet[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
+        const uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer_packet, !periph.canfdout());
+
+        canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE,
+                        UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID,
+                        CANARD_TRANSFER_PRIORITY_LOW,
+                        buffer_packet,
+                        len);
+    }
+    
+#else
     uavcan_protocol_debug_LogMessage pkt {};
     uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE] {};
     va_list ap;
@@ -2586,4 +2627,5 @@ void can_printf(const char *fmt, ...)
                     buffer,
                     len);
 
+#endif
 }