Navigator, waypoints: save index in mission item and use this in navigator

src/modules/uORB/topics/mission.h

@@ -80,6 +80,7 @@ struct mission_item_s
 	float radius;			/**< radius in which the mission is accepted as reached in meters */
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
+	int index;			/**< index matching the mavlink waypoint                */
 };
 
 struct mission_s

src/modules/uORB/topics/mission_item_triplet.h

@@ -67,6 +67,10 @@ struct mission_item_triplet_s
 	struct mission_item_s previous;
 	struct mission_item_s current;
 	struct mission_item_s next;
+
+	int previous_index;
+	int current_index;
+	int next_index;
 };
 
 /**