From bc1f4594ed26b473349591325189ad05370febf1 Mon Sep 17 00:00:00 2001 From: Saibernard <112599512+Saibernard@users.noreply.github.com> Date: Sat, 29 Aug 2026 15:46:04 -0400 Subject: [PATCH] fix(mc_pos_control): bypass the velocity low pass when the cutoff is rejected (#28451) setCutoffFreq refuses a cutoff at or above half the sample rate and leaves the filter untouched, but all four velocity filter calls ignored the return. With the filters at their initial alpha of zero this froze the velocity feedback at zero, and the condition is reachable from a permitted parameter value, MPC_VEL_LP allows up to 50 Hz while the position loop commonly runs at 100 Hz or less. In SIH a hover with MPC_VEL_LP=50 oscillates half a metre in altitude with vertical speed peaks near 1 m/s, and holds 2.5 m within centimetres with this change. Follow the pattern VehicleAngularVelocity already uses, check the return and bypass the low pass stage when the requested cutoff is not achievable. One deliberate behaviour change comes with that, a runtime parameter change from a valid to an unachievable cutoff now bypasses the stage instead of keeping the stale previous configuration. Assisted-by: Claude:claude-fable-5 Signed-off-by: Saibernard Yogendran (cherry picked from commit 71f8b371834f8930e5f7c5e484345c893c256e39) --- .../MulticopterPositionControl.cpp | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 5c6f147ff68c..9801ab806164 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -95,24 +95,18 @@ void MulticopterPositionControl::parameters_update(bool force) _vel_z_notch_filter.disable(); } - // velocity xy/z low pass filter - if (_param_mpc_vel_lp.get() > 0.f) { - _vel_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); - _vel_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); - - } else { - // disable filtering + // velocity xy/z low pass filter, unfiltered when the cutoff is not achievable + if (!((_param_mpc_vel_lp.get() > 0.f) + && _vel_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()) + && _vel_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()))) { _vel_xy_lp_filter.setAlpha(1.f); _vel_z_lp_filter.setAlpha(1.f); } - // velocity derivative xy/z low pass filter - if (_param_mpc_veld_lp.get() > 0.f) { - _vel_deriv_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); - _vel_deriv_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); - - } else { - // disable filtering + // velocity derivative xy/z low pass filter, unfiltered when the cutoff is not achievable + if (!((_param_mpc_veld_lp.get() > 0.f) + && _vel_deriv_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()) + && _vel_deriv_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()))) { _vel_deriv_xy_lp_filter.setAlpha(1.f); _vel_deriv_z_lp_filter.setAlpha(1.f); }