diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 13fcd5daca..1d47167a04 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -271,6 +271,16 @@ Mission::advance_mission()
 	}
 }
 
+int
+Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
+{
+	if (_mission_item.altitude_is_relative) {
+		return _mission_item.altitude + _navigator->get_home_position()->alt;
+	} else {
+		return _mission_item.altitude;
+	}
+}
+
 bool
 Mission::check_dist_1wp()
 {
@@ -354,7 +364,7 @@ Mission::set_mission_items()
 
 	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
 	if (pos_sp_triplet->previous.valid) {
-		_mission_item_previous_alt = _mission_item.altitude;
+		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
 	}
 
 	/* get home distance state */
@@ -440,10 +450,7 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
 
 		/* calculate takeoff altitude */
-		float takeoff_alt = _mission_item.altitude;
-		if (_mission_item.altitude_is_relative) {
-			takeoff_alt += _navigator->get_home_position()->alt;
-		}
+		float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
 
 		/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 		if (_navigator->get_vstatus()->condition_landed) {
@@ -609,14 +616,14 @@ Mission::altitude_sp_foh_update()
 	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
 	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
 	if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
-		pos_sp_triplet->current.alt = _mission_item.altitude;
+		pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
 		* of the mission item as it is used to check if the mission item is reached
 		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
 		* radius around the current waypoint
 		**/
-		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
+		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
 		float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
 		float a = _mission_item_previous_alt - grad * _distance_current_previous;
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index e9f78e8fdc..bc9a2c6c82 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -132,6 +132,8 @@ private:
 	 */
 	void altitude_sp_foh_reset();
 
+	int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
+
 	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful