Navigator, waypoints: save index in mission item and use this in navigator

4 files changed, 99 insertions, 67 deletions

diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index a4e31bda6..a9ee26eac 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -60,6 +60,10 @@
 bool debug = false;
 bool verbose = false;
 
+orb_advert_t mission_pub = -1;
+struct mission_s mission;
+
+#define NUM_MISSIONS_SUPPORTED 10
 
 //#define MAVLINK_WPM_NO_PRINTF
 #define MAVLINK_WPM_VERBOSE 1
@@ -78,9 +82,10 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl
 	mission_item->radius = mavlink_mission_item->param1;
 	mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
+	mission_item->index = mavlink_mission_item->seq;
 }
 
-void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item)
+void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
 {
 
 	mavlink_mission_item->x = (float)mission_item->lat;
@@ -92,8 +97,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi
 	mavlink_mission_item->param1 = mission_item->radius;
 	mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
 	mavlink_mission_item->autocontinue = mission_item->autocontinue;
-
-	mavlink_mission_item->seq = seq;
+	mavlink_mission_item->seq = mission_item->index;
 }
 
 void mavlink_wpm_init(mavlink_wpm_storage *state)
@@ -117,6 +121,13 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
 	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
 
+	mission.count = 0;
+	mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED);
+	if (!mission.items) {
+		mission.count = 0;
+		/* XXX reject waypoints if this fails */
+		warnx("no free RAM to allocate mission, rejecting any waypoints");
+	}
 }
 
 /*
@@ -558,9 +569,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 
 void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
 {
-	static orb_advert_t mission_pub = -1;
-	static struct mission_s mission;
-
 	uint64_t now = mavlink_missionlib_get_system_timestamp();
 
 	switch (msg->msgid) {
@@ -711,7 +719,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					wpm->current_wp_id = wpr.seq;
 
 					mavlink_mission_item_t wp;
-					map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp);
+					map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
 					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
 
 				} else {
@@ -835,14 +843,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						wpm->current_partner_compid = msg->compid;
 						wpm->current_count = wpc.count;
 
-						/* prepare mission topic */
-						mission.count = wpc.count;
-						mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count);
-						
-						if (!mission.items) {
-							mission.count = 0;
-							/* XXX reject waypoints if this fails */
-							warnx("no free RAM to allocate mission, rejecting any waypoints");
+						if (wpc.count > NUM_MISSIONS_SUPPORTED) {
+							warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+						} else {
+							/* prepare mission topic */
+							mission.count = wpc.count;
 						}
 
 #ifdef MAVLINK_WPM_NO_PRINTF
@@ -1087,8 +1092,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				wpm->yaw_reached = false;
 				wpm->pos_reached = false;
 
+				/* prepare mission topic */
+				mission.count = 0;
+				memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED);
+
+				/* Initialize mission publication if necessary */
+				if (mission_pub < 0) {
+					mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+				} else {
+					orb_publish(ORB_ID(mission), mission_pub, &mission);
+				}
+
+
+				warnx("Mission cleared");
+
+				mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
+				warnx("not cleared");
 			}
 
 			break;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index fc2f6d380..9066a3b42 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -69,11 +69,6 @@
 #include <mathlib/mathlib.h>
 #include <dataman/dataman.h>
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
 
 /**
  * navigator app start / stop handling function
@@ -142,8 +137,6 @@ private:
 	unsigned	_mission_item_count;		/** number of mission items copied */
 	struct mission_item_s				*_mission_item;	/**< storage for mission */
 
-	int		_current_mission_item_index;	/** current active mission item , -1 for none */
-
 	struct	fence_s 				_fence;			/**< storage for fence vertices */
 	bool						_fence_valid;		/**< flag if fence is valid */
         bool						_inside_fence;		/**< vehicle is inside fence */
@@ -213,9 +206,11 @@ private:
 
 	bool		fence_valid(const struct fence_s &fence);
 
-	int		add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item);
+	void		add_mission_item(unsigned mission_item_index, 
+			                 const struct mission_item_s *existing_mission_item,
+			                 struct mission_item_s *new_mission_item);
 
-	void		add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item);
+	void		update_mission_item_triplet();
 
 	void		advance_current_mission_item();
 
@@ -260,7 +255,6 @@ Navigator::Navigator() :
 	_max_mission_item_count(10),
 	_fence_valid(false),
 	_inside_fence(true),
-	_current_mission_item_index(-1),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
@@ -626,27 +620,20 @@ Navigator::fence_point(int argc, char *argv[])
 	errx(1, "can't store fence point");
 }
 
-int
-Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) {
+void
+Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) {
 
 	/* Check if there is a further mission as the new next item */
 	while (mission_item_index < _mission_item_count) {
 
 		if (1 /* TODO: check for correct frame */) {
 
-			warnx("copying item number %d", mission_item_index);
 			memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s));
-			return OK;
+			return;
 		}
 		mission_item_index++;
 	}
 
-	return ERROR;
-}
-
-void
-Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item)
-{
 	/* if no existing mission item exists, take curent location */
 	if (existing_mission_item == nullptr) {
 		
@@ -706,54 +693,71 @@ Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_i
 }
 
 void
-Navigator::advance_current_mission_item()
+Navigator::update_mission_item_triplet()
 {
-	/* if there is one more mission available we can just advance by one, otherwise return */
-	if (_mission_item_triplet.next_valid) {
-
-		reset_mission_item_reached();
-
-		/* copy current mission to previous item */
-		memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
-		_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
-
-		/* copy the next to current */
-		memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s));
-		_mission_item_triplet.current_valid = _mission_item_triplet.next_valid;
-
-		_current_mission_item_index++;
+	if (!_mission_item_triplet.current_valid) {
+		
+		/* the current mission item is missing, add one */
+		if (_mission_item_triplet.previous_valid) {
+			/* if we know the last one, proceed to succeeding one */
+			add_mission_item(_mission_item_triplet.previous.index + 1, &_mission_item_triplet.previous, &_mission_item_triplet.current);
+		}
+		else {
+			/* if we don't remember the last one, start new */
+			add_mission_item(0, nullptr, &_mission_item_triplet.current);
+		}
+		_mission_item_triplet.current_valid = true;
+	}
 
+	if (_mission_item_triplet.current_valid && !_mission_item_triplet.next_valid) {
 		
-		/* maybe there are no more mission item, in this case add a loiter mission item */
-		if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
+		if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+			/* if we are already loitering, don't bother about a next mission item */
 
-			add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
+			_mission_item_triplet.next_valid = false;
+		} else {
+
+			add_mission_item(_mission_item_triplet.current.index + 1, &_mission_item_triplet.current, &_mission_item_triplet.next);
 			_mission_item_triplet.next_valid = true;
 		}
 	}
 }
 
 void
-Navigator::restart_mission()
+Navigator::advance_current_mission_item()
 {
+	/* if there is no more mission available, don't advance and return */
+	if (!_mission_item_triplet.next_valid) {
+		return;
+	}
+
 	reset_mission_item_reached();
 
-	_current_mission_item_index = 0;
+	/* copy current mission to previous item */
+	memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
+	_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
-	_mission_item_triplet.previous_valid = false;
+	/* copy the next to current */
+	memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s));
+	_mission_item_triplet.current_valid = _mission_item_triplet.next_valid;
+
+	/* flag the next mission as invalid */
+	_mission_item_triplet.next_valid = false;
+	
+	update_mission_item_triplet();
+}
+
+void
+Navigator::restart_mission()
+{
+	reset_mission_item_reached();
 
-	/* add a new current mission item */
-	if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) {
+	/* forget about the all mission items */
+	_mission_item_triplet.previous_valid = false;
+	_mission_item_triplet.current_valid = false;
+	_mission_item_triplet.next_valid = false;
 
-		add_last_mission_item(nullptr, &_mission_item_triplet.current);
-	} else {
-		/* if current succeeds, we can even add a next item */
-		if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
-			add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
-		}
-		_mission_item_triplet.next_valid = true;
-	}
-	_mission_item_triplet.current_valid = true;
+	update_mission_item_triplet();
 }
 
 
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index f97de94bc..a39a1e4d7 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -80,6 +80,7 @@ struct mission_item_s
 	float radius;			/**< radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
+	int index;			/**< index matching the mavlink waypoint                */
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h
index 48553b0ac..b35eae607 100644
--- a/src/modules/uORB/topics/mission_item_triplet.h
+++ b/src/modules/uORB/topics/mission_item_triplet.h
@@ -67,6 +67,10 @@ struct mission_item_triplet_s
 	struct mission_item_s previous;
 	struct mission_item_s current;
 	struct mission_item_s next;
+
+	int previous_index;
+	int current_index;
+	int next_index;
 };
 
 /**