diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index ac6d5cd065..2ff9d0eb56 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -378,6 +378,10 @@ uint8_t GCS_MAVLINK_Sub::sysid_my_gcs() const
     return sub.g.sysid_my_gcs;
 }
 
+bool GCS_MAVLINK_Sub::vehicle_initialised() const {
+    return sub.ap.initialised;
+}
+
 // try to send a message, return false if it won't fit in the serial tx buffer
 bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
 {
@@ -399,17 +403,14 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
         send_info();
         break;
 
-    case MSG_EXTENDED_STATUS1:
+    case MSG_SYS_STATUS:
         // send extended status only once vehicle has been initialised
         // to avoid unnecessary errors being reported to user
-        if (sub.ap.initialised) {
-            if (PAYLOAD_SIZE(chan, SYS_STATUS) +
-                PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
-                return false;
-            }
-            sub.send_sys_status(chan);
-            send_power_status();
+        if (!vehicle_initialised()) {
+            return true;
         }
+        CHECK_PAYLOAD_SIZE(SYS_STATUS);
+        sub.send_sys_status(chan);
         break;
 
     case MSG_NAV_CONTROLLER_OUTPUT:
@@ -536,7 +537,8 @@ static const ap_message STREAM_RAW_SENSORS_msgs[] = {
     MSG_SENSOR_OFFSETS
 };
 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
-    MSG_EXTENDED_STATUS1, // SYS_STATUS, POWER_STATUS
+    MSG_SYS_STATUS,
+    MSG_POWER_STATUS,
     MSG_MEMINFO,
     MSG_CURRENT_WAYPOINT,
     MSG_GPS_RAW,
@@ -1146,7 +1148,7 @@ void Sub::mavlink_delay_cb()
     if (tnow - last_1hz > 1000) {
         last_1hz = tnow;
         gcs_send_heartbeat();
-        gcs().send_message(MSG_EXTENDED_STATUS1);
+        gcs().send_message(MSG_SYS_STATUS);
     }
     if (tnow - last_50hz > 20) {
         last_50hz = tnow;
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index daf29bd834..ca66f6557c 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -33,6 +33,8 @@ protected:
     int32_t global_position_int_alt() const override;
     int32_t global_position_int_relative_alt() const override;
 
+    bool vehicle_initialised() const override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;