From 0dd42ca9b1e30fa0ed2411203dc94b2d43ac6b58 Mon Sep 17 00:00:00 2001
From: IvoFPV <ogzerr1@gmail.com>
Date: Thu, 8 Aug 2019 09:53:59 +0200
Subject: [PATCH] Add debug mode and debug count to msp

---
 src/main/msp/msp.c | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 0d3b9af2e6..dd09d4d736 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1420,6 +1420,9 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
         sbufWriteU16(dst, gyroConfig()->gyro_offset_yaw);
         sbufWriteU8(dst, gyroConfig()->checkOverflow);
+        //Added in MSP API 1.42
+        sbufWriteU8(dst, systemConfig()->debug_mode);
+        sbufWriteU8(dst, DEBUG_COUNT);
 
         break;
     case MSP_FILTER_CONFIG :
@@ -2075,6 +2078,10 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             gyroConfigMutable()->gyro_offset_yaw = sbufReadU16(src);
             gyroConfigMutable()->checkOverflow = sbufReadU8(src);
         }
+        if (sbufBytesRemaining(src) >= 1) {
+            //Added in MSP API 1.42
+            systemConfigMutable()->debug_mode = sbufReadU8(src);
+        }
 
         validateAndFixGyroConfig();