Mavlink and navigator: handle current waypoint onboard

1 files changed, 16 insertions, 9 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 260356eca..e1dde35bf 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -65,7 +65,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
@@ -288,9 +287,9 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	void		publish_safepoints(unsigned points);
 
@@ -384,7 +383,6 @@ Navigator::Navigator() :
 
 /* publications */
 	_pos_sp_triplet_pub(-1),
-	_mission_result_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -416,7 +414,6 @@ Navigator::Navigator() :
 	_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
 
 	memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 	memset(&_mission_item, 0, sizeof(struct mission_item_s));
 
 	memset(&nav_states_str, 0, sizeof(nav_states_str));
@@ -518,13 +515,16 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
-		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
+
 		_mission.set_offboard_mission_count(offboard_mission.count);
+		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
 
 	} else {
-		_mission.set_current_offboard_mission_index(0);
 		_mission.set_offboard_mission_count(0);
+		_mission.set_current_offboard_mission_index(0);
 	}
+
+	_mission.publish_mission_result();
 }
 
 void
@@ -534,12 +534,12 @@ Navigator::onboard_mission_update()
 
 	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
 		_mission.set_onboard_mission_count(onboard_mission.count);
+		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
 
 	} else {
-		_mission.set_current_onboard_mission_index(0);
 		_mission.set_onboard_mission_count(0);
+		_mission.set_current_onboard_mission_index(0);
 	}
 }
 
@@ -1116,6 +1116,9 @@ Navigator::set_mission_item()
 	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
 
 	if (ret == OK) {
+
+		_mission.report_current_mission_item();
+
 		/* reset time counter for new item */
 		_time_first_inside_orbit = 0;
 
@@ -1537,6 +1540,9 @@ void
 Navigator::on_mission_item_reached()
 {
 	if (myState == NAV_STATE_MISSION) {
+
+		_mission.report_mission_item_reached();
+
 		if (_do_takeoff) {
 			/* takeoff completed */
 			_do_takeoff = false;
@@ -1589,6 +1595,7 @@ Navigator::on_mission_item_reached()
 		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
 		dispatch(EVENT_READY_REQUESTED);
 	}
+	_mission.publish_mission_result();
 }
 
 void