diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 9029c13a4f..9aaa469366 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -47,6 +47,10 @@ int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode; #if defined(USE_DEBUG_TRACE) + +#define DEBUG_TRACE_PREFIX "[%6d.%03d] " +#define DEBUG_TRACE_PREFIX_FORMATTED_SIZE 13 + static serialPort_t * tracePort = NULL; static mspPort_t * mspTracePort = NULL; @@ -105,6 +109,27 @@ static void debugTracePutcp(void *p, char ch) *(*((char **) p))++ = ch; } +static void debugTracePrint(bool synchronous, const char *buf, size_t size) +{ + if (tracePort) { + // Send data via trace UART (if configured & connected - a safeguard against zombie VCP) + if (serialIsConnected(tracePort)) { + serialPrint(tracePort, buf); + if (synchronous) { + waitForSerialPortToFinishTransmitting(tracePort); + } + } + } else if (mspTracePort) { + mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspTracePort, MSP_V2_NATIVE); + } +} + +static size_t debugTraceFormatPrefix(char *buf, const timeMs_t timeMs) +{ + // Write timestamp + return tfp_sprintf(buf, DEBUG_TRACE_PREFIX, timeMs / 1000, timeMs % 1000); +} + void debugTracePrintf(bool synchronous, const char *format, ...) { char buf[128]; @@ -116,9 +141,7 @@ void debugTracePrintf(bool synchronous, const char *format, ...) if (!feature(FEATURE_DEBUG_TRACE)) return; - // Write timestamp - const timeMs_t timeMs = millis(); - charCount = tfp_sprintf(buf, "[%6d.%03d] ", timeMs / 1000, timeMs % 1000); + charCount = debugTraceFormatPrefix(buf, millis()); bufPtr = &buf[charCount]; // Write message @@ -130,16 +153,38 @@ void debugTracePrintf(bool synchronous, const char *format, ...) charCount += 2; va_end(va); - if (tracePort) { - // Send data via trace UART (if configured & connected - a safeguard against zombie VCP) - if (serialIsConnected(tracePort)) { - serialPrint(tracePort, buf); - if (synchronous) { - waitForSerialPortToFinishTransmitting(tracePort); - } + debugTracePrint(synchronous, buf, charCount); +} + +void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size) +{ + // Print lines of up to maxBytes bytes. We need 5 characters per byte + // 0xAB[space|\n] + const size_t charsPerByte = 5; + const size_t maxBytes = 8; + char buf[DEBUG_TRACE_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator + size_t bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE; + const uint8_t *inputPtr = buffer; + + debugTraceFormatPrefix(buf, millis()); + + for (size_t ii = 0; ii < size; ii++) { + tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]); + bufPos += charsPerByte; + if (bufPos == sizeof(buf)-1) { + buf[bufPos-1] = '\n'; + buf[bufPos] = '\0'; + debugTracePrint(synchronous, buf, bufPos + 1); + bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE; } - } else if (mspTracePort) { - mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, charCount, mspTracePort, MSP_V2_NATIVE); + } + + if (bufPos > DEBUG_TRACE_PREFIX_FORMATTED_SIZE) { + buf[bufPos-1] = '\n'; + buf[bufPos] = '\0'; + debugTracePrint(synchronous, buf, bufPos + 1); } } + + #endif diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b1dcc4c591..4616f388ed 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -15,6 +15,8 @@ * along with Cleanflight. If not, see . */ +#include + #define DEBUG16_VALUE_COUNT 4 extern int16_t debug[DEBUG16_VALUE_COUNT]; extern uint8_t debugMode; @@ -61,9 +63,14 @@ typedef enum { #if defined(USE_DEBUG_TRACE) void debugTraceInit(void); void debugTracePrintf(bool synchronous, const char *format, ...); -#define DEBUG_TRACE(fmt, ...) debugTracePrintf(false, fmt, ##__VA_ARGS__) -#define DEBUG_TRACE_SYNC(fmt, ...) debugTracePrintf(true, fmt, ##__VA_ARGS__) +void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size); +#define DEBUG_TRACE(fmt, ...) debugTracePrintf(false, fmt, ##__VA_ARGS__) +#define DEBUG_TRACE_SYNC(fmt, ...) debugTracePrintf(true, fmt, ##__VA_ARGS__) +#define DEBUG_TRACE_BUFFER_HEX(buf, size) debugTracePrintBufferHex(false, buf, size) +#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size) debugTracePrintBufferHex(true, buf, size) #else #define DEBUG_TRACE(fmt, ...) #define DEBUG_TRACE_SYNC(fmt, ...) +#define DEBUG_TRACE_BUFFER_HEX(buf, size) +#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size) #endif \ No newline at end of file