diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 32b2001bf0..0798f69341 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -371,14 +371,15 @@ GCS_MAVLINK_Tracker::data_stream_send(void) We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs */ -void Tracker::mavlink_snoop(const mavlink_message_t* msg) +void GCS_MAVLINK_Tracker::packetReceived(const mavlink_status_t &status, + mavlink_message_t &msg) { // return immediately if sysid doesn't match our target sysid - if ((g.sysid_target != 0) && (g.sysid_target != msg->sysid)) { + if ((tracker.g.sysid_target != 0) && (tracker.g.sysid_target != msg.sysid)) { return; } - switch (msg->msgid) { + switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_check_target(msg); @@ -389,8 +390,8 @@ void Tracker::mavlink_snoop(const mavlink_message_t* msg) { // decode mavlink_global_position_int_t packet; - mavlink_msg_global_position_int_decode(msg, &packet); - tracking_update_position(packet); + mavlink_msg_global_position_int_decode(&msg, &packet); + tracker.tracking_update_position(packet); break; } @@ -398,24 +399,25 @@ void Tracker::mavlink_snoop(const mavlink_message_t* msg) { // decode mavlink_scaled_pressure_t packet; - mavlink_msg_scaled_pressure_decode(msg, &packet); - tracking_update_pressure(packet); + mavlink_msg_scaled_pressure_decode(&msg, &packet); + tracker.tracking_update_pressure(packet); break; } } + GCS_MAVLINK::packetReceived(status, msg); } // locks onto a particular target sysid and sets it's position data stream to at least 1hz -void Tracker::mavlink_check_target(const mavlink_message_t* msg) +void GCS_MAVLINK_Tracker::mavlink_check_target(const mavlink_message_t &msg) { // exit immediately if the target has already been set - if (target_set) { + if (tracker.target_set) { return; } // decode mavlink_heartbeat_t packet; - mavlink_msg_heartbeat_decode(msg, &packet); + mavlink_msg_heartbeat_decode(&msg, &packet); // exit immediately if this is not a vehicle we would track if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) || @@ -426,17 +428,17 @@ void Tracker::mavlink_check_target(const mavlink_message_t* msg) } // set our sysid to the target, this ensures we lock onto a single vehicle - if (g.sysid_target == 0) { - g.sysid_target = msg->sysid; + if (tracker.g.sysid_target == 0) { + tracker.g.sysid_target = msg.sysid; } // 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 - gcs().request_datastream_position(msg->sysid, msg->compid); - gcs().request_datastream_airpressure(msg->sysid, msg->compid); + tracker.gcs().request_datastream_position(msg.sysid, msg.compid); + tracker.gcs().request_datastream_airpressure(msg.sysid, msg.compid); // flag target has been set - target_set = true; + tracker.target_set = true; } uint8_t GCS_MAVLINK_Tracker::sysid_my_gcs() const diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 06b81b383d..a37daada31 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -30,6 +30,8 @@ protected: private: + void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override; + void mavlink_check_target(const mavlink_message_t &msg); void handleMessage(mavlink_message_t * msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp index a57f91e234..4447c88cdd 100644 --- a/AntennaTracker/GCS_Tracker.cpp +++ b/AntennaTracker/GCS_Tracker.cpp @@ -1,20 +1,6 @@ #include "GCS_Tracker.h" #include "Tracker.h" -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++) { diff --git a/AntennaTracker/GCS_Tracker.h b/AntennaTracker/GCS_Tracker.h index 7f75ab24a2..10ebc0298c 100644 --- a/AntennaTracker/GCS_Tracker.h +++ b/AntennaTracker/GCS_Tracker.h @@ -6,6 +6,7 @@ class GCS_Tracker : public GCS { friend class Tracker; // for access to _chan in parameter declarations + friend class GCS_MAVLINK_Tracker; public: @@ -16,8 +17,6 @@ public: GCS_MAVLINK_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; }; const GCS_MAVLINK_Tracker &chan(const uint8_t ofs) const override { return _chan[ofs]; }; - void setup_uarts(AP_SerialManager &serial_manager) override; - private: void request_datastream_position(uint8_t sysid, uint8_t compid); diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index a1820981ca..fb82212b9c 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -208,7 +208,6 @@ private: void send_location(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); void send_simstate(mavlink_channel_t chan); - void mavlink_check_target(const mavlink_message_t* msg); void gcs_data_stream_send(void); void gcs_update(void); void gcs_retry_deferred(void); @@ -268,7 +267,6 @@ private: void handle_battery_failsafe(const char* type_str, const int8_t action); public: - void mavlink_snoop(const mavlink_message_t* msg); void mavlink_delay_cb(); };