finish moving to waypoint after a back transition

src/modules/navigator/mission.cpp

@@ -501,6 +501,31 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 		}
 
+		/* don't advance mission after FW to MC command */
+		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+				&& _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE
+				&& !_navigator->get_vstatus()->is_rotary_wing
+				&& !_navigator->get_vstatus()->condition_landed
+				&& pos_sp_triplet->previous.valid) {
+
+			new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
+		}
+
+		/* after FW to MC transition finish moving to the waypoint */
+		if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE
+				&& pos_sp_triplet->previous.valid) {
+
+			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.lat = pos_sp_triplet->previous.lat;
+			_mission_item.lon = pos_sp_triplet->previous.lon;
+			_mission_item.altitude = pos_sp_triplet->previous.alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
+		}
+
 	}
 
 	/*********************************** set setpoints and check next *********************************************/

src/modules/navigator/mission.h

@@ -233,7 +233,8 @@ private:
 		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
 		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
-		WORK_ITEM_TYPE_ALIGN		/**< align for next waypoint */
+		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
+		WORK_ITEM_TYPE_CMD_BEFORE_MOVE	/**<  */
 	} _work_item_type;	/**< current type of work to do (sub mission item) */
 
 };