diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index d2dc6a2ac5..20d274eeee 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/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 +#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; + +}; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 1f83999fa4..3dd9ae2ad3 100644 --- a/ArduSub/Log.cpp +++ b/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; iregister_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_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i); - } + gcs().setup_uarts(serial_manager); #if LOGGING_ENABLED == ENABLED log_init();