Mavlink and navigator: handle current waypoint onboard

3 files changed, 84 insertions, 19 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
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
index e72caf98e..fdc4ffff6 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/src/modules/navigator/navigator_mission.cpp
@@ -36,13 +36,12 @@
  * Helper class to access missions
  */
 
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <string.h>
-// #include <unistd.h>
-
+#include <string.h>
 #include <stdlib.h>
 #include <dataman/dataman.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/mission_result.h>
 #include "navigator_mission.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -60,8 +59,11 @@ Mission::Mission() :
 	_offboard_mission_item_count(0),
 	_onboard_mission_item_count(0),
 	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE)
-{}
+	_current_mission_type(MISSION_TYPE_NONE),
+	_mission_result_pub(-1)
+{
+	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+}
 
 Mission::~Mission()
 {
@@ -78,8 +80,16 @@ void
 Mission::set_current_offboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
+		warnx("specifically set to %d", new_index);
 		_current_offboard_mission_index = (unsigned)new_index;
+	} else {
+
+		/* if less WPs available, reset to first WP */
+		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
+			_current_offboard_mission_index = 0;
+		}
 	}
+	report_current_mission_item();
 }
 
 void
@@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
 		_current_onboard_mission_index = (unsigned)new_index;
+	} else {
+
+		/* if less WPs available, reset to first WP */
+		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
+			_current_onboard_mission_index = 0;
+		}
 	}
+	// TODO: implement this for onboard missions as well
+	// report_current_mission_item();
 }
 
 void
@@ -266,4 +284,37 @@ Mission::move_to_next()
 	default:
 		break;
 	}
-}
\ No newline at end of file
+}
+
+void
+Mission::report_mission_item_reached()
+{
+	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+		_mission_result.mission_reached = true;
+		_mission_result.mission_index_reached = _current_offboard_mission_index;
+	}
+}
+
+void
+Mission::report_current_mission_item()
+{
+	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+		_mission_result.index_current_mission = _current_offboard_mission_index;
+	}
+}
+
+void
+Mission::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.mission_reached = false;
+}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
index 15d4e86bf..845c16583 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/navigator_mission.h
@@ -40,6 +40,7 @@
 #define NAVIGATOR_MISSION_H
 
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
 
 
 class __EXPORT Mission
@@ -71,7 +72,9 @@ public:
 
 	void		move_to_next();
 
-	void		add_home_pos(struct mission_item_s *new_mission_item);
+	void		report_mission_item_reached();
+	void		report_current_mission_item();
+	void		publish_mission_result();
 
 private:
 	bool		current_onboard_mission_available();
@@ -92,6 +95,10 @@ private:
 		MISSION_TYPE_ONBOARD,
 		MISSION_TYPE_OFFBOARD,
 	} 		_current_mission_type;
+
+	int		_mission_result_pub;
+
+	struct mission_result_s _mission_result;
 };
 
-#endif
\ No newline at end of file
+#endif