From 2f677f007d5570ae97271fe70a21fea4de975b83 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 26 Jan 2018 12:05:00 +1000 Subject: [PATCH 1/4] Initial cut on debug trace facilities --- src/main/build/debug.c | 62 +++++++++++++++++++++++++++++++++++++--- src/main/build/debug.h | 10 +++++++ src/main/fc/fc_init.c | 6 ++++ src/main/io/serial.h | 1 + src/main/main.c | 2 +- src/main/target/common.h | 1 + 6 files changed, 77 insertions(+), 5 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index e60a47d40d8..6c46611cf4a 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 "build/debug.h" +#include "common/printf.h" +#include "drivers/serial.h" +#include "drivers/time.h" +#include "io/serial.h" -#include "debug.h" +#ifdef DEBUG_SECTION_TIMES +timeUs_t sectionTimes[2][4]; +#endif int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode; -#ifdef DEBUG_SECTION_TIMES -timeUs_t sectionTimes[2][4]; +#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 bfaf6b11b98..b1dcc4c5910 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 fec489b95a8..9add56fdab0 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 2a05c7a71b8..81a07afcd19 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 818807236f6..7a46b485d79 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 b4e00bc3409..55e079b21b7 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 From d0c1cd8fcde188dbac5e695d3b4d66c6ca113375 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 28 Jan 2018 16:36:14 +1000 Subject: [PATCH 2/4] Refactor trace string creation --- src/main/build/debug.c | 53 ++++++++++++++++++++++++++++-------------- src/main/fc/cli.c | 3 +-- src/main/fc/config.h | 1 + 3 files changed, 38 insertions(+), 19 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 6c46611cf4a..2b4b3065dd0 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -21,11 +21,18 @@ #include #include "build/debug.h" -#include "common/printf.h" + #include "drivers/serial.h" #include "drivers/time.h" + +#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 @@ -38,45 +45,57 @@ 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 false; + return; } 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) +static void debugTracePutcp(void *p, char ch) { - (void)p; - serialWrite(tracePort, ch); + *(*((char **) p))++ = ch; } void debugTracePrintf(bool synchronous, const char *format, ...) { - char timestamp[16]; + char buf[128]; + char *bufPtr; + int charCount; - if (!tracePort) + if (!feature(FEATURE_DEBUG_TRACE)) return; // Write timestamp - tfp_sprintf(timestamp, "[%10d] ", micros()); - serialPrint(tracePort, timestamp); + charCount = tfp_sprintf(buf, "[%10d] ", micros()); + bufPtr = &buf[charCount]; // Write message va_list va; va_start(va, format); - tfp_format(NULL, debugTracePutp, format, va); - if (synchronous) { - waitForSerialPortToFinishTransmitting(tracePort); - } + charCount += tfp_format(&bufPtr, debugTracePutcp, format, va); + debugTracePutcp(&bufPtr, '\n'); + debugTracePutcp(&bufPtr, 0); + charCount += 2; va_end(va); - // Write newline - serialPrint(tracePort, "\r\n"); + 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 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 13aaf554e7c..808244d4d5b 100755 --- a/src/main/fc/cli.c +++ b/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) */ diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ae9dc5263e2..32df86c9f5c 100644 --- a/src/main/fc/config.h +++ b/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 { From 322ffe7d871a769ada1dd80d89e31bb517cd9294 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 11 Feb 2018 21:48:21 +1000 Subject: [PATCH 3/4] Trace output version on boot. Add example traces to gyro and baro calibration --- src/main/build/debug.c | 10 +++++++++- src/main/sensors/barometer.c | 1 + src/main/sensors/gyro.c | 1 + 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 2b4b3065dd0..648eacdfbad 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -21,6 +21,7 @@ #include #include "build/debug.h" +#include "build/version.h" #include "drivers/serial.h" #include "drivers/time.h" @@ -56,7 +57,14 @@ void debugTraceInit(void) tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED); - DEBUG_TRACE_SYNC("Debug trace facilities initialized"); + 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) diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 7cb1fb3ebf0..d5e12f6a1eb 100644 --- a/src/main/sensors/barometer.c +++ b/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 { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9d6d355b532..79620532029 100644 --- a/src/main/sensors/gyro.c +++ b/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--; From d9d904cee139efaf36cc2beaf72383c0ef4250f0 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 11 Feb 2018 22:17:31 +1000 Subject: [PATCH 4/4] Change from microsecond to second/millisecond timestamp --- src/main/build/debug.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 648eacdfbad..5a90e991961 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -82,7 +82,8 @@ void debugTracePrintf(bool synchronous, const char *format, ...) return; // Write timestamp - charCount = tfp_sprintf(buf, "[%10d] ", micros()); + const timeMs_t timeMs = millis(); + charCount = tfp_sprintf(buf, "[%6d.%03d] ", timeMs / 1000, timeMs % 1000); bufPtr = &buf[charCount]; // Write message