GCS_MAVLink: avoid allocating a GCS_MAVLINK per mavlink channel

libraries/GCS_MAVLink/GCS.cpp

@@ -52,10 +52,7 @@ void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
         return;
     }
     for (uint8_t i=0; i<num_gcs(); i++) {
-        GCS_MAVLINK &c = chan(i);
-        if (!c.initialised) {
-            continue;
-        }
+        GCS_MAVLINK &c = *chan(i);
         if (!c.is_active()) {
             continue;
         }
@@ -89,12 +86,12 @@ bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protoco
     if (c >= num_gcs()) {
         return false;
     }
-    if (chan(c).alternative.handler && handler) {
+    if (chan(c)->alternative.handler && handler) {
         // already have one installed - we may need to add support for
         // multiple alternative handlers
         return false;
     }
-    chan(c).alternative.handler = handler;
+    chan(c)->alternative.handler = handler;
     return true;
 }
 

libraries/GCS_MAVLink/GCS.h

@@ -37,6 +37,19 @@
     }
 #define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
 
+#define GCS_MAVLINK_NUM_STREAM_RATES 10
+class GCS_MAVLINK_Parameters
+{
+public:
+
+    GCS_MAVLINK_Parameters();
+
+    static const struct AP_Param::GroupInfo        var_info[];
+
+    // saveable rate of each stream
+    AP_Int16        streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
+};
+
 ///
 /// @class	GCS_MAVLINK
 /// @brief	MAVLink transport control class
@@ -45,11 +58,13 @@ class GCS_MAVLINK
 {
 public:
     friend class GCS;
-    GCS_MAVLINK();
+
+    GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters, AP_HAL::UARTDriver &uart);
+    virtual ~GCS_MAVLINK() {}
+
     void        update_receive(uint32_t max_time_us=1000);
     void        update_send();
-    void        init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan);
-    void        setup_uart(uint8_t instance);
+    bool        init(uint8_t instance);
     void        send_message(enum ap_message id);
     void        send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
     void        send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) const;
@@ -101,11 +116,6 @@ public:
     virtual uint8_t sysid_my_gcs() const = 0;
     virtual bool sysid_enforce() const { return false; }
 
-    static const struct AP_Param::GroupInfo        var_info[];
-
-    // set to true if this GCS link is active
-    bool            initialised;
-
     // NOTE! The streams enum below and the
     // set of AP_Int16 stream rates _must_ be
     // kept in the same order
@@ -123,6 +133,12 @@ public:
         NUM_STREAMS
     };
 
+    // streams must be moved out into the top level for
+    // GCS_MAVLINK_Parameters to be able to use it.  This is an
+    // extensive change, so we 'll just keep them in sync with a
+    // static assert for now:
+    static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
+
     bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
     // return true if this channel has hardware flow control
     bool have_flow_control();
@@ -280,7 +296,7 @@ protected:
     uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
 
     // saveable rate of each stream
-    AP_Int16        streamRates[NUM_STREAMS];
+    AP_Int16        *streamRates;
 
     virtual bool persist_streamrates() const { return false; }
     void handle_request_data_stream(const mavlink_message_t &msg);
@@ -701,9 +717,10 @@ public:
     void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
     virtual void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text);
     void service_statustext(void);
-    virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0;
-    virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0;
-    virtual uint8_t num_gcs() const = 0;
+    virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0;
+    virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0;
+    // return the number of valid GCS objects
+    uint8_t num_gcs() const { return _num_gcs; };
     void send_message(enum ap_message id);
     void send_mission_item_reached_message(uint16_t mission_index);
     void send_named_float(const char *name, float value) const;
@@ -719,7 +736,6 @@ public:
 
     void update_send();
     void update_receive();
-    virtual void setup_uarts();
 
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
@@ -728,6 +744,10 @@ public:
     virtual uint16_t min_loop_time_remaining_for_message_send_us() const {
         return 200;
     }
+
+    void setup_console();
+    void setup_uarts();
+
     bool out_of_time() const;
 
     // frsky backend
@@ -740,7 +760,7 @@ public:
     bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
 
     // get the VFR_HUD throttle
-    int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0).vfr_hud_throttle():0; }
+    int16_t get_hud_throttle(void) const { return num_gcs()>0?chan(0)->vfr_hud_throttle():0; }
 
     // update uart pass-thru
     void update_passthru();
@@ -753,16 +773,26 @@ public:
 
 protected:
 
+    virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
+                                                 AP_HAL::UARTDriver &uart) = 0;
+
     uint32_t control_sensors_present;
     uint32_t control_sensors_enabled;
     uint32_t control_sensors_health;
     void update_sensor_status_flags();
     virtual void update_vehicle_sensor_status_flags() {}
 
+    GCS_MAVLINK_Parameters chan_parameters[MAVLINK_COMM_NUM_BUFFERS];
+    uint8_t _num_gcs;
+    GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];
+
 private:
 
     static GCS *_singleton;
 
+    void create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
+                                    AP_HAL::UARTDriver &uart);
+
     struct statustext_t {
         uint8_t                 bitmask;
         mavlink_statustext_t    msg;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -76,55 +76,29 @@ uint8_t GCS_MAVLINK::mavlink_private = 0;
 
 GCS *GCS::_singleton = nullptr;
 
-GCS_MAVLINK::GCS_MAVLINK()
+GCS_MAVLINK::GCS_MAVLINK(GCS_MAVLINK_Parameters &parameters,
+                         AP_HAL::UARTDriver &uart)
 {
-    AP_Param::setup_object_defaults(this, var_info);
-}
-
-void
-GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
-{
-    if (!valid_channel(mav_chan)) {
-        return;
-    }
-
-    _port = port;
-    chan = mav_chan;
-
-    mavlink_comm_port[chan] = _port;
-    _queued_parameter = nullptr;
+    _port = &uart;
 
-    snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan);
-    _perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name);
-
-    snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan);
-    _perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name);
-
-    initialised = true;
+    streamRates = parameters.streamRates;
 }
 
-
-/*
-  setup a UART, handling begin() and init()
- */
-void
-GCS_MAVLINK::setup_uart(uint8_t instance)
+bool GCS_MAVLINK::init(uint8_t instance)
 {
     // search for serial port
     const AP_SerialManager& serial_manager = AP::serialmanager();
-    const AP_SerialManager::SerialProtocol protocol = AP_SerialManager::SerialProtocol_MAVLink;
 
-    AP_HAL::UARTDriver *uart = serial_manager.find_serial(protocol, instance);
-    if (uart == nullptr) {
-        // return immediately if not found
-        return;
-    }
+    const AP_SerialManager::SerialProtocol protocol = AP_SerialManager::SerialProtocol_MAVLink;
 
     // get associated mavlink channel
-    mavlink_channel_t mav_chan;
-    if (!serial_manager.get_mavlink_channel(protocol, instance, mav_chan)) {
+    if (!serial_manager.get_mavlink_channel(protocol, instance, chan)) {
         // return immediately in unlikely case mavlink channel cannot be found
-        return;
+        return false;
+    }
+    // and init the gcs instance
+    if (!valid_channel(chan)) {
+        return false;
     }
 
     /*
@@ -135,29 +109,35 @@ GCS_MAVLINK::setup_uart(uint8_t instance)
       0x20 at 115200 on startup, which tells the bootloader to reset
       and boot normally
      */
-    uart->begin(115200);
-    AP_HAL::UARTDriver::flow_control old_flow_control = uart->get_flow_control();
-    uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
+    _port->begin(115200);
+    AP_HAL::UARTDriver::flow_control old_flow_control = _port->get_flow_control();
+    _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
     for (uint8_t i=0; i<3; i++) {
         hal.scheduler->delay(1);
-        uart->write(0x30);
-        uart->write(0x20);
+        _port->write(0x30);
+        _port->write(0x20);
     }
     // since tcdrain() and TCSADRAIN may not be implemented...
     hal.scheduler->delay(1);
     
-    uart->set_flow_control(old_flow_control);
+    _port->set_flow_control(old_flow_control);
 
     // now change back to desired baudrate
-    uart->begin(serial_manager.find_baudrate(protocol, instance));
+    _port->begin(serial_manager.find_baudrate(protocol, instance));
 
-    // and init the gcs instance
-    init(uart, mav_chan);
+    mavlink_comm_port[chan] = _port;
+
+    // create performance counters
+    snprintf(_perf_packet_name, sizeof(_perf_packet_name), "GCS_Packet_%u", chan);
+    _perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_packet_name);
+
+    snprintf(_perf_update_name, sizeof(_perf_update_name), "GCS_Update_%u", chan);
+    _perf_update = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _perf_update_name);
 
-    AP_SerialManager::SerialProtocol mavlink_protocol = AP::serialmanager().get_mavlink_protocol(mav_chan);
+    AP_SerialManager::SerialProtocol mavlink_protocol = serial_manager.get_mavlink_protocol(chan);
     mavlink_status_t *status = mavlink_get_channel_status(chan);
     if (status == nullptr) {
-        return;
+        return false;
     }
     
     if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
@@ -178,6 +158,8 @@ GCS_MAVLINK::setup_uart(uint8_t instance)
         // after experiments with MAVLink2
         status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
     }
+
+    return true;
 }
 
 void GCS_MAVLINK::send_meminfo(void)
@@ -1845,9 +1827,7 @@ void GCS::service_statustext(void)
 void GCS::send_message(enum ap_message id)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
-        if (chan(i).initialised) {
-            chan(i).send_message(id);
-        }
+        chan(i)->send_message(id);
     }
 }
 
@@ -1872,9 +1852,7 @@ void GCS::update_send()
         _missionitemprotocol_rally->update();
     }
     for (uint8_t i=0; i<num_gcs(); i++) {
-        if (chan(i).initialised) {
-            chan(i).update_send();
-        }
+        chan(i)->update_send();
     }
     WITH_SEMAPHORE(_statustext_sem);
     service_statustext();
@@ -1883,9 +1861,7 @@ void GCS::update_send()
 void GCS::update_receive(void)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
-        if (chan(i).initialised) {
-            chan(i).update_receive();
-        }
+        chan(i)->update_receive();
     }
     // also update UART pass-thru, if enabled
     update_passthru();
@@ -1894,17 +1870,62 @@ void GCS::update_receive(void)
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
-        if (chan(i).initialised) {
-            chan(i).mission_item_reached_index = mission_index;
-            chan(i).send_message(MSG_MISSION_ITEM_REACHED);
+        chan(i)->mission_item_reached_index = mission_index;
+        chan(i)->send_message(MSG_MISSION_ITEM_REACHED);
     }
+}
+
+void GCS::setup_console()
+{
+    AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, 0);
+    if (uart == nullptr) {
+        // this is probably not going to end well.
+        return;
+    }
+    if (ARRAY_SIZE(chan_parameters) == 0) {
+        return;
     }
+    create_gcs_mavlink_backend(chan_parameters[0], *uart);
+}
+
+
+GCS_MAVLINK_Parameters::GCS_MAVLINK_Parameters()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void GCS::create_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params, AP_HAL::UARTDriver &uart)
+{
+    if (_num_gcs >= ARRAY_SIZE(chan_parameters)) {
+        return;
+    }
+    _chan[_num_gcs] = new_gcs_mavlink_backend(params, uart);
+    if (_chan[_num_gcs] == nullptr) {
+        return;
+    }
+
+    if (!_chan[_num_gcs]->init(_num_gcs)) {
+        delete _chan[_num_gcs];
+        _chan[_num_gcs] = nullptr;
+        return;
+    }
+
+    _num_gcs++;
 }
 
 void GCS::setup_uarts()
 {
     for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
-        chan(i).setup_uart(i);
+        if (i >= ARRAY_SIZE(chan_parameters)) {
+            // should not happen
+            break;
+        }
+        AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, i);
+        if (uart == nullptr) {
+            // no more mavlink uarts
+            break;
+        }
+        create_gcs_mavlink_backend(chan_parameters[i], *uart);
     }
 
     if (frsky == nullptr) {

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -23,6 +23,12 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {};
  */
 class GCS_MAVLINK_Dummy : public GCS_MAVLINK
 {
+public:
+
+    using GCS_MAVLINK::GCS_MAVLINK;
+
+private:
+
     uint32_t telem_delay() const override { return 0; }
     void handleMessage(const mavlink_message_t &msg) override {}
     bool try_send_message(enum ap_message id) override { return true; }
@@ -53,10 +59,32 @@ extern const AP_HAL::HAL& hal;
 
 class GCS_Dummy : public GCS
 {
-    GCS_MAVLINK_Dummy dummy_backend;
-    uint8_t num_gcs() const override { return 1; }
-    GCS_MAVLINK_Dummy &chan(const uint8_t ofs) override { return dummy_backend; }
-    const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; };
+public:
+
+    using GCS::GCS;
+
+protected:
+
+    GCS_MAVLINK_Dummy *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
+                                               AP_HAL::UARTDriver &uart) override {
+        return new GCS_MAVLINK_Dummy(params, uart);
+    }
+
+private:
+    GCS_MAVLINK_Dummy *chan(const uint8_t ofs) override {
+        if (ofs > _num_gcs) {
+            AP::internalerror().error(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Dummy *)_chan[ofs];
+    };
+    const GCS_MAVLINK_Dummy *chan(const uint8_t ofs) const override {
+        if (ofs > _num_gcs) {
+            AP::internalerror().error(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Dummy *)_chan[ofs];
+    };
 
     void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) override { hal.console->printf("TOGCS: %s\n", text); }
 

libraries/GCS_MAVLink/GCS_Param.cpp

@@ -36,10 +36,6 @@ bool GCS_MAVLINK::param_timer_registered;
 void
 GCS_MAVLINK::queued_param_send()
 {
-    if (!initialised) {
-        return;
-    }
-
     // send parameter async replies
     uint8_t async_replies_sent_count = send_parameter_async_replies();
 

libraries/GCS_MAVLink/examples/routing/routing.cpp

@@ -7,17 +7,19 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <GCS_MAVLink/GCS_Dummy.h>
 #include <AP_Common/AP_FWVersion.h>
+#include <AP_SerialManager/AP_SerialManager.h>
 
 void setup();
 void loop();
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+AP_SerialManager _serialmanager;
 GCS_Dummy _gcs;
 
 extern mavlink_system_t mavlink_system;
 
-const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
+const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     AP_GROUPEND
 };
 
@@ -26,7 +28,7 @@ static MAVLink_routing routing;
 void setup(void)
 {
     hal.console->printf("routing test startup...");
-    gcs().chan(0).init(hal.uartA, MAVLINK_COMM_0);
+    gcs().setup_console();
 }
 
 void loop(void)