diff --git a/src/main/build/debug.c b/src/main/build/debug.c index e60a47d40d..6c46611cf4 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -15,14 +15,68 @@ * along with Cleanflight. If not, see . */ -#include "stdint.h" +#include +#include +#include +#include - -#include "debug.h" - -int16_t debug[DEBUG16_VALUE_COUNT]; -uint8_t debugMode; +#include "build/debug.h" +#include "common/printf.h" +#include "drivers/serial.h" +#include "drivers/time.h" +#include "io/serial.h" #ifdef DEBUG_SECTION_TIMES timeUs_t sectionTimes[2][4]; #endif + +int16_t debug[DEBUG16_VALUE_COUNT]; +uint8_t debugMode; + +#if defined(USE_DEBUG_TRACE) +static serialPort_t * tracePort = NULL; + +void debugTraceInit(void) +{ + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DEBUG_TRACE); + if (!portConfig) { + return false; + } + + tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED); + if (!tracePort) + return; + + DEBUG_TRACE_SYNC("Debug trace facilities initialized"); +} + +static void debugTracePutp(void *p, char ch) +{ + (void)p; + serialWrite(tracePort, ch); +} + +void debugTracePrintf(bool synchronous, const char *format, ...) +{ + char timestamp[16]; + + if (!tracePort) + return; + + // Write timestamp + tfp_sprintf(timestamp, "[%10d] ", micros()); + serialPrint(tracePort, timestamp); + + // Write message + va_list va; + va_start(va, format); + tfp_format(NULL, debugTracePutp, format, va); + if (synchronous) { + waitForSerialPortToFinishTransmitting(tracePort); + } + va_end(va); + + // Write newline + serialPrint(tracePort, "\r\n"); +} +#endif diff --git a/src/main/build/debug.h b/src/main/build/debug.h index bfaf6b11b9..b1dcc4c591 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -57,3 +57,13 @@ typedef enum { DEBUG_FLOW_RAW, DEBUG_COUNT } debugType_e; + +#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__) +#else +#define DEBUG_TRACE(fmt, ...) +#define DEBUG_TRACE_SYNC(fmt, ...) +#endif \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index fec489b95a..9add56fdab 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -255,6 +255,12 @@ void init(void) serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE); #endif +#if defined(USE_DEBUG_TRACE) + // Debug trace uses serial output, so we only can init it after serial port is ready + // From this point on we can use DEBUG_TRACE() to produce real-time debugging information + debugTraceInit(); +#endif + #ifdef USE_SERVOS servosInit(); mixerUpdateStateFlags(); // This needs to be called early to allow pwm mapper to use information about FIXED_WING state diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 2a05c7a71b..81a07afcd1 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -46,6 +46,7 @@ typedef enum { FUNCTION_VTX_TRAMP = (1 << 12), // 4096 FUNCTION_UAV_INTERCONNECT = (1 << 13), // 8192 FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 + FUNCTION_DEBUG_TRACE = (1 << 15), // 32768 } serialPortFunction_e; typedef enum { diff --git a/src/main/main.c b/src/main/main.c index 818807236f..7a46b485d7 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -19,7 +19,7 @@ #include #include "platform.h" - +#include "build/debug.h" #include "drivers/serial.h" #include "drivers/serial_softserial.h" diff --git a/src/main/target/common.h b/src/main/target/common.h index b4e00bc340..55e079b21b 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -62,6 +62,7 @@ #define NAV_FIXED_WING_LANDING #define AUTOTUNE_FIXED_WING #define USE_ASYNC_GYRO_PROCESSING +#define USE_DEBUG_TRACE #define USE_BOOTLOG #define BOOTLOG_DESCRIPTIONS #define USE_STATS