Sub: send both SYS_STATUS or POWER_STATUS or neither

ArduSub/GCS_Mavlink.cpp

@@ -403,9 +403,11 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
         // send extended status only once vehicle has been initialised
         // to avoid unnecessary errors being reported to user
         if (sub.ap.initialised) {
-            CHECK_PAYLOAD_SIZE(SYS_STATUS);
+            if (PAYLOAD_SIZE(chan, SYS_STATUS) +
+                PAYLOAD_SIZE(chan, POWER_STATUS) > comm_get_txspace(chan)) {
+                return false;
+            }
             sub.send_sys_status(chan);
-            CHECK_PAYLOAD_SIZE(POWER_STATUS);
             send_power_status();
         }
         break;