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