From 98daf23d8870267e9b4c40a5350095b42c2fa4da Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 Apr 2024 21:40:30 +1100 Subject: [PATCH] AntennaTracker: move handling of DO_SET_HOME up to GCS_MAVLink base class --- AntennaTracker/GCS_Mavlink.cpp | 9 +-------- AntennaTracker/GCS_Mavlink.h | 3 --- AntennaTracker/Tracker.cpp | 2 +- AntennaTracker/Tracker.h | 3 ++- AntennaTracker/sensors.cpp | 2 +- AntennaTracker/system.cpp | 7 ++++++- 6 files changed, 11 insertions(+), 15 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 270d3a74df..c99d22bb0d 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -451,13 +451,6 @@ MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_ } } -bool GCS_MAVLINK_Tracker::set_home_to_current_location(bool _lock) { - return tracker.set_home(AP::gps().location()); -} -bool GCS_MAVLINK_Tracker::set_home(const Location& loc, bool _lock) { - return tracker.set_home(loc); -} - void GCS_MAVLINK_Tracker::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -584,7 +577,7 @@ void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &m // check if this is the HOME wp if (packet.seq == 0) { - if (!tracker.set_home(tell_command)) { + if (!tracker.set_home(tell_command, false)) { result = MAV_MISSION_ERROR; goto mission_failed; } diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 79610f0031..a431f5217e 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -27,9 +27,6 @@ protected: return 0; // what if we have been picked up and carried somewhere? } - bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; - void send_nav_controller_output() const override; void send_pid_tuning() override; diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index 5fd202b1bd..3c6398ec6a 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -92,7 +92,7 @@ void Tracker::one_second_loop() // set home to current location Location temp_loc; if (ahrs.get_location(temp_loc)) { - if (!set_home(temp_loc)){ + if (!set_home(temp_loc, false)) { // fail silently } } diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 8c745fcf41..975a271b6e 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -197,7 +197,8 @@ private: void init_ardupilot() override; bool get_home_eeprom(Location &loc) const; bool set_home_eeprom(const Location &temp) WARN_IF_UNUSED; - bool set_home(const Location &temp) WARN_IF_UNUSED; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location &temp, bool lock) override WARN_IF_UNUSED; void prepare_servos(); void set_mode(Mode &newmode, ModeReason reason); bool set_mode(uint8_t new_mode, ModeReason reason) override; diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 55946944d9..72094fe15a 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -51,7 +51,7 @@ void Tracker::update_GPS(void) // Now have an initial GPS position // use it as the HOME position in future startups current_loc = gps.location(); - IGNORE_RETURN(set_home(current_loc)); + IGNORE_RETURN(set_home(current_loc, false)); ground_start_count = 0; } } diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 953cde2e07..b2a2de8ca3 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -116,7 +116,12 @@ bool Tracker::set_home_eeprom(const Location &temp) return true; } -bool Tracker::set_home(const Location &temp) +bool Tracker::set_home_to_current_location(bool lock) +{ + return set_home(AP::gps().location(), lock); +} + +bool Tracker::set_home(const Location &temp, bool lock) { // check EKF origin has been set Location ekf_origin;