From ef7fa53a9464436e4d87d86091cbd3b824f02300 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 2 Oct 2023 20:50:47 +0200 Subject: [PATCH 1/2] Add Rate Dynamics to MSP --- src/main/fc/fc_msp.c | 36 +++++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 2 files changed, 38 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8b62fdba39..192a2af314 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1582,6 +1582,21 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif +#ifdef USE_RATE_DYNAMICS + + case MSP2_INAV_RATE_DYNAMICS: + { + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd); + } + break; + +#endif + default: return false; } @@ -3040,6 +3055,27 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif +#ifdef USE_RATE_DYNAMICS + + case MSP2_INAV_SET_RATE_DYNAMICS: + + if (dataSize == 8) { + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src); + ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src); + + } else { + return MSP_RESULT_ERROR; + } + + break; + +#endif + + default: return MSP_RESULT_ERROR; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index f746f5c8ff..9cf1881c73 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -92,3 +92,5 @@ #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 +#define MSP2_INAV_RATE_DYNAMICS 0x2060 +#define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 From 5c6b8a67d64cfa806ca3f1e3ae789adf47f0c3dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 3 Oct 2023 15:52:27 +0200 Subject: [PATCH 2/2] review changes --- src/main/fc/fc_msp.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 192a2af314..2b2d4a8437 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1586,10 +1586,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_RATE_DYNAMICS: { - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter); + sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd); sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter); sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd); }