From f71674ffa4506b676bbc63512304298c862ac638 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 18 Sep 2011 18:03:51 +1000
Subject: [PATCH] GCS: don't send low priority messages unless they fit

check the serial transmit buffer, and don't send low priority messages
unless they can go straight out without blocking the CPU
---
 ArduPlane/GCS_Mavlink.pde | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 3bb1d91d83..43d4256501 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -464,6 +464,14 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char
         // bootup, to try to prevent Xbee bricking
         return;
     }
+
+    if (severity == SEVERITY_LOW &&
+        comm_get_txspace(chan) < MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
+        // don't send low priority messages unless they immediately
+        // fit in the serial buffer
+        return;
+    }
+
 	mavlink_msg_statustext_send(
 				chan,
 				severity,