From 2ff2ceec28532d5594293584ab101086e5060de9 Mon Sep 17 00:00:00 2001
From: IvoFPV <ogzerr1@gmail.com>
Date: Thu, 8 Aug 2019 13:47:26 +0200
Subject: [PATCH] Add dyn notch to MSP

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

diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 0d3b9af2e6..5bd45ccf41 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1453,7 +1453,18 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU16(dst, 0);
         sbufWriteU16(dst, 0);
 #endif
-
+#if defined(USE_GYRO_DATA_ANALYSE)
+        // Added in MSP API 1.42
+        sbufWriteU8(dst, gyroConfig()->dyn_notch_range);
+        sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
+        sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
+        sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, 0);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+#endif
         break;
     case MSP_PID_ADVANCED:
         sbufWriteU16(dst, 0);
@@ -2120,6 +2131,20 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             sbufReadU16(src);
 #endif
         }
+        if (sbufBytesRemaining(src) >= 6) {
+            // Added in MSP API 1.42
+#if defined(USE_GYRO_DATA_ANALYSE)
+            gyroConfigMutable()->dyn_notch_range = sbufReadU8(src);
+            gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
+            gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
+            gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
+#else
+            sbufReadU8(src);
+            sbufReadU8(src);
+            sbufReadU16(src);
+            sbufReadU16(src);
+#endif
+        }
 
         // reinitialize the gyro filters with the new values
         validateAndFixGyroConfig();