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

1 files changed, 56 insertions, 52 deletions

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();
 }