diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index a287defcf2..2a8f5fa9d0 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -79,6 +79,16 @@ void PositionControl::updateState(const vehicle_local_position_s &state, const V
 	_vel_dot = vel_dot;
 }
 
+void PositionControl::setIdle()
+{
+	_pos_sp = _pos;
+	_vel_sp.zero();
+	_acc_sp.zero();
+	_thr_sp.zero();
+	_yaw_sp = _yaw_sp_int = _yaw;
+	_yawspeed_sp = 0.0f;
+}
+
 void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
 {
 	_pos_sp = Vector3f(&setpoint.x);
@@ -96,6 +106,9 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
 
 	if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) {
 		_skipController = true;
+		_pos_sp.zero();
+		_vel_sp.zero();
+		_acc_sp.zero();
 	}
 }
 
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp
index d262106bfc..5810263c70 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -137,6 +137,4 @@ private:
 	void _velocityController(const float &dt);
 	void _updateParams();
 	void _setParams();
-	void _setTakeoffVelocity(const float dt);
-	void _setTakeoffThrust(const float dt);
 };