Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

MulticopterPositionControl: prevent velocity integrator filling up from stale acceleration setpoints

Browse Source 
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.
This commit is contained in:
mahima-yoga
2026-01-07 17:57:09 +01:00
committed by Matthias Grob
parent 0375f1a6f0
commit 7c318a3296
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+1
View File
@@ -408,6 +408,7 @@ void MulticopterPositionControl::Run()
} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) { } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
// clear existing setpoint when controller is no longer active // clear existing setpoint when controller is no longer active
_setpoint = PositionControl::empty_trajectory_setpoint; _setpoint = PositionControl::empty_trajectory_setpoint;
_control.setInputSetpoint(_setpoint);
} }
} }
} }
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+5 -2
View File
@@ -84,8 +84,11 @@ void PositionControl::updateHoverThrust(const float hover_thrust_new)
const float previous_hover_thrust = _hover_thrust; const float previous_hover_thrust = _hover_thrust;
setHoverThrust(hover_thrust_new); setHoverThrust(hover_thrust_new);
_vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * previous_hover_thrust / _hover_thrust if (PX4_ISFINITE(_acc_sp(2))) {
+ CONSTANTS_ONE_G - _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) void PositionControl::setState(const PositionControlStates &states)