diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 7a2e823f82..38da4302c4 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -51,5 +51,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "FFT_TIME",
     "FFT_FREQ",
     "FRSKY_D_RX",
-    "GYRO_RAW"
+    "GYRO_RAW",
+    "MAX7456_SIGNAL",
+    "MAX7456_SPICLOCK"
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 925788aba1..8b02d75b18 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -70,6 +70,8 @@ typedef enum {
     DEBUG_FFT_FREQ,
     DEBUG_FRSKY_D_RX,
     DEBUG_GYRO_RAW,
+    DEBUG_MAX7456_SIGNAL,
+    DEBUG_MAX7456_SPICLOCK,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 2e43433404..be71343a86 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -43,6 +43,17 @@
 
 #include "fc/config.h" // For systemConfig()
 
+// DEBUG_MAX7456_SIGNAL
+#define DEBUG_MAX7456_SIGNAL_MODEREG       0
+#define DEBUG_MAX7456_SIGNAL_SENSE         1
+#define DEBUG_MAX7456_SIGNAL_REINIT        2
+#define DEBUG_MAX7456_SIGNAL_ROWS          3
+
+// DEBUG_MAX7456_SPICLOCK
+#define DEBUG_MAX7456_SPICLOCK_OVERCLOCK   0
+#define DEBUG_MAX7456_SPICLOCK_DEVTYPE     1
+#define DEBUG_MAX7456_SPICLOCK_DIVISOR     2
+
 // VM0 bits
 #define VIDEO_BUFFER_DISABLE        0x01
 #define MAX7456_RESET               0x02
@@ -455,11 +466,11 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
         break;
     }
 
-#ifdef DEBUG_MAX7456_SPI_CLOCK
-    debug[0] = systemConfig()->cpu_overclock;
-    debug[1] = max7456DeviceType;
-    debug[2] = max7456SpiClock;
-#endif
+    if (debugMode == DEBUG_MAX7456_SPICLOCK) {
+        DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, systemConfig()->cpu_overclock);
+        DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DEVTYPE, max7456DeviceType);
+        DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClock);
+    }
 #endif
 
     spiSetDivisor(MAX7456_SPI_INSTANCE, max7456SpiClock);
@@ -558,6 +569,8 @@ void max7456DrawScreen(void)
     static uint16_t pos = 0;
     int k = 0, buff_len=0;
 
+    static uint16_t reInitCount = 0;
+
     if (!max7456Lock && !fontIsLoading) {
 
         // (Re)Initialize MAX7456 at startup or stall is detected.
@@ -581,11 +594,11 @@ void max7456DrawScreen(void)
             videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
             DISABLE_MAX7456;
 
-#ifdef DEBUG_MAX7456_SIGNAL
-            debug[0] = videoSignalReg & VIDEO_MODE_MASK;
-            debug[1] = videoSense & 0x7;
-            debug[3] = max7456GetRowsCount();
-#endif
+            if (debugMode == DEBUG_MAX7456_SIGNAL) {
+                DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_MODEREG, videoSignalReg & VIDEO_MODE_MASK);
+                DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_SENSE, videoSense & 0x7);
+                DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_ROWS, max7456GetRowsCount());
+            }
 
             if (videoSense & STAT_LOS) {
                 videoDetectTimeMs = 0;
@@ -595,9 +608,9 @@ void max7456DrawScreen(void)
                     if (videoDetectTimeMs) {
                         if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
                             max7456ReInit();
-#ifdef DEBUG_MAX7456_SIGNAL
-                            debug[2]++;
-#endif
+                            if (debugMode == DEBUG_MAX7456_SIGNAL) {
+                                DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_REINIT, ++reInitCount);
+                            }
                         }
                     } else {
                         // Wait for signal to stabilize