GCS_MAVLink: move data stream send up

libraries/GCS_MAVLink/GCS.h

@@ -85,6 +85,15 @@ enum ap_message {
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };
 
+// convenience macros for defining which ap_message ids are in which streams:
+#define MAV_STREAM_ENTRY(stream_name)           \
+    {                                           \
+        GCS_MAVLINK::stream_name,               \
+        stream_name ## _msgs,                   \
+        ARRAY_SIZE(stream_name ## _msgs)        \
+    }
+#define MAV_STREAM_TERMINATOR { 0, nullptr, 0 }
+
 ///
 /// @class	GCS_MAVLINK
 /// @brief	MAVLink transport control class
@@ -99,7 +108,7 @@ public:
     void        setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance);
     void        send_message(enum ap_message id);
     void        send_text(MAV_SEVERITY severity, const char *fmt, ...);
-    virtual void        data_stream_send(void) = 0;
+    void        data_stream_send();
     void        queued_param_send();
     void        queued_waypoint_send();
     // packetReceived is called on any successful decode of a mavlink message
@@ -218,9 +227,19 @@ public:
 
     // alternative protocol function handler
     FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
-    
+
+    struct stream_entries {
+        const uint8_t stream_id; // narrowed from uint32_t (enumeration)
+        const uint8_t *ap_message_ids; // narrowed from uint32_t (enumeration)
+        const uint8_t num_ap_message_ids;
+    };
+    // vehicle subclass cpp files should define this:
+    static const struct stream_entries all_stream_entries[];
+
 protected:
 
+    virtual bool in_hil_mode() const { return false; }
+
     // overridable method to check for packet acceptance. Allows for
     // enforcement of GCS sysid
     virtual bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) { return true; }

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -2713,6 +2713,58 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     return ret;
 }
 
+void GCS_MAVLINK::data_stream_send(void)
+{
+    if (waypoint_receiving) {
+        // don't interfere with mission transfer
+        return;
+    }
+
+    if (!hal.scheduler->in_delay_callback()) {
+        // DataFlash_Class will not send log data if we are armed.
+        DataFlash_Class::instance()->handle_log_send();
+    }
+
+    gcs().set_out_of_time(false);
+
+    send_queued_parameters();
+
+    if (gcs().out_of_time()) return;
+
+    if (hal.scheduler->in_delay_callback()) {
+        if (in_hil_mode()) {
+            // in HIL we need to keep sending servo values to ensure
+            // the simulator doesn't pause, otherwise our sensor
+            // calibration could stall
+            if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+                send_message(MSG_SERVO_OUT);
+            }
+            if (stream_trigger(STREAM_RC_CHANNELS)) {
+                send_message(MSG_SERVO_OUTPUT_RAW);
+            }
+        }
+        // send no other streams while in delay, just in case they
+        // take way too long to run
+        return;
+    }
+
+    for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
+        const streams id = (streams)all_stream_entries[i].stream_id;
+        if (!stream_trigger(id)) {
+            continue;
+        }
+        const uint8_t *msg_ids = all_stream_entries[i].ap_message_ids;
+        for (uint8_t j=0; j<all_stream_entries[i].num_ap_message_ids; j++) {
+            const uint8_t msg_id = msg_ids[j];
+            send_message((ap_message)msg_id);
+        }
+        if (gcs().out_of_time()) {
+            break;
+        }
+    }
+}
+
+
 GCS &gcs()
 {
     return *GCS::instance();

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -14,7 +14,6 @@ const AP_FWVersion fwver
  */
 class GCS_MAVLINK_Dummy : public GCS_MAVLINK
 {
-    void data_stream_send(void) override {}
     uint32_t telem_delay() const override { return 0; }
     void handleMessage(mavlink_message_t * msg) override {}
     bool try_send_message(enum ap_message id) { return true; }

libraries/GCS_MAVLink/examples/routing/routing.cpp

@@ -26,8 +26,6 @@ class GCS_MAVLINK_routing : public GCS_MAVLINK
 
 public:
 
-    void data_stream_send(void) override { };
-
 protected:
 
     uint32_t telem_delay() const override { return 0; }