From b81e5a85620a6c0b9e88b49d66d15703d0ab204e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 14 Feb 2017 12:30:32 +1100 Subject: [PATCH] Tracker: create GCS subclass, use inheritted methods --- AntennaTracker/GCS_Mavlink.cpp | 52 ++++++----------------------- AntennaTracker/GCS_Tracker.cpp | 61 ++++++++++++++++++++++++++++++++++ AntennaTracker/GCS_Tracker.h | 30 +++++++++++++++++ AntennaTracker/Log.cpp | 4 +-- AntennaTracker/Parameters.cpp | 8 ++--- AntennaTracker/Tracker.h | 8 ++--- AntennaTracker/system.cpp | 12 ++----- 7 files changed, 112 insertions(+), 63 deletions(-) create mode 100644 AntennaTracker/GCS_Tracker.cpp create mode 100644 AntennaTracker/GCS_Tracker.h diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index a9bea2290a..4b8f5de971 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/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 +#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; + +}; diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 6565e0dc22..7da8ba4ff4 100644 --- a/AntennaTracker/Log.cpp +++ b/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 @@ -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; diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 93019020c2..eb839eb6e0 100644 --- a/AntennaTracker/system.cpp +++ b/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();