From 3e0dcacf58e3fc113df80b486b07b6b15d99098e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 16 May 2018 13:38:39 +1000 Subject: [PATCH] Tracker: handle setting of ekf origin in GCS_MAVLink --- AntennaTracker/GCS_Mavlink.cpp | 5 ----- AntennaTracker/GCS_Mavlink.h | 1 - AntennaTracker/Tracker.h | 1 - AntennaTracker/system.cpp | 23 ----------------------- 4 files changed, 30 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 7cb0344e64..1f17b2706a 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -768,11 +768,6 @@ const AP_FWVersion &GCS_MAVLINK_Tracker::get_fwver() const return tracker.fwver; } -void GCS_MAVLINK_Tracker::set_ekf_origin(const Location& loc) -{ - tracker.set_ekf_origin(loc); -} - /* dummy methods to avoid having to link against AP_Camera */ void AP_Camera::control_msg(mavlink_message_t const*) {} void AP_Camera::configure(float, float, float, float, float, float, float) {} diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index c562eaa947..b0471b1028 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -19,7 +19,6 @@ protected: AP_Rally *get_rally() const override { return nullptr; }; AP_Camera *get_camera() const override { return nullptr; }; const AP_FWVersion &get_fwver() const override; - void set_ekf_origin(const Location& loc) override; uint8_t sysid_my_gcs() const override; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 5fd34d3704..14e6f52f92 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -235,7 +235,6 @@ private: bool get_home_eeprom(struct Location &loc); void set_home_eeprom(struct Location temp); void set_home(struct Location temp); - void set_ekf_origin(const Location& loc); void arm_servos(); void disarm_servos(); void prepare_servos(); diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 1ea64b7a38..e488e438ec 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -165,29 +165,6 @@ void Tracker::set_home(struct Location temp) } } -// sets ekf_origin if it has not been set. -// should only be used when there is no GPS to provide an absolute position -void Tracker::set_ekf_origin(const Location& loc) -{ - // check location is valid - if (!check_latlng(loc)) { - return; - } - - // check EKF origin has already been set - Location ekf_origin; - if (ahrs.get_origin(ekf_origin)) { - return; - } - - if (!ahrs.set_origin(loc)) { - return; - } - - // send ekf origin to GCS - gcs().send_ekf_origin(loc); -} - void Tracker::arm_servos() { hal.util->set_soft_armed(true);