From a8cbd4d271c2e136a52b40bde35fbc4e64163f9c Mon Sep 17 00:00:00 2001
From: Hans Christian Olaussen <41271048+klutvott123@users.noreply.github.com>
Date: Sun, 11 Oct 2020 21:23:08 +0200
Subject: [PATCH] Add Thrust Linearization to MSP

Adds Thrust Linearization to MSP.
---
 src/main/msp/msp.c | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index c9f954cd0..343b8095c 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1862,6 +1862,11 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
 #else
         sbufWriteU8(dst, 0);
 #endif
+#if defined(USE_THRUST_LINEARIZATION)
+        sbufWriteU8(dst, currentPidProfile->thrustLinearization);
+#else
+        sbufWriteU8(dst, 0);
+#endif
         break;
     case MSP_SENSOR_CONFIG:
 #if defined(USE_ACC)
@@ -2714,7 +2719,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             sbufReadU8(src);
 #endif
         }
-        if (sbufBytesRemaining(src) >= 4) {
+        if (sbufBytesRemaining(src) >= 5) {
             // Added in MSP API 1.44
 #if defined(USE_INTERPOLATED_SP)
             currentPidProfile->ff_interpolate_sp = sbufReadU8(src);
@@ -2729,6 +2734,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
 #else
             sbufReadU8(src);
 #endif
+#if defined(USE_THRUST_LINEARIZATION)
+            currentPidProfile->thrustLinearization = sbufReadU8(src);
+#else
+            sbufReadU8(src);
+#endif
         }
         pidInitConfig(currentPidProfile);
 
-- 
2.11.4.GIT