diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 46a4147aff..6009b801ac 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1454,7 +1454,19 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU16(dst, currentPidProfile->pid[PID_YAW].F); sbufWriteU8(dst, currentPidProfile->antiGravityMode); - +#if defined(USE_D_MIN) + sbufWriteU8(dst, currentPidProfile->d_min[PID_ROLL]); + sbufWriteU8(dst, currentPidProfile->d_min[PID_PITCH]); + sbufWriteU8(dst, currentPidProfile->d_min[PID_YAW]); + sbufWriteU8(dst, currentPidProfile->d_min_gain); + sbufWriteU8(dst, currentPidProfile->d_min_advance); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif break; case MSP_SENSOR_CONFIG: #if defined(USE_ACC) @@ -2093,6 +2105,21 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) currentPidProfile->antiGravityMode = sbufReadU8(src); } + if (sbufBytesRemaining(src) >= 5) { +#if defined(USE_D_MIN) + currentPidProfile->d_min[PID_ROLL] = sbufReadU8(src); + currentPidProfile->d_min[PID_PITCH] = sbufReadU8(src); + currentPidProfile->d_min[PID_YAW] = sbufReadU8(src); + currentPidProfile->d_min_gain = sbufReadU8(src); + currentPidProfile->d_min_advance = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif + } pidInitConfig(currentPidProfile); break;