navigator: missions work again, loiter when finished or no mission available or sd card removed works as well

6 files changed, 191 insertions, 38 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7e02f8c15..38badcff9 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -62,20 +62,22 @@ Mission::Mission(Navigator *navigator) :
 	_navigator(navigator),
 	_first_run(true),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_param_loiter_radius(this, "LOITER_RAD"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
+	_current_onboard_mission_index(-1),
+	_current_offboard_mission_index(-1),
 	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE)
+	_mission_type(MISSION_TYPE_NONE),
+	_loiter_set(false)
 {
 	/* load initial params */
 	updateParams();
 	/* set initial mission items */
 	reset();
 
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 }
 
 Mission::~Mission()
@@ -86,13 +88,15 @@ 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 = false; //is_onboard_mission_updated();
+	bool onboard_updated = is_onboard_mission_updated();
 	bool offboard_updated = is_offboard_mission_updated();
 
 	bool updated = false;
@@ -104,11 +108,20 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 		_first_run = false;
 	}
 
+	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
 		advance_mission();
 		set_mission_items(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;
 }
 
@@ -195,17 +208,17 @@ Mission::is_mission_item_reached()
 }
 
 void
-Mission::reset_mission_item_reached() {
+Mission::reset_mission_item_reached()
+{
 	_waypoint_position_reached = false;
 	_waypoint_yaw_reached = false;
 	_time_first_inside_orbit = 0;
 }
 
 void
-Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
+Mission::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->get_home_position()->alt : item->altitude;
@@ -230,19 +243,56 @@ Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_
 	}
 }
 
+void
+Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
+		/* nothing to be done, just use the current item */
+	} else {
+		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 */
+	}
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+	pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get();
+	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;
+}
+
+
 bool
 Mission::is_onboard_mission_updated()
 {
 	bool updated;
-	orb_check(_onboard_mission_sub, &updated);
+	orb_check(_navigator->get_onboard_mission_sub(), &updated);
 
 	if (!updated && !_first_run) {
 		return false;
 	}
 
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
+	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
+		/* accept the current index set by the onboard mission if it is within bounds */
+		if (_onboard_mission.current_index >=0
+		&& _onboard_mission.current_index < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
+				_current_onboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_onboard_mission_index < 0) {
+				_current_onboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+	} else {
 		_onboard_mission.count = 0;
 		_onboard_mission.current_index = 0;
+		_current_onboard_mission_index = 0;
 	}
 	return true;
 }
@@ -251,17 +301,27 @@ bool
 Mission::is_offboard_mission_updated()
 {
 	bool updated;
-	warnx("sub: %d", _offboard_mission_sub);
-	orb_check(_offboard_mission_sub, &updated);
+	orb_check(_navigator->get_offboard_mission_sub(), &updated);
+
 	if (!updated && !_first_run) {
-		warnx("not updated");
 		return false;
 	}
-	struct mission_s offboard_mission;
-	int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission);
-	warnx("ret: %d", ret);
-	if (ret == OK) {
-		warnx("copy new offboard mission");
+	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
+
+		/* determine current index */
+		if (_offboard_mission.current_index >= 0
+		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
+				_current_offboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_offboard_mission_index < 0) {
+				_current_offboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -278,9 +338,9 @@ Mission::is_offboard_mission_updated()
 							      _navigator->get_geofence(),
 							      _navigator->get_home_position()->alt);
 	} else {
-		warnx("no success with orb_copy");
 		_offboard_mission.count = 0;
 		_offboard_mission.current_index = 0;
+		_current_offboard_mission_index = 0;
 	}
 	return true;
 }
@@ -291,11 +351,11 @@ Mission::advance_mission()
 {
 	switch (_mission_type) {
 	case MISSION_TYPE_ONBOARD:
-		_onboard_mission.current_index++;
+		_current_onboard_mission_index++;
 		break;
 
 	case MISSION_TYPE_OFFBOARD:
-		_offboard_mission.current_index++;
+		_current_offboard_mission_index++;
 		break;
 
 	case MISSION_TYPE_NONE:
@@ -307,17 +367,17 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	warnx("set mission items");
-
 	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
 
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting onboard mission item */
 		_mission_type = MISSION_TYPE_ONBOARD;
+		_loiter_set = 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;
 	} else {
 		_mission_type = MISSION_TYPE_NONE;
 	}
@@ -337,15 +397,18 @@ bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
 	if (_param_onboard_enabled.get() > 0
-	    && _onboard_mission.current_index < (int)_onboard_mission.count) {
+	    && _current_onboard_mission_index < (int)_onboard_mission.count) {
 		struct mission_item_s new_mission_item;
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
 					&new_mission_item)) {
 			/* convert the current mission item and set it valid */
-			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
 			current_pos_sp->valid = true;
 
+			reset_mission_item_reached();
+
 			/* TODO: report this somehow */
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
 			return true;
 		}
 	}
@@ -355,9 +418,7 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 bool
 Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
-	warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count );
-	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
-		warnx("theoretically possible");
+	if (_current_offboard_mission_index < (int)_offboard_mission.count) {
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -365,18 +426,20 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 		struct mission_item_s new_mission_item;
-		if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
 			/* convert the current mission item and set it valid */
-			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
 			current_pos_sp->valid = true;
 
+			reset_mission_item_reached();
+
 			report_current_offboard_mission_item();
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
 			return true;
 		} else {
-			warnx("read fail");
+			warnx("ERROR: WP read fail");
 		}
 	}
-	warnx("failed with offboard mission");
 	return false;
 }
 
@@ -443,6 +506,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
+			/* TODO: do this check more gracefully since it is not a serious error */
 			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
 				return false;
 			}
@@ -458,6 +522,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 					/* not supposed to happen unless the datamanager can't access the dataman */
 					return false;
 				}
+				/* TODO: report about DO JUMP count */
 			}
 			/* set new mission item index and repeat
 			 * we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -479,7 +544,7 @@ Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _offboard_mission.current_index;
+		_mission_result.mission_index_reached = _current_offboard_mission_index;
 	}
 	publish_mission_result();
 }
@@ -487,7 +552,7 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _offboard_mission.current_index;
+	_mission_result.index_current_mission = _current_offboard_mission_index;
 	publish_mission_result();
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 65a0991b5..f75c8a882 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -58,7 +58,7 @@
 
 class Navigator;
 
-class Mission : public control::SuperBlock
+class __EXPORT Mission : public control::SuperBlock
 {
 public:
 	/**
@@ -97,6 +97,11 @@ protected:
 	 */
 	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
 
+	/**
+	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 */
+	void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	class Navigator *_navigator;
 
 	bool _waypoint_position_reached;
@@ -179,13 +184,14 @@ private:
 	void publish_mission_result();
 
 	control::BlockParamFloat _param_onboard_enabled;
-
-	int _onboard_mission_sub;
-	int _offboard_mission_sub;
+	control::BlockParamFloat _param_loiter_radius;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
 
+	int _current_onboard_mission_index;
+	int _current_offboard_mission_index;
+
 	struct mission_item_s _mission_item;
 
 	orb_advert_t _mission_result_pub;
@@ -197,6 +203,8 @@ 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/mission_params.c b/src/modules/navigator/mission_params.c
new file mode 100644
index 000000000..9696b3e53
--- /dev/null
+++ b/src/modules/navigator/mission_params.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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_params.c
+ *
+ * Parameters for mission.
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Mission parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter radius (fixed wing only)
+ *
+ * Default value of loiter radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f);
+
+/**
+ * Enable onboard mission
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 2d07bc49c..e2cc475da 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND	= navigator
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  mission.cpp \
+		  mission_params.c \
 		  rtl.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 927e77391..a6ab85519 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -87,9 +87,14 @@ public:
 	 */
 	void		load_fence_from_file(const char *filename);
 
+	/**
+	 * 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; }
 
 private:
@@ -105,6 +110,8 @@ private:
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_onboard_mission_sub;		/**< onboard mission subscription */
+	int		_offboard_mission_sub;		/**< offboard mission subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 9dd253127..b7dfec047 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -106,6 +106,8 @@ Navigator::Navigator() :
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
 	_vstatus({}),
 	_control_mode({}),
 	_global_pos({}),
@@ -251,6 +253,8 @@ Navigator::task_main()
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 
 	/* copy all topics first time */
 	vehicle_status_update();