navigator: more API changes, duplicate code removed

6 files changed, 95 insertions, 119 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 1e86b1b6c..fbfd1259d 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -225,29 +225,27 @@ Mission::set_mission_items()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	set_previous_pos_setpoint(pos_sp_triplet);
+	/* set previous position setpoint to current */
+	set_previous_pos_setpoint();
 
 	/* try setting onboard mission item */
-	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+	if (set_mission_item(true, false, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: onboard mission running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
 	/* try setting offboard mission item */
-	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+	} else if (set_mission_item(false, false, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: offboard mission running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
 	} else {
+		/* no mission available, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
 					"#audio: mission finished");
@@ -257,126 +255,108 @@ Mission::set_mission_items()
 		}
 		_mission_type = MISSION_TYPE_NONE;
 
-		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
-
+		/* set loiter mission item */
 		set_loiter_item(&_mission_item);
 
+		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+
 		reset_mission_item_reached();
 		report_mission_finished();
+
+		_navigator->set_position_setpoint_triplet_updated();
+		return;
 	}
-}
 
-bool
-Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
-{
-	/* make sure param is up to date */
-	updateParams();
-	if (_param_onboard_enabled.get() > 0 &&
-	    _current_onboard_mission_index >= 0&&
-	    _current_onboard_mission_index < (int)_onboard_mission.count) {
-		struct mission_item_s new_mission_item;
-		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(&new_mission_item, current_pos_sp);
+	/* new current mission item set */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	_navigator->set_can_loiter_at_sp(false);
+	reset_mission_item_reached();
+
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		report_current_offboard_mission_item();
+	}
+	// TODO: report onboard mission item somehow
 
-			reset_mission_item_reached();
+	/* try to read next mission item */
+	struct mission_item_s mission_item_next;
+	if (!set_mission_item(_mission_type == MISSION_TYPE_ONBOARD, true, &mission_item_next)) {
+		/* got next mission item, update setpoint triplet */
+		mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
 
-			/* TODO: report this somehow */
-			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
-			return true;
-		}
+	} else {
+		/* next mission item is not available */
+		pos_sp_triplet->next.valid = false;
 	}
-	return false;
+
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
 bool
-Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+Mission::set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item)
 {
-	if (_current_offboard_mission_index >= 0 &&
-	    _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;
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-		struct mission_item_s 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(&new_mission_item, current_pos_sp);
+	/* make sure param is up to date */
+	updateParams();
 
-			reset_mission_item_reached();
+	/* select onboard/offboard mission */
+	int *mission_index_ptr;
+	struct mission_s *mission;
+	dm_item_t dm_item;
+	int mission_index_next;
 
-			report_current_offboard_mission_item();
-			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
-			return true;
+	if (onboard) {
+		/* onboard mission */
+		if (!_param_onboard_enabled.get()) {
+			return false;
 		}
-	}
-	return false;
-}
 
-void
-Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
-{
-	int next_temp_mission_index = _onboard_mission.current_index + 1;
+		mission_index_next = _current_onboard_mission_index + 1;
+		mission_index_ptr = next_item ? &mission_index_next : &_current_onboard_mission_index;
 
-	/* try if there is a next onboard mission */
-	if (_onboard_mission.current_index >= 0 &&
-	    next_temp_mission_index < (int)_onboard_mission.count) {
-		struct mission_item_s new_mission_item;
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
-			/* convert next mission item to position setpoint */
-			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
-			return;
-		}
-	}
+		mission = &_onboard_mission;
 
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
-}
+		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
+
+	} else {
+		/* offboard mission */
+		mission_index_next = _current_offboard_mission_index + 1;
+		mission_index_ptr = next_item ? &mission_index_next : &_current_offboard_mission_index;
+
+		mission = &_offboard_mission;
 
-void
-Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
-{
-	/* try if there is a next offboard mission */
-	int next_temp_mission_index = _offboard_mission.current_index + 1;
-	warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
-	if (_offboard_mission.current_index >= 0 &&
-	    next_temp_mission_index < (int)_offboard_mission.count) {
-		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
 		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+			dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
+	}
+
+	if (*mission_index_ptr >= 0 && *mission_index_ptr < (int)mission->count) {
 		struct mission_item_s new_mission_item;
-		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
-			/* convert next mission item to position setpoint */
-			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
-			return;
+
+		if (read_mission_item(dm_item, true, mission_index_ptr, &new_mission_item)) {
+			memcpy(&mission_item, &new_mission_item, sizeof(struct mission_item_s));
+			return true;
 		}
 	}
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
+	return false;
 }
 
 bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index_ptr,
 		           struct mission_item_s *new_mission_item)
 {
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
-	for (int i=0; i<10; i++) {
+	for (int i = 0; i < 10; i++) {
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+		if (dm_read(dm_item, *mission_index_ptr, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			mavlink_log_critical(_navigator->get_mavlink_fd(),
 			                     "#audio: ERROR waypoint could not be read");
@@ -394,7 +374,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 				if (is_current) {
 					(new_mission_item->do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET,
 						new_mission_item, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
@@ -405,12 +385,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index = new_mission_item->do_jump_mission_index;
+				*mission_index_ptr = new_mission_item->do_jump_mission_index;
 			} else {
 				mavlink_log_info(_navigator->get_mavlink_fd(),
 						 "#audio: DO JUMP repetitions completed");
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
-				(*mission_index)++;
+				(*mission_index_ptr)++;
 			}
 
 		} else {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ac293990f..7d07860de 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -97,16 +97,10 @@ private:
 	void set_mission_items();
 
 	/**
-	 * Try to set the current position setpoint from an onboard mission item
+	 * Try to set given mission item from an offboard/onboard mission item
 	 * @return true if mission item successfully set
 	 */
-	bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
-
-	/**
-	 * Try to set the current position setpoint from an offboard mission item
-	 * @return true if mission item successfully set
-	 */
-	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+	bool set_mission_item(bool onboard, bool next_item, struct mission_item_s *mission_item);
 
 	/**
 	 * Try to set the next position setpoint from an onboard mission item
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index bf3c934a6..cbf7ac987 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -202,9 +202,10 @@ 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)
+MissionBlock::set_previous_pos_setpoint()
 {
-    /* reuse current setpoint as previous setpoint */
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
     if (pos_sp_triplet->current.valid) {
         memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
     }
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 22f42cc31..adf50bc39 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -86,7 +86,7 @@ protected:
     /**
      * Set previous position setpoint to current setpoint
      */
-	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+	void set_previous_pos_setpoint();
 
 	/**
 	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 7cf6bb1a8..6d7afcf4b 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -82,7 +82,7 @@ RTL::on_inactive()
 }
 
 void
-RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
+RTL::on_activation()
 {
 	/* decide where to enter the RTL procedure when we switch into it */
 	if (_rtl_state == RTL_STATE_NONE) {
@@ -105,28 +105,27 @@ RTL::on_activation(struct position_setpoint_triplet_s *pos_sp_triplet)
 		}
 	}
 
-	set_rtl_item(pos_sp_triplet);
+	set_rtl_item();
 }
 
-bool
-RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+RTL::on_active()
 {
 	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
-		set_rtl_item(pos_sp_triplet);
-		return true;
+		set_rtl_item();
 	}
-
-	return false;
 }
 
 void
-RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
+RTL::set_rtl_item()
 {
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 	/* make sure we have the latest params */
 	updateParams();
 
-	set_previous_pos_setpoint(pos_sp_triplet);
+	set_previous_pos_setpoint();
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
@@ -273,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	reset_mission_item_reached();
+
 	/* convert mission item to current position setpoint and make it valid */
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-	reset_mission_item_reached();
-	pos_sp_triplet->current.valid = true;
 	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 9c8b3fdfc..5928f1f07 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -63,15 +63,15 @@ public:
 
 	virtual void on_inactive();
 
-	virtual void on_activation(position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
 
-	virtual bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_active();
 
 private:
 	/**
 	 * Set the RTL item
 	 */
-	void		set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
+	void		set_rtl_item();
 
 	/**
 	 * Move to next RTL item