Tracker: create GCS subclass, use inheritted methods

AntennaTracker/GCS_Mavlink.cpp

@@ -58,7 +58,7 @@ void Tracker::send_heartbeat(mavlink_channel_t chan)
         base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
     }
 
-    gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(MAV_TYPE_ANTENNA_TRACKER,
+    gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_ANTENNA_TRACKER,
                                             base_mode,
                                             custom_mode,
                                             system_status);
@@ -111,7 +111,7 @@ void Tracker::send_hwstatus(mavlink_channel_t chan)
 
 void Tracker::send_waypoint_request(mavlink_channel_t chan)
 {
-    gcs_chan[chan-MAVLINK_COMM_0].queued_waypoint_send();
+    gcs().chan(chan-MAVLINK_COMM_0).queued_waypoint_send();
 }
 
 void Tracker::send_nav_controller_output(mavlink_channel_t chan)
@@ -485,30 +485,8 @@ void Tracker::mavlink_check_target(const mavlink_message_t* msg)
 
     // send data stream request to target on all channels
     //  Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
-    for (uint8_t i=0; i < num_gcs; i++) {
-        if (gcs_chan[i].initialised) {
-            // request position
-            if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
-                mavlink_msg_request_data_stream_send(
-                    (mavlink_channel_t)i,
-                    msg->sysid,
-                    msg->compid,
-                    MAV_DATA_STREAM_POSITION,
-                    g.mavlink_update_rate,
-                    1); // start streaming
-            }
-            // request air pressure
-            if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
-                mavlink_msg_request_data_stream_send(
-                    (mavlink_channel_t)i,
-                    msg->sysid,
-                    msg->compid,
-                    MAV_DATA_STREAM_RAW_SENSORS,
-                    g.mavlink_update_rate,
-                    1); // start streaming
-            }
-        }
-    }
+    gcs().request_datastream_position(msg->sysid, msg->compid);
+    gcs().request_datastream_airpressure(msg->sysid, msg->compid);
 
     // flag target has been set
     target_set = true;
@@ -869,7 +847,9 @@ mission_failed:
 void Tracker::mavlink_delay_cb()
 {
     static uint32_t last_1hz, last_50hz, last_5s;
-    if (!gcs_chan[0].initialised) return;
+    if (!gcs().chan(0).initialised) {
+        return;
+    }
 
     tracker.in_mavlink_delay = true;
     DataFlash.EnableWrites(false);
@@ -899,11 +879,7 @@ void Tracker::mavlink_delay_cb()
  */
 void Tracker::gcs_send_message(enum ap_message id)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].send_message(id);
-        }
-    }
+    gcs().send_message(id);
 }
 
 /*
@@ -911,11 +887,7 @@ void Tracker::gcs_send_message(enum ap_message id)
  */
 void Tracker::gcs_data_stream_send(void)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].data_stream_send();
-        }
-    }
+    gcs().data_stream_send();
 }
 
 /*
@@ -923,11 +895,7 @@ void Tracker::gcs_data_stream_send(void)
  */
 void Tracker::gcs_update(void)
 {
-    for (uint8_t i=0; i<num_gcs; i++) {
-        if (gcs_chan[i].initialised) {
-            gcs_chan[i].update(nullptr);
-        }
-    }
+    gcs().update();
 }
 
 void Tracker::gcs_send_text(MAV_SEVERITY severity, const char *str)

AntennaTracker/GCS_Tracker.cpp

@@ -0,0 +1,61 @@
+#include "GCS_Tracker.h"
+#include "Tracker.h"
+
+bool GCS_Tracker::cli_enabled() const
+{
+    return false;
+}
+
+AP_HAL::BetterStream* GCS_Tracker::cliSerial() {
+    return nullptr;
+}
+
+static void mavlink_snoop_static(const mavlink_message_t* msg)
+{
+    tracker.mavlink_snoop(msg);
+}
+
+void GCS_Tracker::setup_uarts(AP_SerialManager &serial_manager)
+{
+    GCS::setup_uarts(serial_manager);
+
+    for (uint8_t i = 1; i < num_gcs(); i++) {
+        gcs().chan(i).set_snoop(mavlink_snoop_static);
+    }
+}
+
+void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid)
+{
+    for (uint8_t i=0; i < num_gcs(); i++) {
+        if (gcs().chan(i).initialised) {
+            // request position
+            if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
+                mavlink_msg_request_data_stream_send(
+                    (mavlink_channel_t)i,
+                    sysid,
+                    compid,
+                    MAV_DATA_STREAM_POSITION,
+                    tracker.g.mavlink_update_rate,
+                    1); // start streaming
+            }
+        }
+    }
+}
+
+void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid)
+{
+    for (uint8_t i=0; i < num_gcs(); i++) {
+        if (gcs().chan(i).initialised) {
+            // request air pressure
+            if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
+                mavlink_msg_request_data_stream_send(
+                    (mavlink_channel_t)i,
+                    sysid,
+                    compid,
+                    MAV_DATA_STREAM_RAW_SENSORS,
+                    tracker.g.mavlink_update_rate,
+                    1); // start streaming
+            }
+        }
+    }
+}

AntennaTracker/GCS_Tracker.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+#include "GCS_Mavlink.h"
+
+class GCS_Tracker : public GCS
+{
+    friend class Tracker; // 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_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; };
+
+    void setup_uarts(AP_SerialManager &serial_manager) override;
+
+private:
+
+    void request_datastream_position(uint8_t sysid, uint8_t compid);
+    void request_datastream_airpressure(uint8_t sysid, uint8_t compid);
+
+    GCS_MAVLINK_Tracker _chan[MAVLINK_COMM_NUM_BUFFERS];
+
+    bool cli_enabled() const override;
+    AP_HAL::BetterStream* cliSerial() override;
+
+};

AntennaTracker/Log.cpp

@@ -96,9 +96,7 @@ void Tracker::log_init(void)
 {
     DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
 
-    for (uint8_t i=0; i<num_gcs; i++) {
-        gcs_chan[i].reset_cli_timeout();
-    }
+    gcs().reset_cli_timeout();
 
     if (g.log_bitmask != 0) {
         start_logging();

AntennaTracker/Parameters.cpp

@@ -253,19 +253,19 @@ const AP_Param::Info Tracker::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),
 
     // @Param: LOG_BITMASK
     // @DisplayName: Log bitmask

AntennaTracker/Tracker.h

@@ -69,6 +69,7 @@
 
 #include "Parameters.h"
 #include "GCS_Mavlink.h"
+#include "GCS_Tracker.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
@@ -77,6 +78,7 @@
 class Tracker : public AP_HAL::HAL::Callbacks {
 public:
     friend class GCS_MAVLINK_Tracker;
+    friend class GCS_Tracker;
     friend class Parameters;
 
     Tracker(void);
@@ -136,10 +138,8 @@ private:
     bool pitch_servo_out_filt_init = false;
 
     AP_SerialManager serial_manager;
-    const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
-    GCS_MAVLINK_Tracker gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
-    GCS _gcs; // avoid using this; use GCS::instance()
-    GCS &gcs() { return _gcs; }
+    GCS_Tracker _gcs; // avoid using this; use gcs()
+    GCS_Tracker &gcs() { return _gcs; }
 
     AP_BoardConfig BoardConfig;
 

AntennaTracker/system.cpp

@@ -4,11 +4,6 @@
 // mission storage
 static const StorageAccess wp_storage(StorageManager::StorageMission);
 
-static void mavlink_snoop_static(const mavlink_message_t* msg)
-{
-    tracker.mavlink_snoop(msg);
-}
-
 static void mavlink_delay_cb_static()
 {
     tracker.mavlink_delay_cb();
@@ -34,7 +29,7 @@ void Tracker::init_tracker()
     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);
+    gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
 
     // Register mavlink_delay_cb, which will run anytime you have
     // more than 5ms remaining in your call to hal.scheduler->delay
@@ -60,10 +55,7 @@ void Tracker::init_tracker()
     check_usb_mux();
 
     // setup telem slots with serial ports
-    for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
-        gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
-        gcs_chan[i].set_snoop(mavlink_snoop_static);
-    }
+    gcs().setup_uarts(serial_manager);
 
 #if LOGGING_ENABLED == ENABLED
     log_init();