diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index ebfc33f4a5a090..9a8420edba53d7 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -427,4 +427,54 @@ bool Sub::ensure_ekf_origin()
     return true;
 }
 
+// get the altitude relative to the home position or the ekf origin
+float Sub::get_alt_rel() const
+{
+    if (!ap.depth_sensor_present) {
+        return 0;
+    }
+
+    // get relative position
+    float posD;
+    if (ahrs.get_relative_position_D_origin(posD)) {
+        if (ahrs.home_is_set()) {
+            // adjust to the home position
+            auto home = ahrs.get_home();
+            posD -= static_cast<float>(home.alt) * 0.01f;
+        }
+    } else {
+        // fall back to the barometer reading
+        posD = -AP::baro().get_altitude();
+    }
+
+    // convert down to up
+    return -posD;
+}
+
+// get the altitude above mean sea level
+float Sub::get_alt_msl() const
+{
+    if (!ap.depth_sensor_present) {
+        return 0;
+    }
+
+    Location origin;
+    if (!ahrs.get_origin(origin)) {
+        return 0;
+    }
+
+    // get relative position
+    float posD;
+    if (!ahrs.get_relative_position_D_origin(posD)) {
+        // fall back to the barometer reading
+        posD = -AP::baro().get_altitude();
+    }
+
+    // add in the ekf origin altitude
+    posD -= static_cast<float>(origin.alt) * 0.01f;
+
+    // convert down to up
+    return -posD;
+}
+
 AP_HAL_MAIN_CALLBACKS(&sub);
diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 4ae448ac34c469..3ccbe543bbb797 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -93,6 +93,11 @@ int16_t GCS_MAVLINK_Sub::vfr_hud_throttle() const
     return (int16_t)(sub.motors.get_throttle() * 100);
 }
 
+float GCS_MAVLINK_Sub::vfr_hud_alt() const
+{
+    return sub.get_alt_msl();
+}
+
 // Work around to get temperature sensor data out
 void GCS_MAVLINK_Sub::send_scaled_pressure3()
 {
@@ -845,7 +850,8 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const
         );
 }
 
-MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet) {
+MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet)
+{
     if (packet.param1 > 0.5f) {
         sub.arming.disarm(AP_Arming::Method::TERMINATION);
         return MAV_RESULT_ACCEPTED;
@@ -853,17 +859,14 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_
     return MAV_RESULT_FAILED;
 }
 
-int32_t GCS_MAVLINK_Sub::global_position_int_alt() const {
-    if (!sub.ap.depth_sensor_present) {
-        return 0;
-    }
-    return GCS_MAVLINK::global_position_int_alt();
+int32_t GCS_MAVLINK_Sub::global_position_int_alt() const
+{
+    return static_cast<int32_t>(sub.get_alt_msl() * 1000.0f);
 }
-int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const {
-    if (!sub.ap.depth_sensor_present) {
-        return 0;
-    }
-    return GCS_MAVLINK::global_position_int_relative_alt();
+
+int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const
+{
+    return static_cast<int32_t>(sub.get_alt_rel() * 1000.0f);
 }
 
 #if HAL_HIGH_LATENCY2_ENABLED
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 4b49f2246e64ca..f02c782cd0c66c 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -51,6 +51,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK {
     MAV_STATE vehicle_system_status() const override;
 
     int16_t vfr_hud_throttle() const override;
+    float vfr_hud_alt() const override;
 
     MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet);
     MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet);
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index b201aebfe732bd..1116bbb669ed1a 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -428,6 +428,8 @@ class Sub : public AP_Vehicle {
     bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
     bool far_from_EKF_origin(const Location& loc);
+    float get_alt_rel() const WARN_IF_UNUSED;
+    float get_alt_msl() const WARN_IF_UNUSED;
     void exit_mission();
     void set_origin(const Location& loc);
     bool ensure_ekf_origin();