Replace DEBUG_TRACE with LOG

LOG system has multiple levels, selectable both at compile and run
times. FEATURE_TRACE has been removed, since we now rely just on
the log level/topic and the defined outputs for the log messages.

make/source.mk

@@ -7,12 +7,15 @@ COMMON_SRC = \
             build/debug.c \
             build/version.c \
             common/bitarray.c \
+            common/calibration.c \
+            common/colorconversion.c \
             common/crc.c \
             common/encoding.c \
             common/filter.c \
+            common/gps_conversion.c \
+            common/log.c \
             common/logic_condition.c \
             common/maths.c \
-            common/calibration.c \
             common/memory.c \
             common/olc.c \
             common/printf.c \
@@ -146,8 +149,6 @@ COMMON_SRC = \
             cms/cms_menu_vtx_smartaudio.c \
             cms/cms_menu_vtx_tramp.c \
             cms/cms_menu_vtx_ffpv.c \
-            common/colorconversion.c \
-            common/gps_conversion.c \
             drivers/display_ug2864hsweg01.c \
             drivers/rangefinder/rangefinder_hcsr04.c \
             drivers/rangefinder/rangefinder_hcsr04_i2c.c \

src/main/build/debug.c

@@ -15,29 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdarg.h>
-#include <ctype.h>
-
 #include "build/debug.h"
-#include "build/version.h"
-
-#include "drivers/serial.h"
-#include "drivers/time.h"
-
-#include "common/printf.h"
-#include "common/utils.h"
-
-#include "config/feature.h"
-
-#include "io/serial.h"
-
-#include "fc/config.h"
-
-#include "msp/msp.h"
-#include "msp/msp_serial.h"
-#include "msp/msp_protocol.h"
 
 #ifdef DEBUG_SECTION_TIMES
 timeUs_t sectionTimes[2][4];
@@ -45,146 +23,3 @@ timeUs_t sectionTimes[2][4];
 
 int32_t debug[DEBUG32_VALUE_COUNT];
 uint8_t debugMode;
-
-#if defined(USE_DEBUG_TRACE)
-
-#define DEBUG_TRACE_PREFIX                  "[%6d.%03d] "
-#define DEBUG_TRACE_PREFIX_FORMATTED_SIZE   13
-
-static serialPort_t * tracePort = NULL;
-static mspPort_t * mspTracePort = NULL;
-
-void debugTraceInit(void)
-{
-    if (!feature(FEATURE_DEBUG_TRACE)) {
-        return;
-    }
-
-    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DEBUG_TRACE);
-    if (!portConfig) {
-        return;
-    }
-
-    bool tracePortIsSharedWithMSP = false;
-
-    if (determinePortSharing(portConfig, FUNCTION_DEBUG_TRACE) == PORTSHARING_SHARED) {
-        // We support sharing a DEBUG_TRACE port only with MSP
-        if (portConfig->functionMask != (FUNCTION_DEBUG_TRACE | FUNCTION_MSP)) {
-            return;
-        }
-        tracePortIsSharedWithMSP = true;
-    }
-
-    // If the port is shared with MSP, reuse the port
-    if (tracePortIsSharedWithMSP) {
-        const serialPort_t *traceAndMspPort = findSharedSerialPort(FUNCTION_DEBUG_TRACE, FUNCTION_MSP);
-        if (!traceAndMspPort) {
-            return;
-        }
-
-        mspTracePort = mspSerialPortFind(traceAndMspPort);
-        if (!mspTracePort) {
-            return;
-        }
-
-    } else {
-        tracePort = openSerialPort(portConfig->identifier, FUNCTION_DEBUG_TRACE, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
-        if (!tracePort) {
-            return;
-        }
-    }
-    // Initialization done
-    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;
-}
-
-static void debugTracePrint(bool synchronous, const char *buf, size_t size)
-{
-    if (tracePort) {
-        // Send data via trace UART (if configured & connected - a safeguard against zombie VCP)
-        if (serialIsConnected(tracePort)) {
-            serialPrint(tracePort, buf);
-            if (synchronous) {
-                waitForSerialPortToFinishTransmitting(tracePort);
-            }
-        }
-    } else if (mspTracePort) {
-        mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspTracePort, MSP_V2_NATIVE);
-    }
-}
-
-static size_t debugTraceFormatPrefix(char *buf, const timeMs_t timeMs)
-{
-    // Write timestamp
-    return tfp_sprintf(buf, DEBUG_TRACE_PREFIX, timeMs / 1000, timeMs % 1000);
-}
-
-void debugTracePrintf(bool synchronous, const char *format, ...)
-{
-    char buf[128];
-    char *bufPtr;
-    int charCount;
-
-    STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_debug_trace);
-
-    if (!feature(FEATURE_DEBUG_TRACE))
-        return;
-
-    charCount = debugTraceFormatPrefix(buf, millis());
-    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);
-
-    debugTracePrint(synchronous, buf, charCount);
-}
-
-void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size)
-{
-    // Print lines of up to maxBytes bytes. We need 5 characters per byte
-    // 0xAB[space|\n]
-    const size_t charsPerByte = 5;
-    const size_t maxBytes = 8;
-    char buf[DEBUG_TRACE_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator
-    size_t bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE;
-    const uint8_t *inputPtr = buffer;
-
-    debugTraceFormatPrefix(buf, millis());
-
-    for (size_t ii = 0; ii < size; ii++) {
-        tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]);
-        bufPos += charsPerByte;
-        if (bufPos == sizeof(buf)-1) {
-            buf[bufPos-1] = '\n';
-            buf[bufPos] = '\0';
-            debugTracePrint(synchronous, buf, bufPos + 1);
-            bufPos = DEBUG_TRACE_PREFIX_FORMATTED_SIZE;
-        }
-    }
-
-    if (bufPos > DEBUG_TRACE_PREFIX_FORMATTED_SIZE) {
-        buf[bufPos-1] = '\n';
-        buf[bufPos] = '\0';
-        debugTracePrint(synchronous, buf, bufPos + 1);
-    }
-}
-
-
-#endif

src/main/build/debug.h

@@ -71,18 +71,3 @@ typedef enum {
     DEBUG_GENERIC,
     DEBUG_COUNT
 } debugType_e;
-
-#if defined(USE_DEBUG_TRACE)
-void debugTraceInit(void);
-void debugTracePrintf(bool synchronous, const char *format, ...);
-void debugTracePrintBufferHex(bool synchronous, const void *buffer, size_t size);
-#define DEBUG_TRACE(fmt, ...)                   debugTracePrintf(false, fmt, ##__VA_ARGS__)
-#define DEBUG_TRACE_SYNC(fmt, ...)              debugTracePrintf(true, fmt, ##__VA_ARGS__)
-#define DEBUG_TRACE_BUFFER_HEX(buf, size)       debugTracePrintBufferHex(false, buf, size)
-#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size)  debugTracePrintBufferHex(true, buf, size)
-#else
-#define DEBUG_TRACE(fmt, ...)
-#define DEBUG_TRACE_SYNC(fmt, ...)
-#define DEBUG_TRACE_BUFFER_HEX(buf, size)
-#define DEBUG_TRACE_BUFFER_HEX_SYNC(buf, size)
-#endif

src/main/common/log.c

@@ -0,0 +1,196 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include <ctype.h>
+
+#include "build/version.h"
+
+#include "drivers/serial.h"
+#include "drivers/time.h"
+
+#include "common/log.h"
+#include "common/printf.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+#include "config/parameter_group_ids.h"
+
+#include "io/serial.h"
+
+#include "fc/config.h"
+
+#include "msp/msp.h"
+#include "msp/msp_serial.h"
+#include "msp/msp_protocol.h"
+
+#if defined(USE_LOG)
+
+#define LOG_PREFIX                  "[%6d.%03d] "
+#define LOG_PREFIX_FORMATTED_SIZE   13
+
+static serialPort_t * logPort = NULL;
+static mspPort_t * mspLogPort = NULL;
+
+PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
+
+void logInit(void)
+{
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG);
+    if (!portConfig) {
+        return;
+    }
+
+    bool portIsSharedWithMSP = false;
+
+    if (determinePortSharing(portConfig, FUNCTION_LOG) == PORTSHARING_SHARED) {
+        // We support sharing a LOG port only with MSP
+        if (portConfig->functionMask != (FUNCTION_LOG | FUNCTION_MSP)) {
+            return;
+        }
+        portIsSharedWithMSP = true;
+    }
+
+    // If the port is shared with MSP, reuse the port
+    if (portIsSharedWithMSP) {
+        const serialPort_t *logAndMspPort = findSharedSerialPort(FUNCTION_LOG, FUNCTION_MSP);
+        if (!logAndMspPort) {
+            return;
+        }
+
+        mspLogPort = mspSerialPortFind(logAndMspPort);
+        if (!mspLogPort) {
+            return;
+        }
+
+    } else {
+        logPort = openSerialPort(portConfig->identifier, FUNCTION_LOG, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
+        if (!logPort) {
+            return;
+        }
+    }
+    // Initialization done
+    LOG_I_SYNC(SYSTEM, "%s/%s %s %s / %s (%s)",
+        FC_FIRMWARE_NAME,
+        targetName,
+        FC_VERSION_STRING,
+        buildDate,
+        buildTime,
+        shortGitRevision
+    );
+}
+
+static void logPutcp(void *p, char ch)
+{
+    *(*((char **) p))++ = ch;
+}
+
+static void logPrint(bool synchronous, const char *buf, size_t size)
+{
+    if (logPort) {
+        // Send data via UART (if configured & connected - a safeguard against zombie VCP)
+        if (serialIsConnected(logPort)) {
+            serialPrint(logPort, buf);
+            if (synchronous) {
+                waitForSerialPortToFinishTransmitting(logPort);
+            }
+        }
+    } else if (mspLogPort) {
+        mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspLogPort, MSP_V2_NATIVE);
+    }
+}
+
+static size_t logFormatPrefix(char *buf, const timeMs_t timeMs)
+{
+    // Write timestamp
+    return tfp_sprintf(buf, LOG_PREFIX, timeMs / 1000, timeMs % 1000);
+}
+
+static bool logHasOutput(void)
+{
+    return logPort || mspLogPort;
+}
+
+static bool logIsEnabled(logTopic_e topic, unsigned level)
+{
+    return logHasOutput() && (level <= logConfig()->level || (logConfig()->topics & (1 << topic)));
+}
+
+void _logf(logTopic_e topic, unsigned level, bool synchronous, const char *fmt, ...)
+{
+    char buf[128];
+    char *bufPtr;
+    int charCount;
+
+    STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_log);
+
+    if (!logIsEnabled(topic, level)) {
+        return;
+    }
+
+    charCount = logFormatPrefix(buf, millis());
+    bufPtr = &buf[charCount];
+
+    // Write message
+    va_list va;
+    va_start(va, fmt);
+    charCount += tfp_format(&bufPtr, logPutcp, fmt, va);
+    logPutcp(&bufPtr, '\n');
+    logPutcp(&bufPtr, 0);
+    charCount += 2;
+    va_end(va);
+
+    logPrint(synchronous, buf, charCount);
+}
+
+void _logBufferHex(logTopic_e topic, unsigned level, bool synchronous, const void *buffer, size_t size)
+{
+    // Print lines of up to maxBytes bytes. We need 5 characters per byte
+    // 0xAB[space|\n]
+    const size_t charsPerByte = 5;
+    const size_t maxBytes = 8;
+    char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator
+    size_t bufPos = LOG_PREFIX_FORMATTED_SIZE;
+    const uint8_t *inputPtr = buffer;
+
+    if (!logIsEnabled(topic, level)) {
+        return;
+    }
+
+    logFormatPrefix(buf, millis());
+
+    for (size_t ii = 0; ii < size; ii++) {
+        tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]);
+        bufPos += charsPerByte;
+        if (bufPos == sizeof(buf)-1) {
+            buf[bufPos-1] = '\n';
+            buf[bufPos] = '\0';
+            logPrint(synchronous, buf, bufPos + 1);
+            bufPos = LOG_PREFIX_FORMATTED_SIZE;
+        }
+    }
+
+    if (bufPos > LOG_PREFIX_FORMATTED_SIZE) {
+        buf[bufPos-1] = '\n';
+        buf[bufPos] = '\0';
+        logPrint(synchronous, buf, bufPos + 1);
+    }
+}
+
+#endif

src/main/common/log.h

@@ -0,0 +1,112 @@
+#pragma once
+
+#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "config/parameter_group.h"
+
+#include "common/utils.h"
+
+// Log levels. Defined as preprocessor constants instead of
+// a number to allow compile-time comparisons.
+#define LOG_LEVEL_ERROR 0
+#define LOG_LEVEL_WARNING 1
+#define LOG_LEVEL_INFO 2
+#define LOG_LEVEL_VERBOSE 3
+#define LOG_LEVEL_DEBUG 4
+
+typedef enum {
+    LOG_TOPIC_SYSTEM,           // 0, mask = 1
+    LOG_TOPIC_GYRO,             // 1, mask = 2
+    LOG_TOPIC_BARO,             // 2, mask = 4
+    LOG_TOPIC_PITOT,            // 3, mask = 8
+    LOG_TOPIC_PWM,              // 4, mask = 16
+    LOG_TOPIC_TIMER,            // 5, mask = 32
+    LOG_TOPIC_IMU,              // 6, mask = 64
+    LOG_TOPIC_TEMPERATURE,      // 7, mask = 128
+    LOG_TOPIC_POS_ESTIMATOR,    // 8, mask = 256
+
+    LOG_TOPIC_COUNT,
+} logTopic_e;
+
+STATIC_ASSERT(LOG_TOPIC_COUNT < 32, too_many_log_topics);
+
+typedef struct logConfig_s {
+    uint8_t level; // from LOG_LEVEL_ constants. All messages equal or below this verbosity level are printed.
+    uint32_t topics; // All messages with topics in this bitmask (1 << topic) will be printed regardless of their level.
+} logConfig_t;
+
+PG_DECLARE(logConfig_t, logConfig);
+
+void logInit(void);
+void _logf(logTopic_e topic, unsigned level, bool synchronous, const char *fmt, ...) __attribute__ ((format (printf, 4, 5)));
+void _logBufferHex(logTopic_e topic, unsigned level, bool synchronous, const void *buffer, size_t size);
+
+// LOG_* macro definitions
+
+#if !defined(LOG_LEVEL_MAXIMUM)
+#define LOG_LEVEL_MAXIMUM LOG_LEVEL_DEBUG
+#endif
+
+#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_ERROR
+#define LOG_E(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, false, fmt, ##__VA_ARGS__)
+#define LOG_E_SYNC(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, true, fmt, ##__VA_ARGS__)
+#define LOG_BUFFER_E(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, false, buf, size)
+#define LOG_BUFFER_E_SYNC(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_ERROR, true, buf, size)
+#else
+#define LOG_E(...)
+#define LOG_E_SYNC(...)
+#define LOG_BUFFER_E(...)
+#define LOG_BUFFER_E_SYNC(...)
+#endif
+
+#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_WARNING
+#define LOG_W(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, false, fmt, ##__VA_ARGS__)
+#define LOG_W_SYNC(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, true, fmt, ##__VA_ARGS__)
+#define LOG_BUF_W(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, false, buf, size)
+#define LOG_BUF_W_SYNC(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_WARNING, true, buf, size)
+#else
+#define LOG_W(...)
+#define LOG_W_SYNC(...)
+#define LOG_BUF_W(...)
+#define LOG_BUF_W_SYNC(...)
+#endif
+
+#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_INFO
+#define LOG_I(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, false, fmt, ##__VA_ARGS__)
+#define LOG_I_SYNC(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, true, fmt, ##__VA_ARGS__)
+#define LOG_BUF_I(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, false, buf, size)
+#define LOG_BUF_I_SYNC(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_INFO, true, buf, size)
+#else
+#define LOG_I(...)
+#define LOG_I_SYNC(...)
+#define LOG_BUF_I(...)
+#define LOG_BUF_I_SYNC(...)
+#endif
+
+#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_VERBOSE
+#define LOG_V(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, false, fmt, ##__VA_ARGS__)
+#define LOG_V_SYNC(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, true, fmt, ##__VA_ARGS__)
+#define LOG_BUF_V(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, false, buf, size)
+#define LOG_BUF_V_SYNC(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_VERBOSE, true, buf, size)
+#else
+#define LOG_V(...)
+#define LOG_V_SYNC(...)
+#define LOG_BUF_V(...)
+#define LOG_BUF_V_SYNC(...)
+#endif
+
+#if defined(USE_LOG) && LOG_LEVEL_MAXIMUM >= LOG_LEVEL_DEBUG
+#define LOG_D(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, false, fmt, ##__VA_ARGS__)
+#define LOG_D_SYNC(topic, fmt, ...) _logf(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, true, fmt, ##__VA_ARGS__)
+#define LOG_BUF_D(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, false, buf, size)
+#define LOG_BUF_D_SYNC(topic, buf, size) _logBufferHex(LOG_TOPIC_ ## topic, LOG_LEVEL_DEBUG, true, buf, size)
+#else
+#define LOG_D(...)
+#define LOG_D_SYNC(...)
+#define LOG_BUF_D(...)
+#define LOG_BUF_D_SYNC(...)
+#endif

src/main/common/memory.c

@@ -27,9 +27,11 @@
 
 #include "platform.h"
 
-#include "build/debug.h"
+#include "common/log.h"
-#include "drivers/resource.h"
 #include "common/memory.h"
+
+#include "drivers/resource.h"
+
 #include "fc/runtime_config.h"
 
 #if !defined(DYNAMIC_HEAP_SIZE)
@@ -50,11 +52,11 @@ void * memAllocate(size_t wantedSize, resourceOwner_e owner)
         retPointer = &dynHeap[dynHeapFreeWord];
         dynHeapFreeWord += wantedWords;
         dynHeapUsage[owner] += wantedWords * sizeof(uint32_t);
-        DEBUG_TRACE("Memory allocated. Free memory = %d", memGetAvailableBytes());
+        LOG_D(SYSTEM, "Memory allocated. Free memory = %d", memGetAvailableBytes());
     }
     else {
         // OOM
-        DEBUG_TRACE("Out of memory");
+        LOG_E(SYSTEM, "Out of memory");
         ENABLE_ARMING_FLAG(ARMING_DISABLED_OOM);
     }
 

src/main/config/parameter_group_ids.h

@@ -104,7 +104,8 @@
 #define PG_LIGHTS_CONFIG 1014
 #define PG_PINIOBOX_CONFIG 1015
 #define PG_LOGIC_CONDITIONS 1016
-#define PG_INAV_END 1016
+#define PG_LOG_CONFIG 1017
+#define PG_INAV_END 1017
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/accgyro/accgyro.c

@@ -24,8 +24,8 @@
 
 #include "build/atomic.h"
 #include "build/build_config.h"
-#include "build/debug.h"
 
+#include "common/log.h"
 #include "common/maths.h"
 #include "common/utils.h"
 
@@ -62,7 +62,7 @@ const gyroFilterAndRateConfig_t * chooseGyroConfig(uint8_t desiredLpf, uint16_t
         }
     }
 
-    DEBUG_TRACE("GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X",
+    LOG_V(GYRO, "GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X",
                 desiredLpf, desiredRateHz,
                 candidate->gyroLpf, candidate->gyroRateHz,
                 candidate->gyroConfigValues[0], candidate->gyroConfigValues[1]);

src/main/drivers/pwm_output.c

@@ -21,8 +21,8 @@
 #include <string.h>
 
 #include "platform.h"
-#include "build/debug.h"
 
+#include "common/log.h"
 #include "common/maths.h"
 
 #include "drivers/io.h"
@@ -113,7 +113,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
 static pwmOutputPort_t *pwmOutConfigMotor(const timerHardware_t *timHw, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
 {
     if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
-        DEBUG_TRACE("Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
+        LOG_E(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
         return NULL;
     }
 

src/main/drivers/timer.c

@@ -23,10 +23,10 @@
 #include "platform.h"
 
 #include "build/atomic.h"
-#include "build/debug.h"
 
-#include "common/utils.h"
+#include "common/log.h"
 #include "common/memory.h"
+#include "common/utils.h"
 
 #include "drivers/io.h"
 #include "drivers/rcc.h"
@@ -80,7 +80,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw)
     const int timerIndex = lookupTimerIndex(timHw->tim);
     
     if (timerIndex >= HARDWARE_TIMER_DEFINITION_COUNT) {
-        DEBUG_TRACE("Can't find hardware timer definition");
+        LOG_E(TIMER, "Can't find hardware timer definition");
         return NULL;
     }
 
@@ -90,7 +90,7 @@ TCH_t * timerGetTCH(const timerHardware_t * timHw)
         
         // Check for OOM
         if (timerCtx[timerIndex] == NULL) {
-            DEBUG_TRACE("Can't allocate TCH object");
+            LOG_E(TIMER, "Can't allocate TCH object");
             return NULL;
         }
 

src/main/fc/cli.c

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

src/main/fc/config.h

@@ -67,7 +67,6 @@ 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

@@ -30,9 +30,10 @@
 
 #include "common/axis.h"
 #include "common/color.h"
+#include "common/log.h"
 #include "common/maths.h"
-#include "common/printf.h"
 #include "common/memory.h"
+#include "common/printf.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -260,15 +261,15 @@ void init(void)
     serialInit(feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 #endif
 
-    // Initialize MSP serial ports here so DEBUG_TRACE can share a port with MSP.
+    // Initialize MSP serial ports here so LOG can share a port with MSP.
     // XXX: Don't call mspFcInit() yet, since it initializes the boxes and needs
     // to run after the sensors have been detected.
     mspSerialInit();
 
-#if defined(USE_DEBUG_TRACE)
+#if defined(USE_LOG)
-    // Debug trace uses serial output, so we only can init it after serial port is ready
+    // LOG might use 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
+    // From this point on we can use LOG_*() to produce real-time debugging information
-    debugTraceInit();
+    logInit();
 #endif
 
     servosInit();

src/main/fc/settings.yaml

@@ -112,6 +112,8 @@ tables:
     enum: vtxLowerPowerDisarm_e
   - name: filter_type
     values: ["PT1", "BIQUAD"]
+  - name: log_level
+    values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -1811,3 +1813,16 @@ groups:
         min: 0
         max: 255
         type: uint8_t
+
+  - name: PG_LOG_CONFIG
+    type: logConfig_t
+    headers: ["common/log.h"]
+    condition: USE_LOG
+    members:
+        - name: log_level
+          field: level
+          table: log_level
+        - name: log_topics
+          field: topics
+          min: 0,
+          max: UINT32_MAX

src/main/flight/imu.c

@@ -30,6 +30,7 @@
 
 #include "common/axis.h"
 #include "common/filter.h"
+#include "common/log.h"
 #include "common/maths.h"
 #include "common/vector.h"
 #include "common/quaternion.h"
@@ -265,13 +266,13 @@ static void imuCheckAndResetOrientationQuaternion(const fpQuaternion_t * quat, c
         // Previous quaternion valid. Reset to it
         orientation = *quat;
         imuErrorEvent.errorCode = 1;
-        DEBUG_TRACE("AHRS orientation quaternion error. Reset to last known good value");
+        LOG_E(IMU, "AHRS orientation quaternion error. Reset to last known good value");
     }
     else {
         // No valid reference. Best guess from accelerometer
         imuResetOrientationQuaternion(accBF);
         imuErrorEvent.errorCode = 2;
-        DEBUG_TRACE("AHRS orientation quaternion error. Best guess from ACC");
+        LOG_E(IMU, "AHRS orientation quaternion error. Best guess from ACC");
     }
 
 #ifdef USE_BLACKBOX

src/main/io/serial.c

@@ -115,7 +115,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
         serialConfig->portConfigs[i].peripheral_baudrateIndex = BAUD_115200;
     }
 
-    serialConfig->portConfigs[0].functionMask = FUNCTION_MSP | FUNCTION_DEBUG_TRACE;
+    serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
 
 #ifdef SERIALRX_UART
     serialPortConfig_t *serialRxUartConfig = serialFindPortConfiguration(SERIALRX_UART);
@@ -263,7 +263,7 @@ serialPort_t *findNextSharedSerialPort(uint32_t functionMask, serialPortFunction
 }
 
 #define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
-#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_DEBUG_TRACE)
+#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK | FUNCTION_LOG)
 
 bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck)
 {

src/main/io/serial.h

@@ -46,7 +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
+    FUNCTION_LOG                 = (1 << 15), // 32768
     FUNCTION_RANGEFINDER         = (1 << 16), // 65536
     FUNCTION_VTX_FFPV            = (1 << 17), // 131072
 } serialPortFunction_e;

src/main/navigation/navigation_pos_estimator.c

@@ -25,9 +25,9 @@
 #if defined(USE_NAV)
 
 #include "build/build_config.h"
-#include "build/debug.h"
 
 #include "common/axis.h"
+#include "common/log.h"
 #include "common/maths.h"
 
 #include "config/parameter_group.h"
@@ -388,7 +388,7 @@ static void updateIMUTopic(void)
 
             if (gravityCalibrationComplete()) {
                 zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
-                DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS));
+                LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
             }
         }
 

src/main/sensors/barometer.c

@@ -21,12 +21,11 @@
 
 #include "platform.h"
 
-#include "build/debug.h"
+#include "common/calibration.h"
-
+#include "common/log.h"
 #include "common/maths.h"
 #include "common/time.h"
 #include "common/utils.h"
-#include "common/calibration.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -289,7 +288,7 @@ int32_t baroCalculateAltitude(void)
         if (zeroCalibrationIsCompleteS(&zeroCalibration)) {
             zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure);
             baroGroundAltitude = pressureToAltitude(baroGroundPressure);
-            DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
+            LOG_D(BARO, "Barometer calibration complete (%d)", (int)lrintf(baroGroundAltitude));
         }
 
         baro.BaroAlt = 0;

src/main/sensors/gyro.c

@@ -26,9 +26,10 @@
 #include "build/debug.h"
 
 #include "common/axis.h"
-#include "common/maths.h"
 #include "common/calibration.h"
 #include "common/filter.h"
+#include "common/log.h"
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "config/parameter_group.h"
@@ -381,7 +382,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
         dev->gyroZero[1] = v.v[1];
         dev->gyroZero[2] = v.v[2];
 
-        DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]);
+        LOG_D_SYNC(GYRO, "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
     }
     else {

src/main/sensors/pitotmeter.c

@@ -21,8 +21,7 @@
 
 #include "platform.h"
 
-#include "build/debug.h"
+#include "common/log.h"
-
 #include "common/maths.h"
 #include "common/time.h"
 #include "common/utils.h"
@@ -167,7 +166,7 @@ static void performPitotCalibrationCycle(void)
 
     if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
         zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
-        DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero));
+        LOG_D(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
     }
 }
 

src/main/sensors/temperature.c

@@ -21,8 +21,6 @@
 
 #include "platform.h"
 
-#include "build/debug.h"
-
 #include "common/maths.h"
 #include "common/printf.h"
 

src/main/target/common.h

@@ -101,12 +101,14 @@
 #define BOOTLOG_DESCRIPTIONS
 
 #define NAV_NON_VOLATILE_WAYPOINT_CLI
+#else // FLASH_SIZE < 256
+#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif
 
 #if (FLASH_SIZE > 128)
 #define NAV_FIXED_WING_LANDING
 #define USE_AUTOTUNE_FIXED_WING
-#define USE_DEBUG_TRACE
+#define USE_LOG
 #define USE_STATS
 #define USE_GYRO_NOTCH_1
 #define USE_GYRO_NOTCH_2