From 9c8fe796bab8aa32c7fad3fe28e3483fe5466af1 Mon Sep 17 00:00:00 2001 From: Miguel Angel Mulero Martinez Date: Tue, 5 Mar 2019 09:30:48 +0100 Subject: [PATCH] Add dynamic lpf and dterm filters to MSP --- src/main/msp/msp.c | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 46a4147aff..68492aea04 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1401,6 +1401,17 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type); sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type); sbufWriteU16(dst, currentPidProfile->dterm_lowpass2_hz); +#if defined(USE_DYN_LPF) + sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_min_hz); + sbufWriteU16(dst, gyroConfig()->dyn_lpf_gyro_max_hz); + sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_min_hz); + sbufWriteU16(dst, currentPidProfile->dyn_lpf_dterm_max_hz); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif break; case MSP_PID_ADVANCED: @@ -2025,6 +2036,20 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src); currentPidProfile->dterm_lowpass2_hz = sbufReadU16(src); } + if (sbufBytesRemaining(src) >= 8) { +#if defined(USE_DYN_LPF) + gyroConfigMutable()->dyn_lpf_gyro_min_hz = sbufReadU16(src); + gyroConfigMutable()->dyn_lpf_gyro_max_hz = sbufReadU16(src); + currentPidProfile->dyn_lpf_dterm_min_hz = sbufReadU16(src); + currentPidProfile->dyn_lpf_dterm_max_hz = sbufReadU16(src); +#else + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif + } + // reinitialize the gyro filters with the new values validateAndFixGyroConfig(); gyroInitFilters();