Merge remote-tracking branch 'private_julian/fw_autoland_att_tecs_navigator' into fw_autoland_att_tecs_navigator

9 files changed, 546 insertions, 367 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 614f00186..85b17f9ae 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -411,6 +411,21 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
 	return sqrtf(dxy * dxy + dz * dz);
 }
 
+
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z)
+{
+	float dx = x_now - x_next;
+	float dy = y_now - y_next;
+	float dz = z_now - z_next;
+
+	*dist_xy = sqrtf(dx * dx + dy * dy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dx * dx + dy * dy + dz * dz);
+}
+
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 47f643593..5f92e14cf 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -124,10 +124,20 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 		float radius, float arc_start_bearing, float arc_sweep);
 
+/*
+ * Calculate distance in global frame
+ */
 __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
 	                                          double lat_next, double lon_next, float alt_next,
 	                                          float *dist_xy, float *dist_z);
 
+/*
+ * Calculate distance in local frame (NED)
+ */
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z);
+
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 7c10e297b..2ec00a9bc 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -74,6 +74,8 @@
 #include "waypoints.h"
 #include "mavlink_parameters.h"
 
+#include <uORB/topics/mission_result.h>
+
 /* define MAVLink specific parameters */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
@@ -644,6 +646,10 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	thread_running = true;
 
 	/* arm counter to go off immediately */
@@ -690,6 +696,17 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
+		bool updated;
+		orb_check(mission_result_sub, &updated);
+
+		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);
+			}
+		}
+
 		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 8e4bbce36..809900d7d 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -70,11 +70,32 @@ struct mission_s mission;
 
 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
-void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
 {
-	mission_item->lat = (double)mavlink_mission_item->x;
-	mission_item->lon = (double)mavlink_mission_item->y;
-	mission_item->altitude = mavlink_mission_item->z;
+	/* only support global waypoints for now */
+	switch (mavlink_mission_item->frame) {
+		case MAV_FRAME_GLOBAL:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = false;
+			break;
+
+		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+			mission_item->lat = (double)mavlink_mission_item->x;
+			mission_item->lon = (double)mavlink_mission_item->y;
+			mission_item->altitude = mavlink_mission_item->z;
+			mission_item->altitude_is_relative = true;
+			break;
+
+		case MAV_FRAME_LOCAL_NED:
+		case MAV_FRAME_LOCAL_ENU:
+			return MAV_MISSION_UNSUPPORTED_FRAME;
+		case MAV_FRAME_MISSION:
+		default:
+			return MAV_MISSION_ERROR;
+	}
+
 	mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F;
 	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
 	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
@@ -83,14 +104,22 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl
 	mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	mission_item->index = mavlink_mission_item->seq;
+
+	return OK;
 }
 
-void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
 {
-
+	if (mission_item->altitude_is_relative) {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+	} else {
+		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+	}
+	
 	mavlink_mission_item->x = (float)mission_item->lat;
 	mavlink_mission_item->y = (float)mission_item->lon;
 	mavlink_mission_item->z = mission_item->altitude;
+
 	mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
 	mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
 	mavlink_mission_item->command = mission_item->nav_cmd;
@@ -98,6 +127,8 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi
 	mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
 	mavlink_mission_item->autocontinue = mission_item->autocontinue;
 	mavlink_mission_item->seq = mission_item->index;
+
+	return OK;
 }
 
 void mavlink_wpm_init(mavlink_wpm_storage *state)
@@ -111,15 +142,15 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->current_partner_sysid = 0;
 	state->current_partner_compid = 0;
 	state->timestamp_lastaction = 0;
-	state->timestamp_last_send_setpoint = 0;
+	// state->timestamp_last_send_setpoint = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
-	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
-	state->current_active_wp_id = -1;		///< id of current waypoint
-	state->yaw_reached = false;						///< boolean for yaw attitude reached
-	state->pos_reached = false;						///< boolean for position reached
-	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
-	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
+	// state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
+	// state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
+	// state->current_active_wp_id = -1;		///< id of current waypoint
+	// state->yaw_reached = false;						///< boolean for yaw attitude reached
+	// state->pos_reached = false;						///< boolean for position reached
+	// state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
+	// state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
 
 	mission.count = 0;
 	mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED);
@@ -147,16 +178,15 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 
-	if (MAVLINK_WPM_TEXT_FEEDBACK) {
-#ifdef MAVLINK_WPM_NO_PRINTF
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-#else
+// 	if (MAVLINK_WPM_TEXT_FEEDBACK) {
+// #ifdef MAVLINK_WPM_NO_PRINTF
+// 		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
+// #else
 
-		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+// 		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
 
-#endif
-		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
-	}
+// #endif
+// 	}
 }
 
 /*
@@ -206,7 +236,7 @@ void mavlink_wpm_send_setpoint(uint16_t seq)
 				cur->param2, cur->param3, cur->param4, cur->x,
 				cur->y, cur->z, cur->frame, cur->command);
 
-		wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
+		// wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
 
 	} else {
 		if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
@@ -220,7 +250,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 
 	wpc.target_system = wpm->current_partner_sysid;
 	wpc.target_component = wpm->current_partner_compid;
-	wpc.count = count;
+	wpc.count = mission.count;
 
 	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
 	mavlink_missionlib_send_message(&msg);
@@ -292,250 +322,191 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 	// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
 }
 
-/*
- * Calculate distance in global frame.
- *
- * The distance calculation is based on the WGS84 geoid (GPS)
- */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
-{
-
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
-
-		double current_x_rad = wp->x / 180.0 * M_PI;
-		double current_y_rad = wp->y / 180.0 * M_PI;
-		double x_rad = lat / 180.0 * M_PI;
-		double y_rad = lon / 180.0 * M_PI;
-
-		double d_lat = x_rad - current_x_rad;
-		double d_lon = y_rad - current_y_rad;
-
-		double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
-		double c = 2 * atan2(sqrt(a), sqrt(1 - a));
-
-		const double radius_earth = 6371000.0;
-
-		float dxy = radius_earth * c;
-		float dz = alt - wp->z;
-
-		*dist_xy = fabsf(dxy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dxy * dxy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-
-}
-
-/*
- * Calculate distance in local frame (NED)
- */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
-{
-	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
-
-		float dx = (cur->x - x);
-		float dy = (cur->y - y);
-		float dz = (cur->z - z);
-
-		*dist_xy = sqrtf(dx * dx + dy * dy);
-		*dist_z = fabsf(dz);
-
-		return sqrtf(dx * dx + dy * dy + dz * dz);
-
-	} else {
-		return -1.0f;
-	}
-}
-
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
-{
-	static uint16_t counter;
-
-	if ((!global_pos->valid && !local_pos->xy_valid) ||
-		/* no waypoint */
-		wpm->size == 0) {
-		/* nothing to check here, return */
-		return;
-	}
-
-	if (wpm->current_active_wp_id < wpm->size) {
-
-		float orbit;
-		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
-
-		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
-
-			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
-		} else {
-
-			// XXX set default orbit via param
-			orbit = 15.0f;
-		}
-
-		/* keep vertical orbit */
-		float vertical_switch_distance = orbit;
-
-		/* Take the larger turn distance - orbit or turn_distance */
-		if (orbit < turn_distance)
-			orbit = turn_distance;
-
-		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
-		float dist = -1.0f;
-
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
-
-		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
-			/* Check if conditions of mission item are satisfied */
-			// XXX TODO
-		}
-
-		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-			wpm->pos_reached = true;
-		}
 
-		// check if required yaw reached
-		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
-		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
-		if (fabsf(yaw_err) < 0.05f) {
-			wpm->yaw_reached = true;
-		}
-	}
+// void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
+// {
+// 	static uint16_t counter;
 
-	//check if the current waypoint was reached
-	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
-		if (wpm->current_active_wp_id < wpm->size) {
-			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
+// 	if ((!global_pos->valid && !local_pos->xy_valid) ||
+// 		/* no waypoint */
+// 		wpm->size == 0) {
+// 		/* nothing to check here, return */
+// 		return;
+// 	}
 
-			if (wpm->timestamp_firstinside_orbit == 0) {
-				// Announce that last waypoint was reached
-				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
-				wpm->timestamp_firstinside_orbit = now;
-			}
-
-			// check if the MAV was long enough inside the waypoint orbit
-			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-
-			bool time_elapsed = false;
-
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-				time_elapsed = true;
-			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
-				time_elapsed = true;
-			}
+// 	if (wpm->current_active_wp_id < wpm->size) {
 
-			if (time_elapsed) {
+// 		float orbit;
+// 		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
 
-				/* safeguard against invalid missions with last wp autocontinue on */
-				if (wpm->current_active_wp_id == wpm->size - 1) {
-					/* stop handling missions here */
-					cur_wp->autocontinue = false;
-				}
+// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
 
-				if (cur_wp->autocontinue) {
-
-					cur_wp->current = 0;
-
-					float navigation_lat = -1.0f;
-					float navigation_lon = -1.0f;
-					float navigation_alt = -1.0f;
-					int navigation_frame = -1;
-
-					/* initialize to current position in case we don't find a suitable navigation waypoint */
-					if (global_pos->valid) {
-						navigation_lat = global_pos->lat/1e7;
-						navigation_lon = global_pos->lon/1e7;
-						navigation_alt = global_pos->alt;
-						navigation_frame = MAV_FRAME_GLOBAL;
-					} else if (local_pos->xy_valid && local_pos->z_valid) {
-						navigation_lat = local_pos->x;
-						navigation_lon = local_pos->y;
-						navigation_alt = local_pos->z;
-						navigation_frame = MAV_FRAME_LOCAL_NED;
-					}
+// 		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
 
-					/* guard against missions without final land waypoint */
-					/* only accept supported navigation waypoints, skip unknown ones */
-					do {
-
-						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
-						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
-
-							/* this is a navigation waypoint */
-							navigation_frame = cur_wp->frame;
-							navigation_lat = cur_wp->x;
-							navigation_lon = cur_wp->y;
-							navigation_alt = cur_wp->z;
-						}
+// 			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
+// 		} else {
 
-						if (wpm->current_active_wp_id == wpm->size - 1) {
-
-							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
-							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
-								/* the last waypoint was reached, if auto continue is
-								 * activated AND it is NOT a land waypoint, keep the system loitering there.
-								 */
-								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-								cur_wp->frame = navigation_frame;
-								cur_wp->x = navigation_lat;
-								cur_wp->y = navigation_lon;
-								cur_wp->z = navigation_alt;
-							}
+// 			// XXX set default orbit via param
+// 			orbit = 15.0f;
+// 		}
 
-							/* we risk an endless loop for missions without navigation waypoints, abort. */
-							break;
+// 		/* keep vertical orbit */
+// 		float vertical_switch_distance = orbit;
 
-						} else {
-							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-								wpm->current_active_wp_id++;
-						}
+// 		/* Take the larger turn distance - orbit or turn_distance */
+// 		if (orbit < turn_distance)
+// 			orbit = turn_distance;
+
+// 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
+// 		float dist = -1.0f;
+
+// 		float dist_xy = -1.0f;
+// 		float dist_z = -1.0f;
+
+// 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
+// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
+// 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
+// 			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
+
+// 		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
+// 			/* Check if conditions of mission item are satisfied */
+// 			// XXX TODO
+// 		}
+
+// 		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
+// 			wpm->pos_reached = true;
+// 		}
+
+// 		// check if required yaw reached
+// 		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+// 		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+// 		if (fabsf(yaw_err) < 0.05f) {
+// 			wpm->yaw_reached = true;
+// 		}
+// 	}
+
+// 	//check if the current waypoint was reached
+// 	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
+// 		if (wpm->current_active_wp_id < wpm->size) {
+// 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
+
+// 			if (wpm->timestamp_firstinside_orbit == 0) {
+// 				// Announce that last waypoint was reached
+// 				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
+// 				wpm->timestamp_firstinside_orbit = now;
+// 			}
+
+// 			// check if the MAV was long enough inside the waypoint orbit
+// 			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+
+// 			bool time_elapsed = false;
+
+// 			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+// 				time_elapsed = true;
+// 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
+// 				time_elapsed = true;
+// 			}
+
+// 			if (time_elapsed) {
+
+// 				/* safeguard against invalid missions with last wp autocontinue on */
+// 				if (wpm->current_active_wp_id == wpm->size - 1) {
+// 					/* stop handling missions here */
+// 					cur_wp->autocontinue = false;
+// 				}
+
+// 				if (cur_wp->autocontinue) {
+
+// 					cur_wp->current = 0;
+
+// 					float navigation_lat = -1.0f;
+// 					float navigation_lon = -1.0f;
+// 					float navigation_alt = -1.0f;
+// 					int navigation_frame = -1;
+
+// 					/* initialize to current position in case we don't find a suitable navigation waypoint */
+// 					if (global_pos->valid) {
+// 						navigation_lat = global_pos->lat/1e7;
+// 						navigation_lon = global_pos->lon/1e7;
+// 						navigation_alt = global_pos->alt;
+// 						navigation_frame = MAV_FRAME_GLOBAL;
+// 					} else if (local_pos->xy_valid && local_pos->z_valid) {
+// 						navigation_lat = local_pos->x;
+// 						navigation_lon = local_pos->y;
+// 						navigation_alt = local_pos->z;
+// 						navigation_frame = MAV_FRAME_LOCAL_NED;
+// 					}
+
+// 					/* guard against missions without final land waypoint */
+// 					/* only accept supported navigation waypoints, skip unknown ones */
+// 					do {
+
+// 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
+// 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
+
+// 							/* this is a navigation waypoint */
+// 							navigation_frame = cur_wp->frame;
+// 							navigation_lat = cur_wp->x;
+// 							navigation_lon = cur_wp->y;
+// 							navigation_alt = cur_wp->z;
+// 						}
+
+// 						if (wpm->current_active_wp_id == wpm->size - 1) {
+
+// 							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
+// 							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+// 								/* the last waypoint was reached, if auto continue is
+// 								 * activated AND it is NOT a land waypoint, keep the system loitering there.
+// 								 */
+// 								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+// 								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+// 								cur_wp->frame = navigation_frame;
+// 								cur_wp->x = navigation_lat;
+// 								cur_wp->y = navigation_lon;
+// 								cur_wp->z = navigation_alt;
+// 							}
+
+// 							/* we risk an endless loop for missions without navigation waypoints, abort. */
+// 							break;
+
+// 						} else {
+// 							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
+// 								wpm->current_active_wp_id++;
+// 						}
 			
-					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
-
-					// Fly to next waypoint
-					wpm->timestamp_firstinside_orbit = 0;
-					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-					wpm->waypoints[wpm->current_active_wp_id].current = true;
-					wpm->pos_reached = false;
-					wpm->yaw_reached = false;
-					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
-				}
-			}
-		}
-
-	} else {
-		wpm->timestamp_lastoutside_orbit = now;
-	}
-
-	counter++;
-}
+// 					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+// 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
+
+// 					// Fly to next waypoint
+// 					wpm->timestamp_firstinside_orbit = 0;
+// 					mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
+// 					mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+// 					wpm->waypoints[wpm->current_active_wp_id].current = true;
+// 					wpm->pos_reached = false;
+// 					wpm->yaw_reached = false;
+// 					printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
+// 				}
+// 			}
+// 		}
+
+// 	} else {
+// 		wpm->timestamp_lastoutside_orbit = now;
+// 	}
+
+// 	counter++;
+// }
 
 
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
@@ -551,17 +522,17 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 
 #endif
 		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		wpm->current_count = 0;
+		// wpm->current_count = 0;
 		wpm->current_partner_sysid = 0;
 		wpm->current_partner_compid = 0;
-		wpm->current_wp_id = -1;
+		// wpm->current_wp_id = -1;
 
-		if (wpm->size == 0) {
-			wpm->current_active_wp_id = -1;
-		}
+		// if (wpm->size == 0) {
+		// 	wpm->current_active_wp_id = -1;
+		// }
 	}
 
-	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
+	// check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -583,7 +554,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
 					if (wpm->current_wp_id == wpm->size - 1) {
 
-						mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
+						// mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
 
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						wpm->current_wp_id = 0;
@@ -607,25 +578,38 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
 					if (wpc.seq < wpm->size) {
 						// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
-						wpm->current_active_wp_id = wpc.seq;
-						uint32_t i;
+						// wpm->current_active_wp_id = wpc.seq;
+						// uint32_t i;
 
-						for (i = 0; i < wpm->size; i++) {
-							if (i == wpm->current_active_wp_id) {
-								wpm->waypoints[i].current = true;
+						// for (i = 0; i < wpm->size; i++) {
+						// 	if (i == wpm->current_active_wp_id) {
+						// 		wpm->waypoints[i].current = true;
 
-							} else {
-								wpm->waypoints[i].current = false;
-							}
-						}
+						// 	} else {
+						// 		wpm->waypoints[i].current = false;
+						// 	}
+						// }
 
-						mavlink_missionlib_send_gcs_string("NEW WP SET");
+						// mavlink_missionlib_send_gcs_string("NEW WP SET");
 
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
-						mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-						mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-						wpm->timestamp_firstinside_orbit = 0;
+						// wpm->yaw_reached = false;
+						// wpm->pos_reached = false;
+						
+						mission.current_index = wpc.seq;
+
+						/* Initialize mission publication if necessary */
+						if (mission_pub < 0) {
+							mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+						} else {
+							orb_publish(ORB_ID(mission), mission_pub, &mission);
+						}
+						
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+
+						//mavlink_wpm_send_waypoint_current(wpc.seq);
+						// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+						// wpm->timestamp_firstinside_orbit = 0;
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
@@ -720,6 +704,13 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 					mavlink_mission_item_t wp;
 					map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
+
+					if (mission.current_index == wpr.seq) {
+						wp.current = true;
+					} else {
+						wp.current = false;
+					}
+
 					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
 
 				} else {
@@ -848,6 +839,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						} else {
 							/* prepare mission topic */
 							mission.count = wpc.count;
+							/* reset current index */
+							mission.current_index = -1;
 						}
 
 #ifdef MAVLINK_WPM_NO_PRINTF
@@ -880,9 +873,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                            delete waypoints->back();
 //                            waypoints->pop_back();
 //                        }
-						wpm->current_active_wp_id = -1;
-						wpm->yaw_reached = false;
-						wpm->pos_reached = false;
+						// wpm->current_active_wp_id = -1;
+						// wpm->yaw_reached = false;
+						// wpm->pos_reached = false;
 						break;
 
 					} else {
@@ -942,7 +935,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 			mavlink_mission_item_t wp;
 			mavlink_msg_mission_item_decode(msg, &wp);
 
-			mavlink_missionlib_send_gcs_string("GOT WP");
+			// mavlink_missionlib_send_gcs_string("GOT WP");
 //			printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid);
 //			printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid);
 
@@ -956,7 +949,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
 
 				//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
-				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
+				if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) ||
+				    (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id &&
+				    wp.seq < wpm->current_count)) {
 					//mavlink_missionlib_send_gcs_string("DEBUG 2");
 
 //                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
@@ -964,9 +959,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 //                    if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
 //
 					wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-					mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
-					memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
-//                    printf("WP seq: %d\n",wp.seq);
+					// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
+					// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
+
+					int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]);
+					if (ret != OK) {
+
+
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
+						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+						break;
+					}
+
+					if (wp.current) {
+						mission.current_index = wp.seq;
+						warnx("current is: %d", wp.seq);
+					} else {
+						warnx("not current");
+					}
+
 					wpm->current_wp_id = wp.seq + 1;
 
 					// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
@@ -974,23 +985,28 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 //					printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count);
 					if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
+						// mavlink_missionlib_send_gcs_string("GOT ALL WPS");
 						// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count);
 
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
 
-						if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
-							wpm->current_active_wp_id = wpm->rcv_size - 1;
-						}
+						// if (wpm->current_active_wp_id > wpm->rcv_size - 1) {
+						// 	wpm->current_active_wp_id = wpm->rcv_size - 1;
+						// }
 
-						// switch the waypoints list
-						// FIXME CHECK!!!
-						uint32_t i;
+						// bool copy_error = false;
 
-						for (i = 0; i < wpm->current_count; ++i) {
-							wpm->waypoints[i] = wpm->rcv_waypoints[i];
-							map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]);
-						}
+						// // switch the waypoints list
+						// // FIXME CHECK!!!
+						// uint32_t i;
+
+						// for (i = 0; i < wpm->current_count; ++i) {
+						// 	wpm->waypoints[i] = wpm->rcv_waypoints[i];
+						// 	if (map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]) != OK) {
+						// 		copy_error = true;
+						// 	}
+
+						// }
 						// TODO: update count?
 
 						/* Initialize mission publication if necessary */
@@ -1006,25 +1022,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 						//get the new current waypoint
 
-						for (i = 0; i < wpm->size; i++) {
-							if (wpm->waypoints[i].current == 1) {
-								wpm->current_active_wp_id = i;
-								//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
-								wpm->yaw_reached = false;
-								wpm->pos_reached = false;
-								mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
-								mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
-								wpm->timestamp_firstinside_orbit = 0;
-								break;
-							}
-						}
-
-						if (i == wpm->size) {
-							wpm->current_active_wp_id = -1;
-							wpm->yaw_reached = false;
-							wpm->pos_reached = false;
-							wpm->timestamp_firstinside_orbit = 0;
-						}
+						// for (i = 0; i < wpm->size; i++) {
+						// 	if (wpm->waypoints[i].current == 1) {
+						// 		wpm->current_active_wp_id = i;
+						// 		//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
+						// 		// wpm->yaw_reached = false;
+						// 		// wpm->pos_reached = false;
+						// 		mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
+						// 		// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
+						// 		// wpm->timestamp_firstinside_orbit = 0;
+						// 		break;
+						// 	}
+						// }
+
+						// if (i == wpm->size) {
+						// 	wpm->current_active_wp_id = -1;
+						// 	wpm->yaw_reached = false;
+						// 	wpm->pos_reached = false;
+						// 	wpm->timestamp_firstinside_orbit = 0;
+						// }
 
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 
@@ -1033,38 +1049,41 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					}
 
 				} else {
-					if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-						//we're done receiving waypoints, answer with ack.
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
-						printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
-					}
 
-					// if (verbose)
-					{
-						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
-							break;
-
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-							if (!(wp.seq == 0)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-							if (!(wp.seq == wpm->current_wp_id)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
-								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-							} else if (!(wp.seq < wpm->current_count)) {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
-							} else {
-//								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-							}
-						} else {
-//							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
-						}
-					}
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_INVALID_SEQUENCE);
+
+					// if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
+					// 	//we're done receiving waypoints, answer with ack.
+					// 	mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
+					// 	printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
+					// }
+
+// 					// if (verbose)
+// 					{
+// 						if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
+// //							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
+// 							break;
+
+// 						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
+// 							if (!(wp.seq == 0)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
+// 							} else {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 							}
+// 						} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+// 							if (!(wp.seq == wpm->current_wp_id)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
+// 								mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
+
+// 							} else if (!(wp.seq < wpm->current_count)) {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+// 							} else {
+// //								 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 							}
+// 						} else {
+// //							 printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+// 						}
+// 					}
 				}
 			} else {
 				//we we're target but already communicating with someone else
@@ -1088,9 +1107,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 				// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
 				// Delete all waypoints
 				wpm->size = 0;
-				wpm->current_active_wp_id = -1;
-				wpm->yaw_reached = false;
-				wpm->pos_reached = false;
+				// wpm->current_active_wp_id = -1;
+				// wpm->yaw_reached = false;
+				// wpm->pos_reached = false;
 
 				/* prepare mission topic */
 				mission.count = 0;
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index d7d6b31dc..fc68285e9 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -56,6 +56,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/mission.h>
 
 // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -81,7 +82,7 @@ enum MAVLINK_WPM_CODES {
 /* WAYPOINT MANAGER - MISSION LIB */
 
 #define MAVLINK_WPM_MAX_WP_COUNT 15
-#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
+// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
 #ifndef MAVLINK_WPM_TEXT_FEEDBACK
 #define MAVLINK_WPM_TEXT_FEEDBACK 0						  ///< Report back status information as text
 #endif
@@ -92,33 +93,37 @@ enum MAVLINK_WPM_CODES {
 
 struct mavlink_wpm_storage {
 	mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
-#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
-	mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
-#endif
+// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
+	// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
+// #endif
 	uint16_t size;
 	uint16_t max_size;
 	uint16_t rcv_size;
 	enum MAVLINK_WPM_STATES current_state;
 	int16_t current_wp_id;							///< Waypoint in current transmission
-	int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
+	// int16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
 	uint16_t current_count;
 	uint8_t current_partner_sysid;
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_firstinside_orbit;
-	uint64_t timestamp_lastoutside_orbit;
+	// uint64_t timestamp_last_send_setpoint;
+	// uint64_t timestamp_firstinside_orbit;
+	// uint64_t timestamp_lastoutside_orbit;
 	uint32_t timeout;
-	uint32_t delay_setpoint;
-	float accept_range_yaw;
-	float accept_range_distance;
-	bool yaw_reached;
-	bool pos_reached;
-	bool idle;
+	// uint32_t delay_setpoint;
+	// float accept_range_yaw;
+	// float accept_range_distance;
+	// bool yaw_reached;
+	// bool pos_reached;
+	// bool idle;
 };
 
 typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
+int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
+int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
+
+
 void mavlink_wpm_init(mavlink_wpm_storage *state);
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
 			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 2e8844801..b88fc804c 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -58,6 +58,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
@@ -144,11 +145,13 @@ private:
 
 	orb_advert_t	_triplet_pub;			/**< publish position setpoint triplet */
 	orb_advert_t	_fence_pub;			/**< publish fence topic */ 
+	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct home_position_s				_home_pos;		/**< home position for RTL */
 	struct mission_item_triplet_s			_mission_item_triplet;	/**< triplet of mission items */
+	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -215,12 +218,16 @@ private:
 
 	void		publish_mission_item_triplet();
 
+	void		publish_mission_result();
+
 	int		advance_current_mission_item();
 
 	void		reset_mission_item_reached();
 
 	void		check_mission_item_reached();
 
+	void		report_mission_reached();
+
 	void		start_waypoint();
 
 	void		start_loiter(mission_item_s *new_loiter_position);
@@ -266,6 +273,7 @@ Navigator::Navigator() :
 /* publications */
 	_triplet_pub(-1),
 	_fence_pub(-1),
+	_mission_result_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -295,6 +303,8 @@ Navigator::Navigator() :
 	memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s));
 	memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
 
+	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+
 	/* fetch initial values */
 	parameters_update();
 }
@@ -348,7 +358,7 @@ Navigator::mission_update()
 			 * if the first part changed: start again at first waypoint
 			 * if the first part remained unchanged: continue with the (possibly changed second part)
 			 */
-			if (_current_mission_index  < _mission_item_count && _current_mission_index <  mission.count) { //check if not finished and if the new mission is not a shorter mission
+			if (mission.current_index == -1 && _current_mission_index  < _mission_item_count && _current_mission_index <  mission.count) { //check if not finished and if the new mission is not a shorter mission
 				for (unsigned i = 0; i < _current_mission_index; i++) {
 					if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
 						/* set flag to restart mission next we're in auto */
@@ -361,8 +371,11 @@ Navigator::mission_update()
 //						warnx("Mission item is equivalent i=%d", i);
 //					}
 				}
-			} else {
+			} else if (mission.current_index >= 0 && mission.current_index < mission.count) {
 				/* set flag to restart mission next we're in auto */
+				_current_mission_index = mission.current_index;
+				mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
+			} else {
 				_current_mission_index = 0;
 				mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
 			}
@@ -586,6 +599,9 @@ Navigator::task_main()
 					check_mission_item_reached();
 
 					if (_mission_item_reached) {
+
+						report_mission_reached();
+
 						mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index);
 						if (advance_current_mission_item() != OK) {
 							set_mode(NAVIGATION_MODE_LOITER_WAYPOINT);
@@ -896,6 +912,20 @@ Navigator::publish_mission_item_triplet()
 	}
 }
 
+void
+Navigator::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish the 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);
+	}
+}
+
 int
 Navigator::advance_current_mission_item()
 {
@@ -945,6 +975,9 @@ Navigator::reset_mission_item_reached()
 	_time_first_inside_orbit = 0;
 
 	_mission_item_reached = false;
+
+	_mission_result.mission_reached = false;
+	_mission_result.mission_index = 0;
 }
 
 void
@@ -1042,6 +1075,15 @@ Navigator::check_mission_item_reached()
 }
 
 void
+Navigator::report_mission_reached()
+{
+	_mission_result.mission_reached = true;
+	_mission_result.mission_index = _current_mission_index;
+
+	publish_mission_result();
+}
+
+void
 Navigator::start_waypoint()
 {
 	reset_mission_item_reached();
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ecc1a3b3a..e73e7ff8d 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -129,6 +129,9 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
 
+#include "topics/mission_result.h"
+ORB_DEFINE(mission_result, struct mission_result_s);
+
 #include "topics/fence.h"
 ORB_DEFINE(fence, unsigned);
 
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 441efe458..56350d349 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -90,6 +90,7 @@ struct mission_s
 {
 	struct mission_item_s *items;
 	unsigned count;
+	int current_index;		/**< default -1, start at the one changed latest */
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
new file mode 100644
index 000000000..c99706b97
--- /dev/null
+++ b/src/modules/uORB/topics/mission_result.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mission_result.h
+ * Mission results that navigator needs to pass on to commander and mavlink.
+ */
+
+#ifndef TOPIC_MISSION_RESULT_H
+#define TOPIC_MISSION_RESULT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct mission_result_s
+{
+	bool mission_reached;		/**< true if mission has been reached			*/
+	unsigned mission_index;		/**< index of the mission which has been reached	*/
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(mission_result);
+
+#endif