diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 975a271b6e303..7b1032b6b655c 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -197,7 +197,6 @@ 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_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); diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index b2a2de8ca32e7..763b14f183e2d 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -116,25 +116,19 @@ bool Tracker::set_home_eeprom(const Location &temp) return true; } -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; - if (ahrs.get_origin(ekf_origin)) { - if (!ahrs.set_home(temp)) { - return false; - } + if (!AP_Vehicle::set_home(temp, lock)) { + return false; } + // also store this away for next boot: if (!set_home_eeprom(temp)) { return false; } + // and move our current location there, in case we don't have an + // absolute position source: current_loc = temp; return true; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3285f267976e5..d03d293e4b6b2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -740,7 +740,6 @@ class Copter : public AP_Vehicle { void update_home_from_EKF(); void set_home_to_current_location_inflight(); bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; // compassmot.cpp MAV_RESULT mavlink_compassmot(const GCS_MAVLINK &gcs_chan); diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index fea41d19e5563..6c3f72d8003b0 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -37,42 +37,16 @@ void Copter::set_home_to_current_location_inflight() { } // set_home_to_current_location - set home to current GPS location -bool Copter::set_home_to_current_location(bool lock) { - // get current location from EKF - Location temp_loc; - if (ahrs.get_location(temp_loc)) { - if (!set_home(temp_loc, lock)) { - return false; - } - // we have successfully set AHRS home, set it for SmartRTL -#if MODE_SMARTRTL_ENABLED == ENABLED - g2.smart_rtl.set_home(true); -#endif - return true; - } - return false; -} - -// set_home - sets ahrs home (used for RTL) to specified location -// returns true if home location set successfully -bool Copter::set_home(const Location& loc, bool lock) +bool Copter::set_home_to_current_location(bool lock) { - // check EKF origin has been set - Location ekf_origin; - if (!ahrs.get_origin(ekf_origin)) { - return false; - } - - // set ahrs home (used for RTL) - if (!ahrs.set_home(loc)) { + if (!AP_Vehicle::set_home_to_current_location(lock)) { return false; } - // lock home position - if (lock) { - ahrs.lock_home(); - } + // we have successfully set AHRS home, set it for SmartRTL +#if MODE_SMARTRTL_ENABLED == ENABLED + g2.smart_rtl.set_home(true); +#endif - // return success return true; } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 6aba8c71861ef..205cf11e28ca6 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -424,7 +424,6 @@ class Sub : public AP_Vehicle { void update_home_from_EKF(); void set_home_to_current_location_inflight(); bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; float get_alt_rel() const WARN_IF_UNUSED; float get_alt_msl() const WARN_IF_UNUSED; void exit_mission(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index b12626ed4e806..1e21e55df582d 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -49,27 +49,3 @@ bool Sub::set_home_to_current_location(bool lock) } return false; } - -// set_home - sets ahrs home (used for RTL) to specified location -// returns true if home location set successfully -bool Sub::set_home(const Location& loc, bool lock) -{ - // check if EKF origin has been set - Location ekf_origin; - if (!ahrs.get_origin(ekf_origin)) { - return false; - } - - // set ahrs home (used for RTL) - if (!ahrs.set_home(loc)) { - return false; - } - - // lock home position - if (lock) { - ahrs.lock_home(); - } - - // return success - return true; -} diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 086f17f44a552..09ec39cb2af46 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -310,8 +310,6 @@ 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) override WARN_IF_UNUSED; - bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; // ekf_check.cpp void ekf_check(); diff --git a/Blimp/commands.cpp b/Blimp/commands.cpp index 2185955e2454f..8432b9dfa288c 100644 --- a/Blimp/commands.cpp +++ b/Blimp/commands.cpp @@ -32,42 +32,3 @@ void Blimp::set_home_to_current_location_inflight() } } } - -// set_home_to_current_location - set home to current GPS location -bool Blimp::set_home_to_current_location(bool lock) -{ - // get current location from EKF - Location temp_loc; - if (ahrs.get_location(temp_loc)) { - if (!set_home(temp_loc, lock)) { - return false; - } - return true; - } - return false; -} - -// set_home - sets ahrs home (used for RTL) to specified location -// initialises inertial nav and compass on first call -// returns true if home location set successfully -bool Blimp::set_home(const Location& loc, bool lock) -{ - // check EKF origin has been set - Location ekf_origin; - if (!ahrs.get_origin(ekf_origin)) { - return false; - } - - // set ahrs home (used for RTL) - if (!ahrs.set_home(loc)) { - return false; - } - - // lock home position - if (lock) { - ahrs.lock_home(); - } - - // return success - return true; -} diff --git a/Rover/commands.cpp b/Rover/commands.cpp index c4a67344d2064..7e9410c4df578 100644 --- a/Rover/commands.cpp +++ b/Rover/commands.cpp @@ -3,32 +3,24 @@ // set ahrs home to current location from inertial-nav location bool Rover::set_home_to_current_location(bool lock) { - Location temp_loc; - if (ahrs.have_inertial_nav() && ahrs.get_location(temp_loc)) { - if (!set_home(temp_loc, lock)) { - return false; - } - // we have successfully set AHRS home, set it for SmartRTL - g2.smart_rtl.set_home(true); - return true; + if (!AP_Vehicle::set_home_to_current_location(lock)) { + return false; } - return false; + + // we have successfully set AHRS home, set it for SmartRTL + g2.smart_rtl.set_home(true); + + return true; } // sets ahrs home to specified location // returns true if home location set successfully bool Rover::set_home(const Location& loc, bool lock) { - // set ahrs home - if (!ahrs.set_home(loc)) { + if (!AP_Vehicle::set_home(loc, lock)) { return false; } - // lock home position - if (lock) { - ahrs.lock_home(); - } - // Save Home to EEPROM mode_auto.mission.write_home_to_storage(); diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 829383dd310e4..6f4a10b6692ba 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -1098,6 +1098,49 @@ bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_lis } #endif +#if AP_AHRS_ENABLED +// set_home - sets ahrs home (used for RTL) to specified location +// returns true if home location set successfully +bool AP_Vehicle::set_home(const Location& loc, bool lock) +{ + // check EKF origin has been set + Location ekf_origin; + if (!ahrs.get_origin(ekf_origin)) { + return false; + } + + // set ahrs home (used for RTL) + if (!ahrs.set_home(loc)) { + return false; + } + + // lock home position + if (lock) { + ahrs.lock_home(); + } + + // return success + return true; +} + +// set_home_to_current_location - set home to current AHRS location +bool AP_Vehicle::set_home_to_current_location(bool lock) +{ + // get current location from AHRS + Location temp_loc; + if (!ahrs.get_location(temp_loc)) { + return false; + } + + if (!set_home(temp_loc, lock)) { + return false; + } + + return true; +} + +#endif // AP_AHRS_ENABLED + AP_Vehicle *AP_Vehicle::_singleton = nullptr; AP_Vehicle *AP_Vehicle::get_singleton() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 0ea6a802ca665..92a1137c1b206 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -290,8 +290,8 @@ 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; } + virtual bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; + virtual bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; #endif protected: