From fd1f1c81efb384e46a5767555a58061082612c9f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 17:24:04 +0200
Subject: [PATCH] navigator: added parameter for acceptance radius for take-off
 mission items

---
 src/modules/navigator/mission_block.cpp  | 14 ++++++++++----
 src/modules/navigator/navigator.h        |  2 ++
 src/modules/navigator/navigator_main.cpp |  3 ++-
 src/modules/navigator/navigator_params.c | 11 +++++++++++
 4 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 861aed8139..08576750c9 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached()
 							  _navigator_priv->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-
-			/* require only altitude for takeoff */
-			if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+			/* require only altitude for takeoff for multicopter */
+			if (_navigator_priv->get_global_position()->alt >
+				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} 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_priv->get_takeoff_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
+			/* for normal mission items used their acceptance radius */
 			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 296294a918..fe7485f566 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -109,6 +109,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -156,6 +157,7 @@ private:
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
+	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index da06f826a6..dc85738134 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -122,7 +122,8 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
 	_update_triplet(false),
-	_param_loiter_radius(this, "LOITER_RAD")
+	_param_loiter_radius(this, "LOITER_RAD"),
+	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
 {
 	updateParams();
 }
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 9235f3046b..ca31adecc6 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -53,3 +53,14 @@
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
+
+/**
+ * Takeoff Acceptance Radius (FW only)
+ *
+ * Acceptance radius for fixedwing.
+ *
+ * @unit meters
+ * @min 1.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);