Add selectable debug options

src/main/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/debug.h

@@ -17,6 +17,7 @@
 
 #define DEBUG16_VALUE_COUNT 4
 extern int16_t debug[DEBUG16_VALUE_COUNT];
+extern uint8_t debugMode;
 
 #define DEBUG_SECTION_TIMES
 
@@ -39,3 +40,12 @@ extern uint32_t sectionTimes[2][4];
 #define TIME_SECTION_END(index) {}
 
 #endif
+
+typedef enum {
+    DEBUG_CYCLETIME = 1,
+    DEBUG_BATTERY,
+    DEBUG_GYRO,
+    DEBUG_ACCELEROMETER,
+    DEBUG_MIXER,
+    DEBUG_AIRMODE
+} debugType_e;

src/main/flight/mixer.c

@@ -811,6 +811,8 @@ void mixTable(void)
 
         if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i];
         if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i];
+
+        if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i];
     }
 
     // Scale roll/pitch/yaw uniformly to fit within throttle range
@@ -850,7 +852,7 @@ void mixTable(void)
         motorLimitReached = true;
         q_number_t mixReduction;
         qConstruct(&mixReduction, throttleRange, rollPitchYawMixRange, Q12_NUMBER);
-        //float mixReduction = (float) throttleRange / rollPitchYawMixRange;
+
         for (i = 0; i < motorCount; i++) {
             rollPitchYawMix[i] =  qMultiply(mixReduction,rollPitchYawMix[i]);
         }
@@ -858,15 +860,21 @@ void mixTable(void)
         if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
             throttleMin = throttleMax = throttleMin + (throttleRange / 2);
         }
+
+        if (debugMode == DEBUG_AIRMODE) debug[0] = rollPitchYawMixRange;
+
     } else {
         motorLimitReached = false;
         throttleMin = throttleMin + (rollPitchYawMixRange / 2);
         throttleMax = throttleMax - (rollPitchYawMixRange / 2);
+        if (debugMode == DEBUG_AIRMODE) debug[0] = 0;
     }
 
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (i = 0; i < motorCount; i++) {
+        if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMix[i];
+
         motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
 
         if (isFailsafeActive) {

src/main/io/serial_cli.c

@@ -416,6 +416,16 @@ static const char * const lookupDeltaMethod[] = {
     "ERROR", "MEASUREMENT"
 };
 
+static const char * const lookupTableDebug[] = {
+    "NONE",
+    "CYCLETIME",
+    "BATTERY",
+    "GYRO",
+    "ACCELEROMETER",
+    "MIXER",
+    "AIRMODE"
+};
+
 typedef struct lookupTableEntry_s {
     const char * const *values;
     const uint8_t valueCount;
@@ -441,6 +451,7 @@ typedef enum {
     TABLE_BARO_HARDWARE,
     TABLE_MAG_HARDWARE,
     TABLE_DELTA_METHOD,
+	TABLE_DEBUG,
 } lookupTableIndex_e;
 
 static const lookupTableEntry_t lookupTables[] = {
@@ -462,7 +473,8 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
     { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
     { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
-    { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) }
+    { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) },
+    { lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) }
 };
 
 #define VALUE_TYPE_OFFSET 0
@@ -526,7 +538,7 @@ const clivalue_t valueTable[] = {
     { "rc_smoothing",               VAR_INT8   | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON } },
     { "roll_yaw_cam_mix_degrees",   VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.fpvCamAngleDegrees, .config.minmax = { 0,  50 } },
     { "max_aux_channels",           VAR_UINT8  | MASTER_VALUE,  &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0,  13 } },
-    { "debug_mode",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.debug_mode, .config.lookup = { TABLE_OFF_ON } },
+    { "debug_mode",                 VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
 
     { "min_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },
     { "max_throttle",               VAR_UINT16 | MASTER_VALUE,  &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO,  PWM_RANGE_MAX } },

src/main/main.c

@@ -182,6 +182,8 @@ void init(void)
 
     systemInit();
 
+    debugMode = masterConfig.debug_mode;
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
     detectHardwareRevision();
 #endif

src/main/mw.c

@@ -94,8 +94,6 @@ enum {
     ALIGN_MAG = 2
 };
 
-//#define JITTER_DEBUG 0  // Specify debug value for jitter debug
-
 /* VBAT monitoring interval (in microseconds) - 1s*/
 #define VBATINTERVAL (6 * 3500)
 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
@@ -657,7 +655,7 @@ void taskMainPidLoop(void)
     writeServos();
 #endif
 
-    if (masterConfig.debug_mode) {
+    if (debugMode == DEBUG_CYCLETIME) {
         static uint32_t previousMotorUpdateTime, motorCycleTime;
 
         motorCycleTime = micros() - previousMotorUpdateTime;
@@ -757,7 +755,7 @@ void taskMainPidLoopCheck(void) {
     cycleTime = micros() - previousTime;
     previousTime = micros();
 
-    if (masterConfig.debug_mode) {
+    if (debugMode == DEBUG_CYCLETIME) {
         // Debugging parameters
         debug[0] = cycleTime;
         debug[1] = averageSystemLoadPercent;

src/main/sensors/acceleration.c

@@ -20,6 +20,7 @@
 #include <math.h>
 
 #include "platform.h"
+#include "debug.h"
 
 #include "common/axis.h"
 #include "common/filter.h"
@@ -187,7 +188,10 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
         return;
     }
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = accADCRaw[axis];
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
+        accSmooth[axis] = accADCRaw[axis];
+    }
 
     if (accLpfCutHz) {
         if (!accFilterInitialised) {

src/main/sensors/battery.c

@@ -69,12 +69,17 @@ static void updateBatteryVoltage(void)
 
     // store the battery voltage with some other recent battery voltage readings
     vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
+
+    if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
+
     if (!vbatFilterStateIsSet) {
         BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update
         vbatFilterStateIsSet = true;
     }
     vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState);
     vbat = batteryAdcToVoltage(vbatSample);
+
+    if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
 }
 
 #define VBATTERY_STABLE_DELAY 40

src/main/sensors/gyro.c

@@ -140,7 +140,10 @@ void gyroUpdate(void)
         return;
     }
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
+        gyroADC[axis] = gyroADCRaw[axis];
+    }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);