From d78c3a224267f4dbd1fac72e893c81b83b43df9b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 17:17:41 +0200
Subject: navigator: new class structure, loiter and mission working

---
 src/modules/navigator/mission_block.cpp | 215 ++++++++++++++++++++++++++++++++
 1 file changed, 215 insertions(+)
 create mode 100644 src/modules/navigator/mission_block.cpp

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
new file mode 100644
index 000000000..92090d995
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mission_block.cpp
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+
+#include "navigator.h"
+#include "mission_block.h"
+
+
+MissionBlock::MissionBlock(Navigator *navigator) :
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0),
+	_mission_item({0}),
+	_mission_item_valid(false),
+	_navigator_priv(navigator)
+{
+}
+
+MissionBlock::~MissionBlock()
+{
+}
+
+bool
+MissionBlock::is_mission_item_reached()
+{
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator_priv->get_global_position()->lat,
+							  _navigator_priv->get_global_position()->lon,
+							  _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) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+MissionBlock::reset_mission_item_reached()
+{
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
+}
+
+void
+MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
+{
+	sp->valid = true;
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
+	}
+}
+
+bool
+MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	if (_navigator_priv->get_is_in_loiter()) {
+		/* already loitering, bail out */
+		return false;
+	}
+
+	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
+		/* leave position setpoint as is */
+	} else {
+		/* use current position */
+		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+	}
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+	pos_sp_triplet->current.loiter_direction = 1;
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+
+	_navigator_priv->set_is_in_loiter(true);
+	return true;
+}
+
-- 
cgit v1.2.3


From d3f182d433551c72a2fd8105481938129b8332ed Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 9 Jun 2014 11:37:33 +0200
Subject: navigator: don't check reached for land waypoints

---
 src/modules/navigator/mission_block.cpp | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 92090d995..861aed813 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,6 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
+	/* don't check landed WPs */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return false;
+	}
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-- 
cgit v1.2.3


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: 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(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 861aed813..08576750c 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 296294a91..fe7485f56 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 da06f826a..dc8573813 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 9235f3046..ca31adecc 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);
-- 
cgit v1.2.3


From 9d3d5a30af0781b975d17c5d73796f593c78b6e4 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:13:47 +0200
Subject: navigator: move set_previous_pos_sp to MissionBlock class

---
 src/modules/navigator/mission.cpp       | 12 +-----------
 src/modules/navigator/mission.h         |  6 ------
 src/modules/navigator/mission_block.cpp |  9 +++++++++
 src/modules/navigator/mission_block.h   |  5 +++++
 4 files changed, 15 insertions(+), 17 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b1..becaa09e8 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -223,7 +223,7 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 }
 
-void
-Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-		                   struct position_setpoint_s *previous_pos_sp)
-{
-	/* reuse current setpoint as previous setpoint */
-	if (current_pos_sp->valid) {
-		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
-	}
-}
-
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96a..842c8c9ee 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -106,12 +106,6 @@ private:
 	 */
 	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	/**
-	 * Set previous position setpoint
-	 */
-	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-			               struct position_setpoint_s *previous_pos_sp);
-
 	/**
 	 * Try to set the current position setpoint from an onboard mission item
 	 * @return true if mission item successfully set
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 08576750c..a8eec92ca 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	}
 }
 
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
 bool
 MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 47e6fe81f..a740deea4 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -82,6 +82,11 @@ public:
 	 */
 	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *
-- 
cgit v1.2.3


From c5a5604ae975294347f1571258a633c11ef61261 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 00:17:25 +0200
Subject: navigator: loiter fixes

---
 src/modules/navigator/loiter.cpp         |  3 +--
 src/modules/navigator/mission.cpp        |  8 +++----
 src/modules/navigator/mission_block.cpp  | 40 ++++++++++++++++++--------------
 src/modules/navigator/mission_block.h    |  3 +--
 src/modules/navigator/navigator.h        |  6 ++---
 src/modules/navigator/navigator_main.cpp |  4 ++--
 src/modules/navigator/rtl.cpp            |  9 +------
 7 files changed, 34 insertions(+), 39 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 8bbb79d5e..f5eb1e7a5 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -70,11 +70,10 @@ bool
 Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	/* set loiter item, don't reuse an existing position setpoint */
-	return set_loiter_item(false, pos_sp_triplet);;
+	return set_loiter_item(pos_sp_triplet);
 }
 
 void
 Loiter::on_inactive()
 {
 }
-
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index becaa09e8..c09f61744 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -253,9 +253,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: no mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
+        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		set_loiter_item(pos_sp_triplet);
 		reset_mission_item_reached();
 		report_mission_finished();
 	}
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index a8eec92ca..41d81ad9b 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
 }
 
 bool
-MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	if (_navigator_priv->get_is_in_loiter()) {
-		/* already loitering, bail out */
-		return false;
-	}
-
-	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* leave position setpoint as is */
-	} else {
+    /* don't change setpoint if 'can_loiter_at_sp' flag set */
+	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
 		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+
+	    _navigator_priv->set_can_loiter_at_sp(true);
 	}
-	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
-	pos_sp_triplet->current.loiter_direction = 1;
 
-	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.valid = true;
-	pos_sp_triplet->next.valid = false;
+    if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
+            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_direction != 1
+            || pos_sp_triplet->previous.valid
+            || !pos_sp_triplet->current.valid
+            || pos_sp_triplet->next.valid) {
+        /* position setpoint triplet should be updated */
+        pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_direction = 1;
+
+        pos_sp_triplet->previous.valid = false;
+        pos_sp_triplet->current.valid = true;
+        pos_sp_triplet->next.valid = false;
+        return true;
+    }
 
-	_navigator_priv->set_is_in_loiter(true);
-	return true;
+    return false;
 }
-
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index a740deea4..8726964fa 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -90,11 +90,10 @@ public:
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *
-	 * @param true if the current position setpoint should be re-used
 	 * @param the position setpoint triplet to set
 	 * @return true if setpoint has changed
 	 */
-	bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
+	bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
 
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index dadd15527..4709f7196 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -102,7 +102,7 @@ public:
 	/**
 	 * Setters
 	 */
-	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 
 	/**
 	 * Getters
@@ -113,7 +113,7 @@ public:
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence&	get_geofence() { return _geofence; }
-	bool		get_is_in_loiter() { return _is_in_loiter; }
+	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	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; }
@@ -161,7 +161,7 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
-	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d762b8a9d..f39300590 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -347,7 +347,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_navigation_mode = &_mission;
@@ -365,7 +365,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_TERMINATION:
 			default:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 		}
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 46194ed52..411f8c527 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
     }
 
     set_previous_pos_setpoint(pos_sp_triplet);
+    _navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
@@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(true);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
 		break;
 	}
-- 
cgit v1.2.3


From 9ae44291b169a90945f090487e3757db9fc8c075 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:27:08 +0200
Subject: navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER and
 RTL_STATE_LANDED instead of FINISHED

---
 src/modules/navigator/mission_block.cpp | 25 ++++++----
 src/modules/navigator/rtl.cpp           | 84 ++++++++++++++++++++++++---------
 src/modules/navigator/rtl.h             |  3 +-
 src/modules/uORB/topics/mission.h       |  1 +
 4 files changed, 82 insertions(+), 31 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 41d81ad9b..5e545706d 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,10 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
-	/* don't check landed WPs */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return false;
+		return _navigator_priv->get_vstatus()->condition_landed;
 	}
+
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
@@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
 
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
+	case NAV_CMD_LAND:
 		sp->type = SETPOINT_TYPE_LAND;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = SETPOINT_TYPE_LOITER;
+		break;
 
-	} else {
+	default:
 		sp->type = SETPOINT_TYPE_POSITION;
+		break;
 	}
 }
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c5bb06c3b..b7961a260 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -53,6 +53,8 @@
 #include "navigator.h"
 #include "rtl.h"
 
+#define DELAY_SIGMA	0.01f
+
 RTL::RTL(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
@@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		if (_rtl_state == RTL_STATE_NONE) {
 			/* for safety reasons don't go into RTL if landed */
 			if (_navigator->get_vstatus()->condition_landed) {
-				_rtl_state = RTL_STATE_FINISHED;
+				_rtl_state = RTL_STATE_LANDED;
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
 			/* if lower than return altitude, climb up first */
@@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		}
 
 		set_rtl_item(pos_sp_triplet);
-
 		updated = true;
-	}
 
-	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		/* RTL finished, nothing to do */
-		pos_sp_triplet->next.valid = false;
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-		return;
-	}
-
 	set_previous_pos_setpoint(pos_sp_triplet);
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
@@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_RETURN: {
 
+	case RTL_STATE_RETURN: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		 // don't change altitude
@@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_DESCEND: {
 
+	case RTL_STATE_DESCEND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		_mission_item.acceptance_radius = _param_acceptance_radius.get();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
@@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_LAND: {
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
 
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -262,18 +300,20 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < 0) {
-			_rtl_state = RTL_STATE_FINISHED;
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 		break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_FINISHED;
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
 		break;
 
-	case RTL_STATE_FINISHED:
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
 		break;
 
 	default:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index bcccf7454..c4286b391 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -97,8 +97,9 @@ private:
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
 		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
-		RTL_STATE_FINISHED,
+		RTL_STATE_LANDED,
 	} _rtl_state;
 
 	control::BlockParamFloat _param_return_alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d25db3a77..d9dd61df1 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -50,6 +50,7 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
+	NAV_CMD_IDLE=0,
 	NAV_CMD_WAYPOINT=16,
 	NAV_CMD_LOITER_UNLIMITED=17,
 	NAV_CMD_LOITER_TURN_COUNT=18,
-- 
cgit v1.2.3


From 1bae18377a444279f91b75281ffde5da70cc6086 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:29:00 +0200
Subject: navigator: is_mission_item_reached() for LOITER items fixed

---
 src/modules/navigator/mission_block.cpp | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 5e545706d..8ef7be449 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -74,15 +74,10 @@ MissionBlock::is_mission_item_reached()
 	}
 
 	/* TODO: count turns */
-#if 0
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
 		return false;
 	}
-#endif
 
 	hrt_abstime now = hrt_absolute_time();
 
-- 
cgit v1.2.3


From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:09:49 +0200
Subject: navigator: use common "acceptance radius" parameter for all modes

---
 src/modules/navigator/mission_block.cpp  |  4 ++--
 src/modules/navigator/navigator.h        |  4 ++--
 src/modules/navigator/navigator_main.cpp |  2 +-
 src/modules/navigator/navigator_params.c |  6 +++---
 src/modules/navigator/rtl.cpp            | 15 +++++++--------
 src/modules/navigator/rtl.h              |  1 -
 src/modules/navigator/rtl_params.c       | 12 ------------
 7 files changed, 15 insertions(+), 29 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 8ef7be449..4f177d76e 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached()
 		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()) {
+				altitude_amsl - _navigator_priv->get_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()) {
+			if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 4709f7196..184ecd365 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -115,7 +115,7 @@ 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(); }
-	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -165,7 +165,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 */
+	control::BlockParamFloat _param_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 e22c4d72d..546602741 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -123,7 +123,7 @@ Navigator::Navigator() :
 	_rtl(this, "RTL"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index ca31adecc..084afe340 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -55,12 +55,12 @@
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Takeoff Acceptance Radius (FW only)
+ * Acceptance Radius
  *
- * Acceptance radius for fixedwing.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
  * @min 1.0
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b7961a260..af2ff308c 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_param_acceptance_radius(this, "ACCEPT_RAD")
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
@@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = false;
@@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = autoland;
@@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_IDLE;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index c4286b391..90299c3fa 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -105,7 +105,6 @@ private:
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
-	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 63724f461..bfe6ce7e1 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
-
-/**
- * RTL acceptance radius
- *
- * Acceptance radius for waypoints set for RTL
- *
- * @unit meters
- * @min 1
- * @max
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);
-- 
cgit v1.2.3


From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:34:19 +0200
Subject: navigator: make MissionBlock subclass of NavigatorMode

---
 src/modules/navigator/loiter.cpp        |  3 +--
 src/modules/navigator/loiter.h          |  2 +-
 src/modules/navigator/mission.cpp       |  3 +--
 src/modules/navigator/mission.h         |  2 +-
 src/modules/navigator/mission_block.cpp | 44 ++++++++++++++++-----------------
 src/modules/navigator/mission_block.h   | 11 +++------
 src/modules/navigator/rtl.cpp           |  3 +--
 src/modules/navigator/rtl.h             |  2 +-
 8 files changed, 32 insertions(+), 38 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f5eb1e7a5..542483fb1 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,8 +53,7 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator)
+	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 685ae6141..65ff5c31e 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -47,7 +47,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
-class Loiter : public NavigatorMode, MissionBlock
+class Loiter : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c09f61744..72255103b 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -58,8 +58,7 @@
 #include "mission.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 842c8c9ee..6e4761946 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -62,7 +62,7 @@
 
 class Navigator;
 
-class Mission : public NavigatorMode, MissionBlock
+class Mission : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 4f177d76e..9b8d3d9c7 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -52,13 +52,13 @@
 #include "mission_block.h"
 
 
-MissionBlock::MissionBlock(Navigator *navigator) :
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item({0}),
-	_mission_item_valid(false),
-	_navigator_priv(navigator)
+	_mission_item_valid(false)
 {
 }
 
@@ -70,7 +70,7 @@ bool
 MissionBlock::is_mission_item_reached()
 {
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _navigator_priv->get_vstatus()->condition_landed;
+		return _navigator->get_vstatus()->condition_landed;
 	}
 
 	/* TODO: count turns */
@@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
 			              : _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator_priv->get_global_position()->lat,
-							  _navigator_priv->get_global_position()->lon,
-							  _navigator_priv->get_global_position()->alt,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_acceptance_radius()) {
+			if (_navigator->get_global_position()->alt >
+				altitude_amsl - _navigator->get_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_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
 			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
@@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
 	sp->yaw = item->yaw;
 	sp->loiter_radius = item->loiter_radius;
 	sp->loiter_direction = item->loiter_direction;
@@ -211,25 +211,25 @@ bool
 MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
     /* don't change setpoint if 'can_loiter_at_sp' flag set */
-	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
+	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
-		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
 
-	    _navigator_priv->set_can_loiter_at_sp(true);
+	    _navigator->set_can_loiter_at_sp(true);
 	}
 
     if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
             || pos_sp_triplet->current.loiter_direction != 1
             || pos_sp_triplet->previous.valid
             || !pos_sp_triplet->current.valid
             || pos_sp_triplet->next.valid) {
         /* position setpoint triplet should be updated */
         pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
         pos_sp_triplet->current.loiter_direction = 1;
 
         pos_sp_triplet->previous.valid = false;
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 8726964fa..f99002752 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -47,17 +47,17 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 
+#include "navigator_mode.h"
+
 class Navigator;
 
-class MissionBlock
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
-	 *
-	 * @param pointer to parent class
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
@@ -101,9 +101,6 @@ public:
 
 	mission_item_s _mission_item;
 	bool _mission_item_valid;
-
-private:
-	Navigator *_navigator_priv;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index af2ff308c..043f773a4 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -56,8 +56,7 @@
 #define DELAY_SIGMA	0.01f
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 90299c3fa..b4b729e89 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -54,7 +54,7 @@
 
 class Navigator;
 
-class RTL : public NavigatorMode, MissionBlock
+class RTL : public MissionBlock
 {
 public:
 	/**
-- 
cgit v1.2.3


From 20de2da032083187bd89914942ffc8574904c885 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 30 Jun 2014 12:20:23 +0200
Subject: Navigator: Warning fixes

---
 src/modules/navigator/mission_block.cpp | 4 +++-
 src/modules/navigator/rtl.cpp           | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

(limited to 'src/modules/navigator/mission_block.cpp')

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 9b8d3d9c7..26a573544 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -42,6 +42,8 @@
 #include <string.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <math.h>
+#include <float.h>
 
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -222,7 +224,7 @@ MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet
 	}
 
     if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
+            || (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
             || pos_sp_triplet->current.loiter_direction != 1
             || pos_sp_triplet->previous.valid
             || !pos_sp_triplet->current.valid
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 043f773a4..597a2c0ec 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -227,7 +227,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_navigator->set_can_loiter_at_sp(true);
 
 		if (autoland) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
 
 		} else {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
-- 
cgit v1.2.3