Sub: create GCS subclass, use inheritted methods

This also adds a patch to set up gcs channel 0 early to make
BoardConfig happy

ArduSub/GCS_Mavlink.cpp

@@ -76,7 +76,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
     uint8_t mav_type;
     mav_type = MAV_TYPE_SUBMARINE;
 
-    gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(mav_type,
+    gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(mav_type,
                                             base_mode,
                                             custom_mode,
                                             system_status);
@@ -1804,7 +1804,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
 void Sub::mavlink_delay_cb()
 {
     static uint32_t last_1hz, last_50hz, last_5s;
-    if (!gcs_chan[0].initialised || in_mavlink_delay) {
+    if (!gcs().chan(0).initialised || in_mavlink_delay) {
         return;
     }
 
@@ -1838,11 +1838,7 @@ void Sub::mavlink_delay_cb()
  */
 void Sub::gcs_send_message(enum ap_message id)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
+    gcs().send_message(id);
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].send_message(id);
-        }
-    }
 }
 
 /*
@@ -1850,12 +1846,7 @@ void Sub::gcs_send_message(enum ap_message id)
  */
 void Sub::gcs_send_mission_item_reached_message(uint16_t mission_index)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
+    gcs().send_mission_item_reached_message(mission_index);
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].mission_item_reached_index = mission_index;
-            gcs_chan[i].send_message(MSG_MISSION_ITEM_REACHED);
-        }
-    }
 }
 
 /*
@@ -1863,11 +1854,7 @@ void Sub::gcs_send_mission_item_reached_message(uint16_t mission_index)
  */
 void Sub::gcs_data_stream_send(void)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
+    gcs().data_stream_send();
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].data_stream_send();
-        }
-    }
 }
 
 /*
@@ -1875,11 +1862,7 @@ void Sub::gcs_data_stream_send(void)
  */
 void Sub::gcs_check_input(void)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
+    gcs().update();
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].update(NULL);
-        }
-    }
 }
 
 void Sub::gcs_send_text(MAV_SEVERITY severity, const char *str)

ArduSub/GCS_Sub.cpp

@@ -0,0 +1,11 @@
+#include "GCS_Sub.h"
+#include "Sub.h"
+
+bool GCS_Sub::cli_enabled() const
+{
+    return false;
+}
+
+AP_HAL::BetterStream* GCS_Sub::cliSerial() {
+    return NULL;
+}

ArduSub/GCS_Sub.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+#include "GCS_Mavlink.h"
+
+class GCS_Sub : public GCS
+{
+    friend class Sub; // for access to _chan in parameter declarations
+
+public:
+
+    // return the number of valid GCS objects
+    uint8_t num_gcs() const override { return ARRAY_SIZE(_chan); };
+
+    // return GCS link at offset ofs
+    GCS_MAVLINK_Sub &chan(const uint8_t ofs) override {
+        return _chan[ofs];
+    };
+
+private:
+
+    GCS_MAVLINK_Sub _chan[MAVLINK_COMM_NUM_BUFFERS];
+
+    bool cli_enabled() const override;
+    AP_HAL::BetterStream* cliSerial() override;
+
+};

ArduSub/Log.cpp

@@ -486,9 +486,7 @@ void Sub::log_init(void)
 {
     DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
 
-    for (uint8_t i=0; i<num_gcs; i++) {
+    gcs().reset_cli_timeout();
-        gcs_chan[i].reset_cli_timeout();
-    }
 }
 
 #else // LOGGING_ENABLED

ArduSub/Parameters.cpp

@@ -584,19 +584,19 @@ const AP_Param::Info Sub::var_info[] = {
 
     // @Group: SR0_
     // @Path: GCS_Mavlink.cpp
-    GOBJECTN(gcs_chan[0],  gcs0,       "SR0_",     GCS_MAVLINK),
+    GOBJECTN(_gcs._chan[0],  gcs0,       "SR0_",     GCS_MAVLINK),
 
     // @Group: SR1_
     // @Path: GCS_Mavlink.cpp
-    GOBJECTN(gcs_chan[1],  gcs1,       "SR1_",     GCS_MAVLINK),
+    GOBJECTN(_gcs._chan[1],  gcs1,       "SR1_",     GCS_MAVLINK),
 
     // @Group: SR2_
     // @Path: GCS_Mavlink.cpp
-    GOBJECTN(gcs_chan[2],  gcs2,       "SR2_",     GCS_MAVLINK),
+    GOBJECTN(_gcs._chan[2],  gcs2,       "SR2_",     GCS_MAVLINK),
 
     // @Group: SR3_
     // @Path: GCS_Mavlink.cpp
-    GOBJECTN(gcs_chan[3],  gcs3,       "SR3_",     GCS_MAVLINK),
+    GOBJECTN(_gcs._chan[3],  gcs3,       "SR3_",     GCS_MAVLINK),
 
     // @Group: AHRS_
     // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp

ArduSub/Sub.h

@@ -81,6 +81,7 @@
 #include "GCS_Mavlink.h"
 #include "Parameters.h"
 #include "AP_Arming_Sub.h"
+#include "GCS_Sub.h"
 
 // libraries which are dependent on #defines in defines.h and/or config.h
 #if OPTFLOW == ENABLED
@@ -122,6 +123,7 @@
 class Sub : public AP_HAL::HAL::Callbacks {
 public:
     friend class GCS_MAVLINK_Sub;
+    friend class GCS_Sub;
     friend class Parameters;
     friend class ParametersG2;
     friend class AP_Arming_Sub;
@@ -207,11 +209,9 @@ private:
 
     // GCS selection
     AP_SerialManager serial_manager;
-    static const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
 
-    GCS_MAVLINK_Sub gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
+    GCS_Sub _gcs; // avoid using this; use gcs()
-    GCS _gcs; // avoid using this; use gcs()
+    GCS_Sub &gcs() { return _gcs; }
-    GCS &gcs() { return _gcs; }
 
     // User variables
 #ifdef USERHOOK_VARIABLES

ArduSub/system.cpp

@@ -41,6 +41,9 @@ void Sub::init_ardupilot()
     // initialise serial port
     serial_manager.init();
 
+    // setup first port early to allow BoardConfig to report errors
+    gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
+
     // init cargo gripper
 #if GRIPPER_ENABLED == ENABLED
     g2.gripper.init();
@@ -62,9 +65,7 @@ void Sub::init_ardupilot()
     hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
 
     // setup telem slots with serial ports
-    for (uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
+    gcs().setup_uarts(serial_manager);
-        gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
-    }
 
 #if LOGGING_ENABLED == ENABLED
     log_init();