From b2a694f71dcc246edd32c7b8befa0b3dd6f4879d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 5 Jun 2015 10:34:17 +0200
Subject: [PATCH] navigator: Use the controller radius also as lower bound for
 mission items

---
 src/modules/navigator/mission_block.cpp  |  4 ++--
 src/modules/navigator/navigator.h        | 15 +++++++++++++++
 src/modules/navigator/navigator_main.cpp |  8 +++++++-
 3 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index dce7d45171..42c74428ad 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
-			if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
 				_waypoint_position_reached = true;
 			}
 		} else {
 			/* for normal mission items used their acceptance radius */
-			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
 				_waypoint_position_reached = true;
 			}
 		}
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 83f3487a00..95e3f9f964 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -144,7 +144,22 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+
+	/**
+	 * Get the acceptance radius
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
 	float		get_acceptance_radius();
+
+	/**
+	 * Get the acceptance radius given the mission item preset radius
+	 *
+	 * @param mission_item_radius the radius to use in case the controller-derived radius is smaller
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
+	float		get_acceptance_radius(float mission_item_radius);
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 4bd7c08be0..e05d079a0a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -574,7 +574,13 @@ Navigator::publish_position_setpoint_triplet()
 float
 Navigator::get_acceptance_radius()
 {
-	float radius = _param_acceptance_radius.get();
+	return get_acceptance_radius(_param_acceptance_radius.get());
+}
+
+float
+Navigator::get_acceptance_radius(float mission_item_radius)
+{
+	float radius = mission_item_radius;
 
 	if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) {
 		if (_nav_caps.turn_distance > radius) {