Merge pull request #2691 from iNavFlight/de_debug_traces

Debug trace facilities

src/main/build/debug.c

@@ -15,14 +15,96 @@
  * 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 "build/version.h"
 
-#include "debug.h"
+#include "drivers/serial.h"
+#include "drivers/time.h"
 
-int16_t debug[DEBUG16_VALUE_COUNT];
-uint8_t debugMode;
+#include "common/printf.h"
+
+#include "config/feature.h"
+
+#include "io/serial.h"
+
+#include "fc/config.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)
+{
+    if (!feature(FEATURE_DEBUG_TRACE)) {
+        return;
+    }
+
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DEBUG_TRACE);
+    if (!portConfig) {
+        return;
+    }
+
+    tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
+
+    DEBUG_TRACE_SYNC("%s/%s %s %s / %s (%s)",
+        FC_FIRMWARE_NAME,
+        targetName,
+        FC_VERSION_STRING,
+        buildDate,
+        buildTime,
+        shortGitRevision
+    );
+}
+
+static void debugTracePutcp(void *p, char ch)
+{
+    *(*((char **) p))++ = ch;
+}
+
+void debugTracePrintf(bool synchronous, const char *format, ...)
+{
+    char buf[128];
+    char *bufPtr;
+    int charCount;
+
+    if (!feature(FEATURE_DEBUG_TRACE))
+        return;
+
+    // Write timestamp
+    const timeMs_t timeMs = millis();
+    charCount = tfp_sprintf(buf, "[%6d.%03d] ", timeMs / 1000, timeMs % 1000);
+    bufPtr = &buf[charCount];
+
+    // Write message
+    va_list va;
+    va_start(va, format);
+    charCount += tfp_format(&bufPtr, debugTracePutcp, format, va);
+    debugTracePutcp(&bufPtr, '\n');
+    debugTracePutcp(&bufPtr, 0);
+    charCount += 2;
+    va_end(va);
+
+    if (tracePort) {
+        // Send data via trace UART (if configured)
+        serialPrint(tracePort, buf);
+        if (synchronous) {
+            waitForSerialPortToFinishTransmitting(tracePort);
+        }
+    }
+    else {
+        // Send data via MSPV2_TRACE message
+        // TODO
+    }
+}
+#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/cli.c

@@ -160,8 +160,7 @@ static const char * const featureNames[] = {
     "", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    "SUPEREXPO", "VTX", "RX_SPI", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", "OSD", "FW_LAUNCH", NULL
-
+    "SUPEREXPO", "VTX", "RX_SPI", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE", "OSD", "FW_LAUNCH", "TRACE", NULL
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */

src/main/fc/config.h

@@ -74,6 +74,7 @@ typedef enum {
     FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
     FEATURE_OSD = 1 << 29,
     FEATURE_FW_LAUNCH = 1 << 30,
+    FEATURE_DEBUG_TRACE = 1 << 31,
 } features_e;
 
 typedef struct systemConfig_s {

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/sensors/barometer.c

@@ -270,6 +270,7 @@ static void performBaroCalibrationCycle(void)
         if ((millis() - baroCalibrationTimeout) > 250) {
             baroGroundAltitude = pressureToAltitude(baroGroundPressure);
             baroCalibrationFinished = true;
+            DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
         }
     }
     else {

src/main/sensors/gyro.c

@@ -352,6 +352,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
     }
 
     if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
+        DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]);
         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
     }
     gyroCalibration->calibratingG--;

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