From 4d21630bf0dbf54a71cfd4b4fc1e35fd44066c21 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 4 Dec 2018 16:08:20 +1100
Subject: [PATCH] Sub: send both SYS_STATUS or POWER_STATUS or neither

---
 ArduSub/GCS_Mavlink.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 210ad73243..16ca413aad 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/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;