navigator: mission and loiter working now

7 files changed, 202 insertions, 47 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
new file mode 100644
index 000000000..6da08f072
--- /dev/null
+++ b/src/modules/navigator/loiter.cpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 loiter.cpp
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "loiter.h"
+
+Loiter::Loiter(Navigator *navigator, const char *name) :
+	Mission(navigator, name)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	reset();
+}
+
+Loiter::~Loiter()
+{
+}
+
+bool
+Loiter::update(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);;
+}
+
+void
+Loiter::reset()
+{
+}
+
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
new file mode 100644
index 000000000..4ae265e44
--- /dev/null
+++ b/src/modules/navigator/loiter.h
@@ -0,0 +1,69 @@
+/***************************************************************************
+ *
+ *   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 loiter.h
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_LOITER_H
+#define NAVIGATOR_LOITER_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include "mission.h"
+
+class Navigator;
+
+class Loiter : public Mission
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Loiter(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~Loiter();
+
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	virtual void reset();
+};
+
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 879e5b618..839c4c960 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -70,8 +70,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE),
-	_loiter_set(false)
+	_mission_type(MISSION_TYPE_NONE)
 {
 	/* load initial params */
 	updateParams();
@@ -88,13 +87,11 @@ void
 Mission::reset()
 {
 	_first_run = true;
-	_loiter_set = false;
 }
 
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-
 	/* check if anything has changed */
 	bool onboard_updated = is_onboard_mission_updated();
 	bool offboard_updated = is_offboard_mission_updated();
@@ -115,13 +112,6 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 		updated = true;
 	}
 
-	/* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */
-	if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) {
-		bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached);
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
-		updated = true;
-		_loiter_set = true;
-	}
 	return updated;
 }
 
@@ -243,12 +233,18 @@ Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, st
 	}
 }
 
-void
+bool
 Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+	if (_navigator->get_is_in_loiter()) {
+		/* already loitering, bail out */
+		return false;
+	}
+
 	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* nothing to be done, just use the current item */
+		/* leave position setpoint as is */
 	} else {
+		/* use current position */
 		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;
@@ -261,6 +257,9 @@ Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_tri
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.valid = true;
 	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_is_in_loiter(true);
+	return true;
 }
 
 
@@ -372,14 +371,18 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting onboard mission item */
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_loiter_set = false;
+		_navigator->set_is_in_loiter(false);
 
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting offboard mission item */
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_loiter_set = false;
+		_navigator->set_is_in_loiter(false);
 	} else {
 		_mission_type = MISSION_TYPE_NONE;
+
+		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
+		reset_mission_item_reached();
+		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
 	}
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index a9ef71606..4d0083d85 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -32,6 +32,7 @@
  ****************************************************************************/
 /**
  * @file mission.h
+ *
  * Helper class to access missions
  *
  * @author Julian Oes <julian@oes.ch>
@@ -58,7 +59,7 @@
 
 class Navigator;
 
-class __EXPORT Mission : public control::SuperBlock
+class Mission : public control::SuperBlock
 {
 public:
 	/**
@@ -99,17 +100,12 @@ protected:
 
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 * @return true if setpoint has changed
 	 */
-	void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
+	bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
 
 	class Navigator *_navigator;
 
-	bool _waypoint_position_reached;
-	bool _waypoint_yaw_reached;
-	hrt_abstime _time_first_inside_orbit;
-
-	bool _first_run;
-
 private:
 	/**
 	 * Update onboard mission topic
@@ -183,6 +179,12 @@ private:
 	 */
 	void publish_mission_result();
 
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	bool _first_run;
+
 	control::BlockParamFloat _param_onboard_enabled;
 	control::BlockParamFloat _param_loiter_radius;
 
@@ -203,8 +205,6 @@ private:
 		MISSION_TYPE_OFFBOARD
 	} _mission_type;
 
-	bool _loiter_set;
-
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 
 };
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 88d8aac5d..f4243c5b8 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND	= navigator
 SRCS		= navigator_main.cpp \
 		  mission.cpp \
 		  mission_params.c \
+		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9f877cd76..0c551bb41 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 
 #include "mission.h"
+#include "loiter.h"
 #include "rtl.h"
 #include "geofence.h"
 
@@ -88,14 +89,20 @@ public:
 	void		load_fence_from_file(const char *filename);
 
 	/**
+	 * Setters
+	 */
+	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+
+	/**
 	 * Getters
 	 */
-	struct vehicle_status_s* get_vstatus() { return &_vstatus; }
-	struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
-	struct home_position_s* get_home_position() { return &_home_pos; }
-	int get_onboard_mission_sub() { return _onboard_mission_sub; }
-	int get_offboard_mission_sub() { return _offboard_mission_sub; }
-	Geofence& get_geofence() { return _geofence; }
+	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	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; }
 
 private:
 
@@ -133,14 +140,11 @@ private:
 	bool		_inside_fence;			/**< vehicle is inside fence */
 
 	Mission		_mission;			/**< class that handles the missions */
-	//Loiter		_loiter;			/**< class that handles loiter */
+	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
 
-	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
-	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
-	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
-
-	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
+	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	/**
 	 * Retrieve global position
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3c8875a74..44c1075c1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -102,9 +102,9 @@ Navigator::Navigator() :
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-	_pos_sp_triplet_pub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
+	_pos_sp_triplet_pub(-1),
 	_vstatus({}),
 	_control_mode({}),
 	_global_pos({}),
@@ -119,11 +119,8 @@ Navigator::Navigator() :
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(this, "MIS"),
-	//_loiter(&_global_pos, &_home_pos, &_vstatus),
+	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_waypoint_position_reached(false),
-	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
 	_update_triplet(false)
 {
 }
@@ -325,26 +322,29 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_mission.reset();
+				_loiter.reset();
 				_rtl.reset();
+				_is_in_loiter = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_update_triplet = _mission.update(&_pos_sp_triplet);
-				_rtl.reset();
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
-				//_loiter.update();
+				_update_triplet = _loiter.update(&_pos_sp_triplet);
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
 			case NAVIGATION_STATE_AUTO_RTL_RC:
 			case NAVIGATION_STATE_AUTO_RTL_DL:
-				_mission.reset();
 				_update_triplet = _rtl.update(&_pos_sp_triplet);
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
+				_mission.reset();
+				_loiter.reset();
+				_rtl.reset();
+				_is_in_loiter = false;
 				break;
-
 		}
 
 		if (_update_triplet ) {
@@ -354,7 +354,6 @@ Navigator::task_main()
 
 		perf_end(_loop_perf);
 	}
-
 	warnx("exiting.");
 
 	_navigator_task = -1;