diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b19..becaa09e83 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -223,7 +223,7 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 }
 
-void
-Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-		                   struct position_setpoint_s *previous_pos_sp)
-{
-	/* reuse current setpoint as previous setpoint */
-	if (current_pos_sp->valid) {
-		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
-	}
-}
-
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96af..842c8c9ee8 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -106,12 +106,6 @@ private:
 	 */
 	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	/**
-	 * Set previous position setpoint
-	 */
-	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-			               struct position_setpoint_s *previous_pos_sp);
-
 	/**
 	 * Try to set the current position setpoint from an onboard mission item
 	 * @return true if mission item successfully set
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 08576750c9..a8eec92ca6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	}
 }
 
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
 bool
 MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 47e6fe81fe..a740deea48 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -82,6 +82,11 @@ public:
 	 */
 	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *