diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index cf49f4522f..f3eb2ac7f2 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -972,6 +972,21 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
     }     // end switch
 } // end handle mavlink
 
+uint64_t GCS_MAVLINK_Sub::capabilities() const
+{
+    return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
+            MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
+            MAV_PROTOCOL_CAPABILITY_MISSION_INT |
+            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
+            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
+            MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
+#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
+            (sub.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
+#endif
+            MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
+            GCS_MAVLINK::capabilities()
+        );
+}
 
 // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
 void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t *msg)
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 0f27935d52..78d4eb7eac 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -36,6 +36,7 @@ protected:
     bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
 
     void send_nav_controller_output() const override;
+    uint64_t capabilities() const override;
 
 private:
 
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 6044803b1e..1631dc1bde 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -660,7 +660,6 @@ private:
 
     void auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
     void log_init(void);
-    void init_capabilities(void);
     void accel_cal_update(void);
 
     void failsafe_leak_check();
diff --git a/ArduSub/capabilities.cpp b/ArduSub/capabilities.cpp
deleted file mode 100644
index c7f07cf516..0000000000
--- a/ArduSub/capabilities.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "Sub.h"
-
-void Sub::init_capabilities()
-{
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_INT);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION);
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET);
-#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
-    hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
-#endif
-}
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index feb385d424..84b7110742 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -27,8 +27,6 @@ void Sub::init_ardupilot()
                         AP::fwversion().fw_string,
                         (unsigned)hal.util->available_memory());
 
-    init_capabilities();
-
     // load parameters from EEPROM
     load_parameters();