diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index d46d94b5aa..17577421a6 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -56,10 +56,6 @@ void PositionControl::updateState(const PositionControlStates &states)
 
 bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
-	// If full manual is required (thrust already generated), don't run position/velocity
-	// controller and just return thrust.
-	_skip_controller = false;
-
 	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
 	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
 	_acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
@@ -68,9 +64,10 @@ bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
 	_yawspeed_sp = setpoint.yawspeed;
 	bool mapping_succeeded = _interfaceMapping();
 
-	if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) {
-		_skip_controller = true;
-	}
+	// If full manual is required (thrust already generated), don't run position/velocity
+	// controller and just return thrust.
+	_skip_controller = PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1])
+			   && PX4_ISFINITE(setpoint.thrust[2]);
 
 	return mapping_succeeded;
 }
@@ -203,8 +200,6 @@ bool PositionControl::_interfaceMapping()
 
 	// check failsafe
 	if (failsafe) {
-		_skip_controller = true;
-
 		// point the thrust upwards
 		_thr_sp(0) = _thr_sp(1) = 0.0f;
 		// throttle down such that vehicle goes down with