Mavlink and navigator: handle current waypoint onboard

6 files changed, 142 insertions, 68 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index d2c94ca92..38f753c39 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -59,11 +59,11 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
@@ -958,7 +958,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
 	mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
 	mavlink_missionlib_send_message(&msg);
 
-	if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
+	if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
 }
 
 /*
@@ -981,11 +981,11 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 		mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
 		mavlink_missionlib_send_message(&msg);
 
-		if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
+		if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-		if (verbose) warnx("ERROR: index out of bounds");
+		if (_verbose) warnx("ERROR: index out of bounds");
 	}
 }
 
@@ -1001,7 +1001,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
 	mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
 
-	if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
+	if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
 }
 
 void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
@@ -1031,10 +1031,10 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
 		mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
 		mavlink_missionlib_send_message(&msg);
 
-		if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
+		if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
 	} else {
 		mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-		if (verbose) warnx("ERROR: could not read WP%u", seq);
+		if (_verbose) warnx("ERROR: could not read WP%u", seq);
 	}
 }
 
@@ -1049,11 +1049,11 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 		mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
 
-		if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
+		if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-		if (verbose) warnx("ERROR: Waypoint index exceeds list capacity");
+		if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity");
 	}
 }
 
@@ -1074,10 +1074,9 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
 	mavlink_missionlib_send_message(&msg);
 
-	if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
+	if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
 }
 
-
 void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 {
 	/* check for timed-out operations */
@@ -1085,7 +1084,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 
 		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
+		if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
 
 		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 		wpm->current_partner_sysid = 0;
@@ -1117,7 +1116,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-				if (verbose) warnx("REJ. WP CMD: curr partner id mismatch");
+				if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch");
 			}
 
 			break;
@@ -1140,18 +1139,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-						if (verbose) warnx("IGN WP CURR CMD: Not in list");
+						if (_verbose) warnx("IGN WP CURR CMD: Not in list");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-					if (verbose) warnx("IGN WP CURR CMD: Busy");
+					if (_verbose) warnx("IGN WP CURR CMD: Busy");
 
 				}
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-				if (verbose) warnx("REJ. WP CMD: target id mismatch");
+				if (_verbose) warnx("REJ. WP CMD: target id mismatch");
 			}
 
 			break;
@@ -1173,7 +1172,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 						wpm->current_partner_compid = msg->compid;
 
 					} else {
-						if (verbose) warnx("No waypoints send");
+						if (_verbose) warnx("No waypoints send");
 					}
 
 					wpm->current_count = wpm->size;
@@ -1181,11 +1180,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-					if (verbose) warnx("IGN REQUEST LIST: Busy");
+					if (_verbose) warnx("IGN REQUEST LIST: Busy");
 				}
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
-				if (verbose) warnx("REJ. REQUEST LIST: target id mismatch");
+				if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch");
 			}
 
 			break;
@@ -1201,7 +1200,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (wpr.seq >= wpm->size) {
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
+					if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
 					break;
 				}
 
@@ -1212,11 +1211,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
 					if (wpr.seq == 0) {
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+						if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-						if (verbose) warnx("REJ. WP CMD: First id != 0");
+						if (_verbose) warnx("REJ. WP CMD: First id != 0");
 						break;
 					}
 
@@ -1224,22 +1223,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wpr.seq == wpm->current_wp_id) {
 
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+						if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 
 					} else if (wpr.seq == wpm->current_wp_id + 1) {
 
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+						if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
 					
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
+						if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
 						break;
 					}
 
 				} else {
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
+					if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
 					break;
 				}
 
@@ -1252,7 +1251,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				} else {
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-					if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
+					if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
 				}
 
 
@@ -1261,12 +1260,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
+					if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
 
 				} else {
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-					if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+					if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 				}
 			}
 			break;
@@ -1282,18 +1281,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 
 					if (wpc.count > NUM_MISSIONS_SUPPORTED) {
-						if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+						if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
 						break;
 					}
 
 					if (wpc.count == 0) {
 						mavlink_missionlib_send_gcs_string("COUNT 0");
-						if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
+						if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
 						break;
 					}
 					
-					if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
+					if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
 
 					wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
 					wpm->current_wp_id = 0;
@@ -1307,19 +1306,19 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wpm->current_wp_id == 0) {
 						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
+						if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
+						if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
 					}
 				} else {
 						mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-						if (verbose) warnx("IGN MISSION_COUNT CMD: Busy");
+						if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy");
 				}
 			} else {
 
 				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+				if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 		}
 		break;
@@ -1389,15 +1388,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					break;
 				}
 
-				if (wp.current) {
-					mission.current_index = wp.seq;
-				}
+//				if (wp.current) {
+//					warnx("current is: %d", wp.seq);
+//					mission.current_index = wp.seq;
+//				}
+				// XXX ignore current set
+				mission.current_index = -1;
 
 				wpm->current_wp_id = wp.seq + 1;
 
 				if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
 					
-					if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
+					if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
 
 					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
@@ -1416,7 +1418,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+				if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
@@ -1448,14 +1450,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
-					if (verbose) warnx("IGN WP CLEAR CMD: Busy");
+					if (_verbose) warnx("IGN WP CLEAR CMD: Busy");
 				}
 
 
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 
 				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+				if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
 			}
 
 			break;
@@ -1550,6 +1552,9 @@ Mavlink::task_main(int argc, char *argv[])
 			_mode = MODE_ONBOARD;
 			break;
 
+		case 'v':
+			_verbose = true;
+
 		default:
 			usage();
 			break;
@@ -1767,9 +1772,12 @@ Mavlink::task_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
 
-			if (mission_result.mission_reached) {
-				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
-			}
+			if (_verbose) warnx("Got mission result");
+
+			if (mission_result.mission_reached)
+				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
+
+			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
 		}
 
 		mavlink_waypoint_eventloop(hrt_absolute_time());
@@ -1842,7 +1850,7 @@ Mavlink::status()
 
 static void usage()
 {
-	errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]");
+	errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index c667a41da..9461a51f8 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -293,7 +293,7 @@ private:
 	mavlink_wpm_storage wpm_s;
 	mavlink_wpm_storage *wpm;
 
-	bool verbose;
+	bool _verbose;
 	int _uart;
 	int _baudrate;
 	bool gcs_link;
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
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index c99706b97..7c3921198 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -54,7 +54,8 @@
 struct mission_result_s
 {
 	bool mission_reached;		/**< true if mission has been reached			*/
-	unsigned mission_index;		/**< index of the mission which has been reached	*/
+	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
+	unsigned index_current_mission; /**< index of the current mission			*/
 };
 
 /**