diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8b62fdba39..2b2d4a8437 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