Initial cut on debug trace facilities

src/main/build/debug.c

@@ -15,14 +15,68 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "stdint.h"
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include <ctype.h>
 
-
+#include "build/debug.h"
-#include "debug.h"
+#include "common/printf.h"
-
+#include "drivers/serial.h"
-int16_t debug[DEBUG16_VALUE_COUNT];
+#include "drivers/time.h"
-uint8_t debugMode;
+#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

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

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

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 {

src/main/main.c

@@ -19,7 +19,7 @@
 #include <stdint.h>
 
 #include "platform.h"
-
+#include "build/debug.h"
 #include "drivers/serial.h"
 #include "drivers/serial_softserial.h"
 

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