diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index a671918c2e..a7b6871dde 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -645,6 +645,7 @@ Mission::set_mission_items()
 		/* yaw is aligned now */
 		if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+			set_previous_pos_setpoint();
 		}
 
 	}