diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 270d3a74df838..c99d22bb0d43f 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 79610f00311ec..a431f5217e230 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -27,9 +27,6 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK 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 5fd202b1bdf4a..3c6398ec6a564 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 8c745fcf41952..975a271b6e303 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -197,7 +197,8 @@ class Tracker : public AP_Vehicle { 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 55946944d9476..72094fe15aa6d 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 953cde2e07f1f..b2a2de8ca32e7 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; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b3be8285f7f9b..e0e891432efee 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -739,8 +739,8 @@ class Copter : public AP_Vehicle { // commands.cpp void update_home_from_EKF(); void set_home_to_current_location_inflight(); - bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; bool far_from_EKF_origin(const Location& loc); // compassmot.cpp diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9c7eeccf1644e..3c10b8e0cefa4 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -711,13 +711,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int return GCS_MAVLINK::handle_preflight_reboot(packet, msg); } -bool GCS_MAVLINK_Copter::set_home_to_current_location(bool _lock) { - return copter.set_home_to_current_location(_lock); -} -bool GCS_MAVLINK_Copter::set_home(const Location& loc, bool _lock) { - return copter.set_home(loc, _lock); -} - MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { #if MODE_GUIDED_ENABLED == ENABLED diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 1961916e03bae..30d325c01df7e 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -48,8 +48,6 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; - 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; uint64_t capabilities() const override; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 751544d915bee..aabe9cfbe536d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -781,25 +781,25 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, } -bool GCS_MAVLINK_Plane::set_home_to_current_location(bool _lock) +bool Plane::set_home_to_current_location(bool _lock) { - if (!plane.set_home_persistently(AP::gps().location())) { + if (!set_home_persistently(AP::gps().location())) { return false; } if (_lock) { AP::ahrs().lock_home(); } - if ((plane.control_mode == &plane.mode_rtl) + if ((control_mode == &mode_rtl) #if HAL_QUADPLANE_ENABLED - || (plane.control_mode == &plane.mode_qrtl) + || (control_mode == &mode_qrtl) #endif ) { // if in RTL head to the updated home location - plane.control_mode->enter(); + control_mode->enter(); } return true; } -bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock) +bool Plane::set_home(const Location& loc, bool _lock) { if (!AP::ahrs().set_home(loc)) { return false; @@ -807,13 +807,13 @@ bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock) if (_lock) { AP::ahrs().lock_home(); } - if ((plane.control_mode == &plane.mode_rtl) + if ((control_mode == &mode_rtl) #if HAL_QUADPLANE_ENABLED - || (plane.control_mode == &plane.mode_qrtl) + || (control_mode == &mode_qrtl) #endif ) { // if in RTL head to the updated home location - plane.control_mode->enter(); + control_mode->enter(); } return true; } diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index b3b59cb2d8790..da77d1b357d56 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -42,8 +42,6 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK bool persist_streamrates() const override { return true; } - bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; uint64_t capabilities() const override; void send_nav_controller_output() const override; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 189189e9dcb84..71b450256c4d9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -986,6 +986,8 @@ class Plane : public AP_Vehicle { // set home location and store it persistently: bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; // control_modes.cpp void read_control_switch(); diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 3ea5a72fa5e2c..61e822bdf292c 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -475,13 +475,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) return MAV_RESULT_ACCEPTED; } -bool GCS_MAVLINK_Sub::set_home_to_current_location(bool _lock) { - return sub.set_home_to_current_location(_lock); -} -bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool _lock) { - return sub.set_home(loc, _lock); -} - MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch(packet.command) { diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 4b49f2246e64c..908149d1772ab 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -29,9 +29,6 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { int32_t global_position_int_alt() const override; int32_t global_position_int_relative_alt() const override; - 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_banner() override; void send_nav_controller_output() const override; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 483425f791507..52f5725bab703 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -423,8 +423,8 @@ class Sub : public AP_Vehicle { void userhook_SuperSlowLoop(); void update_home_from_EKF(); void set_home_to_current_location_inflight(); - bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; bool far_from_EKF_origin(const Location& loc); void exit_mission(); bool verify_loiter_unlimited(); diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 826d92b98c49f..ccaa200a1c989 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -310,8 +310,8 @@ class Blimp : public AP_Vehicle // commands.cpp void update_home_from_EKF(); void set_home_to_current_location_inflight(); - bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; bool far_from_EKF_origin(const Location& loc); // ekf_check.cpp diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index e6107aab6fa3a..ff52827562b4a 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -448,15 +448,6 @@ MAV_RESULT GCS_MAVLINK_Blimp::handle_command_do_set_roi(const Location &roi_loc) return MAV_RESULT_ACCEPTED; } -bool GCS_MAVLINK_Blimp::set_home_to_current_location(bool _lock) -{ - return blimp.set_home_to_current_location(_lock); -} -bool GCS_MAVLINK_Blimp::set_home(const Location& loc, bool _lock) -{ - return blimp.set_home(loc, _lock); -} - MAV_RESULT GCS_MAVLINK_Blimp::handle_command_int_do_reposition(const mavlink_command_int_t &packet) { const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 71f10a414e7c3..35276b03a46d5 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -35,8 +35,6 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK bool mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const override; #endif - 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; //TODO Apparently can't remove this or the build fails. uint64_t capabilities() const override; diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 129cc8c329fc6..f38e8f3dd0498 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -656,14 +656,6 @@ MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlin return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg); } -bool GCS_MAVLINK_Rover::set_home_to_current_location(bool _lock) { - return rover.set_home_to_current_location(_lock); -} - -bool GCS_MAVLINK_Rover::set_home(const Location& loc, bool _lock) { - return rover.set_home(loc, _lock); -} - MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch (packet.command) { diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 048bb86969a4e..2d4ca700a29db 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -31,8 +31,6 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK bool persist_streamrates() const override { return true; } - bool set_home_to_current_location(bool lock) override; - bool set_home(const Location& loc, bool lock) override; uint64_t capabilities() const override; void send_nav_controller_output() const override; diff --git a/Rover/Rover.h b/Rover/Rover.h index e12ed2db608b8..8873b844e2946 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -295,8 +295,8 @@ class Rover : public AP_Vehicle { bool is_balancebot() const; // commands.cpp - bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; + 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 update_home(); // crash_check.cpp diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 6395cab877854..0ea6a802ca665 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -289,6 +289,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { */ virtual bool get_rate_ef_targets(Vector3f& rate_ef_targets) const { return false; } +#if AP_AHRS_ENABLED + virtual bool set_home_to_current_location(bool lock) WARN_IF_UNUSED { return false; } + virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED { return false; } +#endif + protected: virtual void init_ardupilot() = 0; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index bd26264667b56..abcdf00cbdfc0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -526,8 +526,8 @@ class GCS_MAVLINK virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_int_external_position_estimate(const mavlink_command_int_t &packet); - virtual bool set_home_to_current_location(bool lock) = 0; - virtual bool set_home(const Location& loc, bool lock) = 0; + bool set_home_to_current_location(bool lock); + bool set_home(const Location& loc, bool lock); #if AP_ARMING_ENABLED virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_int_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f7d17bd2d735f..a473626b0bbca 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5057,6 +5057,23 @@ void GCS_MAVLINK::handle_landing_target(const mavlink_message_t &msg) } +bool GCS_MAVLINK::set_home_to_current_location(bool _lock) +{ +#if AP_VEHICLE_ENABLED + return AP::vehicle()->set_home_to_current_location(_lock); +#else + return false; +#endif +} + +bool GCS_MAVLINK::set_home(const Location& loc, bool _lock) { +#if AP_VEHICLE_ENABLED + return AP::vehicle()->set_home(loc, _lock); +#else + return false; +#endif +} + MAV_RESULT GCS_MAVLINK::handle_command_do_set_home(const mavlink_command_int_t &packet) { if (is_equal(packet.param1, 1.0f) || (packet.x == 0 && packet.y == 0)) { diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index e40968ca82231..723f927dcdef6 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -33,9 +33,6 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; } MAV_STATE vehicle_system_status() const override { return MAV_STATE_CALIBRATING; } - bool set_home_to_current_location(bool _lock) override { return false; } - bool set_home(const Location& loc, bool _lock) override { return false; } - void send_nav_controller_output() const override {}; void send_pid_tuning() override {}; };