diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
index ae6756aa8b..e46e2990c1 100644
--- a/msg/position_setpoint.msg
+++ b/msg/position_setpoint.msg
@@ -33,3 +33,4 @@ float32 a_y			# acceleration y setpoint
 float32 a_z			# acceleration z setpoint
 bool acceleration_valid		# true if acceleration setpoint is valid/should be used
 bool acceleration_is_force	# interprete acceleration as force
+float32 acceptance_radius   # navigation acceptance_radius if we're doint waypoint navigation
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d6ec292f6d..3dd53bedaf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1059,6 +1059,16 @@ void MulticopterPositionControl::control_auto(float dt)
 			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
 		}
 
+		/* if we're near the current pos SP don't reset it anymore, else it will make if jump back,
+		 * especially noticable in altitude after takeoff.
+		 */
+		if (current_setpoint_valid
+				&& _pos_sp_triplet.current.acceptance_radius > 0.0f
+				&& (curr_sp - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius) {
+			_reset_pos_sp = false;
+			_reset_alt_sp = false;
+		}
+
 	} else {
 		/* no waypoint, do nothing, setpoint was already reset */
 	}
@@ -1439,6 +1449,7 @@ MulticopterPositionControl::task_main()
 						if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) {
 							_landing_thrust = thrust_sp(2);
 						}
+						// XXX: we probably need to add a margin here becaue we're limiting the complete thrust vector further down
 						thr_max = -_landing_thrust;
 
 					} else {
diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp
index 25da635622..b8182e2fed 100644
--- a/src/modules/navigator/land.cpp
+++ b/src/modules/navigator/land.cpp
@@ -79,7 +79,6 @@ Land::on_activation()
     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
     pos_sp_triplet->previous.valid = false;
     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-    pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
     pos_sp_triplet->next.valid = false;
 
     _navigator->set_can_loiter_at_sp(false);
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 1819337a4c..897173d0d9 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -222,6 +222,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 				_navigator->get_loiter_radius();
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
+	sp->acceptance_radius = item->acceptance_radius;
 
 	switch (item->nav_cmd) {
 	case NAV_CMD_DO_SET_SERVO:
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index 4b5dc49776..9851212320 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -84,12 +84,11 @@ Takeoff::on_activation()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->previous.valid = false;
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 	pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw;
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;
 
-	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
+	_navigator->set_can_loiter_at_sp(true);
 
 	_navigator->set_position_setpoint_triplet_updated();
 }