From 7f89994785716dfe69905fc424491ba867e2ea3d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 4 Jul 2016 15:41:16 -0400
Subject: [PATCH] use NAV_ACC_RAD for vertical waypoint acceptance (#4978)

---
 ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 +++-
 src/modules/navigator/mission_block.cpp   | 9 ++++++---
 src/modules/navigator/navigator_params.c  | 1 +
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index f876353263..1ae8f81227 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -11,7 +11,9 @@ then
 	param set RTL_DESCEND_ALT 100
 	param set RTL_LAND_DELAY -1
 
-	param set NAV_ACC_RAD 50
+	# FW uses L1 distance for acceptance radius 
+	#  set a smaller NAV_ACC_RAD for vertical acceptance distance
+	param set NAV_ACC_RAD 10
 
 	param set PE_VELNE_NOISE 0.3
 	param set PE_VELD_NOISE 0.35
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index f00e05d1fe..cd838e3fa6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -170,7 +170,8 @@ MissionBlock::is_mission_item_reached()
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (!_navigator->get_vstatus()->is_rotary_wing &&
@@ -183,7 +184,8 @@ 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 <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -195,7 +197,8 @@ MissionBlock::is_mission_item_reached()
 				mission_acceptance_radius = _navigator->get_acceptance_radius();
 			}
 
-			if (dist >= 0.0f && dist <= mission_acceptance_radius) {
+			if (dist >= 0.0f && dist <= mission_acceptance_radius
+				&& dist_z <= _navigator->get_default_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		}
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 63701b4416..f2eaf9ba5c 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -58,6 +58,7 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Acceptance Radius
  *
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
+ * For fixed wing NAV_ACC_RAD is the vertical acceptance, as the L1 turning distance is used for horizontal acceptance.
  *
  * @unit m
  * @min 0.05