From feddaabc428be9c9875b16163820e359e6fae892 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 26 Apr 2019 14:44:34 +1000
Subject: [PATCH] GCS_MAVLink: handle out-of-time to send messages in parent
 class

---
 libraries/GCS_MAVLink/GCS.cpp        | 22 ++++++++++++++++++++++
 libraries/GCS_MAVLink/GCS.h          | 15 +++++++--------
 libraries/GCS_MAVLink/GCS_Common.cpp |  1 -
 3 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp
index d7b84c847f..035e8787c7 100644
--- a/libraries/GCS_MAVLink/GCS.cpp
+++ b/libraries/GCS_MAVLink/GCS.cpp
@@ -1,6 +1,9 @@
 #include "GCS.h"
+
+#include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -179,3 +182,22 @@ void GCS::update_sensor_status_flags()
 
     update_vehicle_sensor_status_flags();
 }
+
+bool GCS::out_of_time() const
+{
+    // while we are in the delay callback we are never out of time:
+    if (hal.scheduler->in_delay_callback()) {
+        return false;
+    }
+
+    // we always want to be able to send messages out while in the error loop:
+    if (AP_BoardConfig::in_sensor_config_error()) {
+        return false;
+    }
+
+    if (min_loop_time_remaining_for_message_send_us() <= AP::scheduler().time_available_usec()) {
+        return false;
+    }
+
+    return true;
+}
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index f2403ea48a..6b0921b48a 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -966,12 +966,14 @@ public:
     void update_receive();
     virtual void setup_uarts();
 
-    bool out_of_time() const {
-        return _out_of_time;
-    }
-    void set_out_of_time(bool val) {
-        _out_of_time = val;
+    // minimum amount of time (in microseconds) that must remain in
+    // the main scheduler loop before we are allowed to send any
+    // mavlink messages.  We want to prioritise the main flight
+    // control loop over communications
+    virtual uint16_t min_loop_time_remaining_for_message_send_us() const {
+        return 200;
     }
+    bool out_of_time() const;
 
     // frsky backend
     AP_Frsky_Telem frsky;
@@ -1024,9 +1026,6 @@ private:
     // queue of outgoing statustext messages
     ObjectArray<statustext_t> _statustext_queue{_status_capacity};
 
-    // true if we are running short on time in our main loop
-    bool _out_of_time;
-
     // true if we have already allocated protocol objects:
     bool initialised_missionitemprotocol_objects;
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index e5f414c128..7e5c76a3c1 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1438,7 +1438,6 @@ void GCS_MAVLINK::update_send()
 #endif
 
     const uint32_t start = AP_HAL::millis();
-    gcs().set_out_of_time(false);
     while (AP_HAL::millis() - start < 5) { // spend a max of 5ms sending messages.  This should never trigger - out_of_time() should become true
         if (gcs().out_of_time()) {
             break;