From 05e5417f10f89b7e8e1275c0a61a10e4c79eb5b1 Mon Sep 17 00:00:00 2001
From: Miguel Angel Mulero Martinez <migmul@gmail.com>
Date: Wed, 6 Mar 2019 10:07:29 +0100
Subject: [PATCH] Add integrated yaw to MSP

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

diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 8246074b02..92611d6ad0 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1484,6 +1484,14 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, 0);
         sbufWriteU8(dst, 0);
 #endif
+#if defined(USE_INTEGRATED_YAW_CONTROL)
+        sbufWriteU8(dst, currentPidProfile->use_integrated_yaw);
+        sbufWriteU8(dst, currentPidProfile->integrated_yaw_relax);
+#else
+        sbufWriteU8(dst, 0);
+        sbufWriteU8(dst, 0);
+#endif
+
         break;
     case MSP_SENSOR_CONFIG:
 #if defined(USE_ACC)
@@ -2149,6 +2157,15 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             sbufReadU8(src);
             sbufReadU8(src);
             sbufReadU8(src);
+#endif
+        }
+        if (sbufBytesRemaining(src) >= 2) {
+#if defined(USE_INTEGRATED_YAW_CONTROL)
+            currentPidProfile->use_integrated_yaw = sbufReadU8(src);
+            currentPidProfile->integrated_yaw_relax = sbufReadU8(src);
+#else
+            sbufReadU8(src);
+            sbufReadU8(src);
 #endif
         }
         pidInitConfig(currentPidProfile);