diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c
index e69b094961..6036775eb8 100644
--- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c
+++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c
@@ -30,8 +30,6 @@
 
 #include "platform.h"
 
-#include "build/debug.h"
-
 #include "common/axis.h"
 #include "common/maths.h"
 
@@ -105,7 +103,6 @@ bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t
         if (in == data) {
             return true;
         } else {
-            debug[3]++;
             mpu9250SpiWriteRegister(dev, reg, data);
             delayMicroseconds(100);
         }
diff --git a/src/main/fc/core.c b/src/main/fc/core.c
index 18259a967a..03c8881990 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1295,10 +1295,8 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
     subTaskMotorUpdate(currentTimeUs);
     subTaskPidSubprocesses(currentTimeUs);
 
-    if (debugMode == DEBUG_CYCLETIME) {
-        DEBUG_SET(DEBUG_CYCLETIME, 0, getTaskDeltaTimeUs(TASK_SELF));
-        DEBUG_SET(DEBUG_CYCLETIME, 1, getAverageSystemLoadPercent());
-    }
+    DEBUG_SET(DEBUG_CYCLETIME, 0, getTaskDeltaTimeUs(TASK_SELF));
+    DEBUG_SET(DEBUG_CYCLETIME, 1, getAverageSystemLoadPercent());
 }
 
 bool isFlipOverAfterCrashActive(void)
diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c
index 45b010bb59..62357b325f 100644
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -520,12 +520,10 @@ static FAST_CODE void processRcSmoothingFilter(void)
             }
 
             // rx frame rate training blackbox debugging
-            if (debugMode == DEBUG_RC_SMOOTHING_RATE) {
-                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);              // log each rx frame interval
-                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
-                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
-                DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);                       // indicates whether guard time is active
-            }
+            DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 0, currentRxRefreshRate);              // log each rx frame interval
+            DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 1, rcSmoothingData.training.count);    // log the training step count
+            DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 2, rcSmoothingData.averageFrameTimeUs);// the current calculated average
+            DEBUG_SET(DEBUG_RC_SMOOTHING_RATE, 3, sampleState);                       // indicates whether guard time is active
         }
         // Get new values to be smoothed
         for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 54cc0b9cd3..6df8c8f97f 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -382,9 +382,7 @@ uint32_t baroUpdate(void)
     static barometerState_e state = BAROMETER_NEEDS_PRESSURE_START;
     timeUs_t sleepTime = 1000; // Wait 1ms between states
 
-    if (debugMode == DEBUG_BARO) {
-        debug[0] = state;
-    }
+    DEBUG_SET(DEBUG_BARO, 0, state);
 
     // Tell the scheduler to ignore how long this task takes unless the pressure is being read
     // as that takes the longest
@@ -447,11 +445,9 @@ uint32_t baroUpdate(void)
                 state = BAROMETER_NEEDS_TEMPERATURE_START;
             }
 
-            if (debugMode == DEBUG_BARO) {
-                debug[1] = baroTemperature;
-                debug[2] = baroPressure;
-                debug[3] = baroPressureSum;
-            }
+            DEBUG_SET(DEBUG_BARO, 1, baroTemperature);
+            DEBUG_SET(DEBUG_BARO, 2, baroPressure);
+            DEBUG_SET(DEBUG_BARO, 3, baroPressureSum);
 
             sleepTime = baro.dev.ut_delay;
             break;