From 342e502da452ecdbc771debe592ee3ee4c4ee07f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 16 Jan 2020 14:04:41 +1100
Subject: [PATCH] AP_vehicle: let AP_Vehicle base class worry about scheduler
 delay callback

---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 43 +++++++++++++++++++++++++++++
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 +++
 2 files changed, 47 insertions(+)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 0aad5f76a5..434ea4b4c3 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -54,6 +54,49 @@ void AP_Vehicle::init_vehicle()
 #endif
 }
 
+
+/*
+ *  a delay() callback that processes MAVLink packets. We set this as the
+ *  callback in long running library initialisation routines to allow
+ *  MAVLink to process packets while waiting for the initialisation to
+ *  complete
+ */
+void AP_Vehicle::scheduler_delay_callback()
+{
+    static uint32_t last_1hz, last_50hz, last_5s;
+
+    AP_Logger &logger = AP::logger();
+
+    // don't allow potentially expensive logging calls:
+    logger.EnableWrites(false);
+
+    const uint32_t tnow = AP_HAL::millis();
+    if (tnow - last_1hz > 1000) {
+        last_1hz = tnow;
+        gcs().send_message(MSG_HEARTBEAT);
+        gcs().send_message(MSG_SYS_STATUS);
+    }
+    if (tnow - last_50hz > 20) {
+        last_50hz = tnow;
+        gcs().update_receive();
+        gcs().update_send();
+        _singleton->notify.update();
+    }
+    if (tnow - last_5s > 5000) {
+        last_5s = tnow;
+        gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
+    }
+
+    logger.EnableWrites(true);
+}
+
+void AP_Vehicle::register_scheduler_delay_callback()
+{
+    // Register scheduler_delay_cb, which will run anytime you have
+    // more than 5ms remaining in your call to hal.scheduler->delay
+    hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
+}
+
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 25dfede9e1..8a5f45d5b0 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -197,11 +197,15 @@ protected:
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Scheduler::Task scheduler_tasks[];
 
+    void register_scheduler_delay_callback();
+
 private:
 
     static AP_Vehicle *_singleton;
     bool init_done;
 
+    static void scheduler_delay_callback();
+
     // true if vehicle is probably flying
     bool likely_flying;
     // time when likely_flying last went true