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.

Author: mahima-yoga

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);
 				}
 			}
 		}

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)