diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
index 3933e931a8..6c00ab39b5 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
@@ -188,13 +188,15 @@ void FlightTask::_evaluateVehicleLocalPositionSetpoint()
 void FlightTask::_evaluateDistanceToGround()
 {
 	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
-	_dist_to_ground = -_position(2);
-
 	if (PX4_ISFINITE(_dist_to_bottom)) {
 		_dist_to_ground = _dist_to_bottom;
 
 	} else if (_sub_home_position.get().valid_alt) {
 		_dist_to_ground = -(_position(2) - _sub_home_position.get().z);
+
+	} else {
+		_dist_to_ground = -_position(2);
+
 	}
 }