debug mode via config

src/main/build/debug.c

@@ -21,6 +21,7 @@
 #include "debug.h"
 
 int16_t debug[DEBUG16_VALUE_COUNT];
+uint8_t debugMode;
 
 #ifdef DEBUG_SECTION_TIMES
 uint32_t sectionTimes[2][4];

src/main/build/debug.h

@@ -17,6 +17,9 @@
 
 #define DEBUG16_VALUE_COUNT 4
 extern int16_t debug[DEBUG16_VALUE_COUNT];
+extern uint8_t debugMode;
+
+#define DEBUG_SET(mode, index, value) {if (debugMode == (mode)) {debug[(index)] = (value);}}
 
 #define DEBUG_SECTION_TIMES
 
@@ -39,3 +42,23 @@ extern uint32_t sectionTimes[2][4];
 #define TIME_SECTION_END(index) {}
 
 #endif
+
+typedef enum {
+    DEBUG_NONE,
+    DEBUG_CYCLETIME,
+    DEBUG_BATTERY,
+    DEBUG_GYRO,
+    DEBUG_ACCELEROMETER,
+    DEBUG_MIXER,
+    DEBUG_AIRMODE,
+    DEBUG_PIDLOOP,
+    DEBUG_NOTCH,
+    DEBUG_RC_INTERPOLATION,
+    DEBUG_VELOCITY,
+    DEBUG_DTERM_FILTER,
+    DEBUG_ANGLERATE,
+    DEBUG_ESC_SENSOR,
+    DEBUG_SCHEDULER,
+    DEBUG_STACK,
+    DEBUG_COUNT
+} debugType_e;

src/main/config/config_master.h

@@ -140,6 +140,8 @@ typedef struct master_s {
 
     char name[MAX_NAME_LENGTH + 1];
 
+    uint8_t debug_mode;
+
     uint8_t magic_ef;                       // magic number, should be 0xEF
     uint8_t chk;                            // XOR checksum
     /*

src/main/fc/fc_init.c

@@ -177,6 +177,8 @@ void init(void)
     addBootlogEvent2(BOOT_EVENT_CONFIG_LOADED, BOOT_EVENT_FLAGS_NONE);
     systemState |= SYSTEM_STATE_CONFIG_LOADED;
 
+    debugMode = masterConfig.debug_mode;
+
     systemInit();
 
     i2cSetOverclock(masterConfig.i2c_overclock);

src/main/fc/serial_cli.c

@@ -104,6 +104,7 @@ uint8_t cliMode = 0;
 
 #include "telemetry/frsky.h"
 #include "telemetry/telemetry.h"
+#include "build/debug.h"
 
 #if FLASH_SIZE > 128
 #define PLAY_SOUND
@@ -330,6 +331,25 @@ static const char * const lookupTableAsyncMode[] = {
 };
 #endif
 
+static const char * const lookupTableDebug[DEBUG_COUNT] = {
+    "NONE",
+    "CYCLETIME",
+    "BATTERY",
+    "GYRO",
+    "ACCELEROMETER",
+    "MIXER",
+    "AIRMODE",
+    "PIDLOOP",
+    "NOTCH",
+    "RC_INTERPOLATION",
+    "VELOCITY",
+    "DFILTER",
+    "ANGLERATE",
+    "ESC_SENSOR",
+    "SCHEDULER",
+    "STACK"
+};
+
 typedef struct lookupTableEntry_s {
     const char * const *values;
     const uint8_t valueCount;
@@ -384,6 +404,7 @@ typedef enum {
 #ifdef OSD
     TABLE_OSD,
 #endif
+    TABLE_DEBUG,
 } lookupTableIndex_e;
 
 static const lookupTableEntry_t lookupTables[] = {
@@ -434,6 +455,7 @@ static const lookupTableEntry_t lookupTables[] = {
 #endif
 #ifdef OSD
     { lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
+    { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
 #endif
 };
 
@@ -653,8 +675,6 @@ static const clivalue_t valueTable[] = {
     { "disarm_kill_switch",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ARMING_CONFIG, offsetof(armingConfig_t, disarm_kill_switch) },
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  60 }, PG_ARMING_CONFIG, offsetof(armingConfig_t, auto_disarm_delay) },
 
-
-
 };
 
 #else
@@ -669,6 +689,8 @@ typedef struct {
 const clivalue_t valueTable[] = {
     { "i2c_overclock",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.i2c_overclock, .config.lookup = { TABLE_OFF_ON } },
 
+    { "debug_mode",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
+
 #ifdef ASYNC_GYRO_PROCESSING
     { "acc_task_frequency",         VAR_UINT16 | MASTER_VALUE,  &masterConfig.accTaskFrequency, .config.minmax = { ACC_TASK_FREQUENCY_MIN,  ACC_TASK_FREQUENCY_MAX } },
     { "attitude_task_frequency",    VAR_UINT16 | MASTER_VALUE,  &masterConfig.attitudeTaskFrequency, .config.minmax = { ATTITUDE_TASK_FREQUENCY_MIN,  ATTITUDE_TASK_FREQUENCY_MAX } },

src/main/sensors/gyro.c

@@ -57,6 +57,8 @@
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
+#include "build/debug.h"
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -376,7 +378,13 @@ void gyroUpdate(void)
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         gyro.gyroADC[axis] -= gyroZero[axis];
         float gyroADCf = (float)gyro.gyroADC[axis];
+
+        DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
+
         gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
+
+        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
+
 #ifdef USE_GYRO_NOTCH_1
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
 #endif