From 7c318a3296aa2413a6e8feeb4b08f723f1e80907 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Wed, 7 Jan 2026 17:57:09 +0100 Subject: [PATCH] MulticopterPositionControl: prevent velocity integrator filling up from stale acceleration setpoints When position control is disabled, clear the setpoint properly to prevent stale values. This fixes a bug where switching to position mode in the same control loop as a hover thrust estimate update could fill up the velocity integrator. --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 1 + .../mc_pos_control/PositionControl/PositionControl.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 0501dadcc8..0d6b57c68b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -408,6 +408,7 @@ void MulticopterPositionControl::Run() } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) { // clear existing setpoint when controller is no longer active _setpoint = PositionControl::empty_trajectory_setpoint; + _control.setInputSetpoint(_setpoint); } } } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 72ae5a4a31..30e35f3235 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -84,8 +84,11 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new) const float previous_hover_thrust = _hover_thrust; setHoverThrust(hover_thrust_new); - _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust - + CONSTANTS_ONE_G - _acc_sp(2); + if (PX4_ISFINITE(_acc_sp(2))) { + _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust + + CONSTANTS_ONE_G - _acc_sp(2); + } + } void PositionControl::setState(const PositionControlStates &states)