From 4e94046a4a249f90b6a63903711455ec7040e6b2 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 13 Oct 2022 11:23:33 -0700 Subject: [PATCH] AP_Periph: can_printf() to send longer strings using multiple packets --- Tools/AP_Periph/can.cpp | 42 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 1968321342..29c86d6a87 100644 --- a/Tools/AP_Periph/can.cpp +++ b/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 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 }