From 4312513a77c27e8bd4eddf9b07bdd5d9b7108b70 Mon Sep 17 00:00:00 2001
From: supiiik <mjenis@atlas.sk>
Date: Wed, 23 May 2018 13:33:43 +0200
Subject: [PATCH] MSP changed

Changed MSP, all new data are at the end
---
 src/main/cms/cms_menu_imu.c |  2 +-
 src/main/flight/pid.c       |  2 +-
 src/main/interface/msp.c    | 12 ++++++++----
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 0fce8bb347..aacc0f91b1 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -285,7 +285,7 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
 static OSD_Entry cmsx_menuProfileOtherEntries[] = {
     { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
 
-    { "D SETPT WT",  OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_dtermSetpointWeight,    0,    2000,   1, 10 }, 0 },
+	{ "D SETPT WT",  OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_dtermSetpointWeight,    0,    2000,  1, 10 }, 0 },
     { "SETPT TRS",   OME_FLOAT,  NULL, &(OSD_FLOAT_t)  { &cmsx_setpointRelaxRatio,     0,    100,   1, 10 }, 0 },
     { "ANGLE STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_angleStrength,          0,    200,   1  }   , 0 },
     { "HORZN STR",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonStrength,        0,    200,   1  }   , 0 },
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 10859b5dcc..3f057b6cff 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -88,7 +88,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 );
 #endif
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index f9a7fc2bed..ec4877653e 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1224,8 +1224,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, 0); // reserved
         sbufWriteU8(dst, currentPidProfile->vbatPidCompensation);
         sbufWriteU8(dst, currentPidProfile->setpointRelaxRatio);
-        sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
-//        sbufWriteU8(dst, 0); // reserved
+        sbufWriteU8(dst, currentPidProfile->dtermSetpointWeight);
+        sbufWriteU8(dst, 0); // reserved
         sbufWriteU8(dst, 0); // reserved
         sbufWriteU8(dst, 0); // reserved
         sbufWriteU16(dst, currentPidProfile->rateAccelLimit);
@@ -1234,6 +1234,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, 0); // was pidProfile.levelSensitivity
         sbufWriteU16(dst, currentPidProfile->itermThrottleThreshold);
         sbufWriteU16(dst, currentPidProfile->itermAcceleratorGain);
+		sbufWriteU16(dst, currentPidProfile->dtermSetpointWeight);
         break;
 
     case MSP_SENSOR_CONFIG:
@@ -1680,8 +1681,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         sbufReadU8(src); // reserved
         currentPidProfile->vbatPidCompensation = sbufReadU8(src);
         currentPidProfile->setpointRelaxRatio = sbufReadU8(src);
-        currentPidProfile->dtermSetpointWeight = sbufReadU16(src);  // not sure, if this is OK. Low bit is written 1st, so it should be compatible
-//        sbufReadU8(src); // reserved
+        currentPidProfile->dtermSetpointWeight = sbufReadU8(src);
+        sbufReadU8(src); // reserved
         sbufReadU8(src); // reserved
         sbufReadU8(src); // reserved
         currentPidProfile->rateAccelLimit = sbufReadU16(src);
@@ -1694,6 +1695,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
             currentPidProfile->itermThrottleThreshold = sbufReadU16(src);
             currentPidProfile->itermAcceleratorGain = sbufReadU16(src);
         }
+		if (sbufBytesRemaining(src) >= 2) {
+			currentPidProfile->dtermSetpointWeight = sbufReadU16(src);
+		}
         pidInitConfig(currentPidProfile);
         break;