diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
index a1fb5062d2..a23a25bcf6 100644
--- a/libraries/AP_AHRS/AP_AHRS.cpp
+++ b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -41,11 +41,11 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
     AP_GROUPINFO("GPS_GAIN",  2, AP_AHRS, gps_gain, 1.0f),
 
     // @Param: GPS_USE
-    // @DisplayName: AHRS use GPS for navigation
-    // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available.
-    // @Values: 0:Disabled,1:Enabled
+    // @DisplayName: AHRS DCM use GPS for navigation
+    // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS whenever it is available. A value of 2 means to use GPS for height as well as position in DCM.
+    // @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
     // @User: Advanced
-    AP_GROUPINFO("GPS_USE",  3, AP_AHRS, _gps_use, 1),
+    AP_GROUPINFO("GPS_USE",  3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
 
     // @Param: YAW_P
     // @DisplayName: Yaw P
diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index 9688d54b6c..b5940a12f5 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -631,7 +631,14 @@ protected:
     AP_Float gps_gain;
 
     AP_Float beta;
-    AP_Int8 _gps_use;
+
+    enum class GPSUse : uint8_t {
+        Disable = 0,
+        Enable  = 1,
+        EnableWithHeight = 2,
+    };
+
+    AP_Enum<GPSUse> _gps_use;
     AP_Int8 _wind_max;
     AP_Int8 _board_orientation;
     AP_Int8 _gps_minsats;
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 4d1e73af1c..e1c1ff10e9 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -414,7 +414,7 @@ AP_AHRS_DCM::_yaw_gain(void) const
 // return true if we have and should use GPS
 bool AP_AHRS_DCM::have_gps(void) const
 {
-    if (AP::gps().status() <= AP_GPS::NO_FIX || !_gps_use) {
+    if (AP::gps().status() <= AP_GPS::NO_FIX || _gps_use == GPSUse::Disable) {
         return false;
     }
     return true;
@@ -1031,15 +1031,21 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
 {
     loc.lat = _last_lat;
     loc.lng = _last_lng;
-    loc.alt = AP::baro().get_altitude() * 100 + _home.alt;
+    const auto &baro = AP::baro();
+    const auto &gps = AP::gps();
+    if (_gps_use == GPSUse::EnableWithHeight &&
+        gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        loc.alt = gps.location().alt;
+    } else {
+        loc.alt = baro.get_altitude() * 100 + _home.alt;
+    }
     loc.relative_alt = 0;
     loc.terrain_alt = 0;
     loc.offset(_position_offset_north, _position_offset_east);
-    const AP_GPS &_gps = AP::gps();
     if (_flags.fly_forward && _have_position) {
         float gps_delay_sec = 0;
-        _gps.get_lag(gps_delay_sec);
-        loc.offset_bearing(_gps.ground_course(), _gps.ground_speed() * gps_delay_sec);
+        gps.get_lag(gps_delay_sec);
+        loc.offset_bearing(gps.ground_course(), gps.ground_speed() * gps_delay_sec);
     }
     return _have_position;
 }
@@ -1125,7 +1131,13 @@ bool AP_AHRS_DCM::set_home(const Location &loc)
 //  a relative ground position to home in meters, Down
 void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const
 {
-    posD = -AP::baro().get_altitude();
+    const auto &gps = AP::gps();
+    if (_gps_use == GPSUse::EnableWithHeight &&
+        gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        posD = (_home.alt - gps.location().alt) * 0.01;
+    } else {
+        posD = -AP::baro().get_altitude();
+    }
 }
 
 /*
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index b9eed933c6..2049309c93 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -1416,7 +1416,7 @@ void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const
     float originD;
     if (!get_relative_position_D_origin(originD) ||
         !get_origin(originLLH)) {
-        posD = -AP::baro().get_altitude();
+        AP_AHRS_DCM::get_relative_position_D_home(posD);
         return;
     }