diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 4d3787b473..ff67df4b61 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -113,7 +113,6 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
 void PositionControl::update(const float dt)
 {
 	if (_skip_controller) {
-
 		// Already received a valid thrust set-point.
 		// Limit the thrust vector.
 		float thr_mag = _thr_sp.length();
@@ -129,11 +128,11 @@ void PositionControl::update(const float dt)
 		_pos_sp = _pos;
 		_vel_sp = _vel;
 		_acc_sp = _vel_dot;
-
-	} else {
-		_positionControl();
-		_velocityControl(dt);
+		return;
 	}
+
+	_positionControl();
+	_velocityControl(dt);
 }
 
 bool PositionControl::_interfaceMapping()
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index d6c6f76dc1..07cf0b03f6 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -42,7 +42,6 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
-#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
 struct PositionControlStates {