diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index e29008a7af..60d88f3a03 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -164,8 +164,10 @@ void Rover::ahrs_update() // update position have_position = ahrs.get_position(current_loc); - // update home from EKF if necessary - update_home_from_EKF(); + // set home from EKF if necessary and possible + if (!ahrs.home_is_set()) { + set_home_to_current_location(false); + } // if using the EKF get a speed update now (from accelerometers) Vector3f velocity; @@ -281,10 +283,8 @@ void Rover::one_second_loop(void) // cope with changes to mavlink system ID mavlink_system.sysid = g.sysid_this_mav; - // update home position if not soft armed and gps position has - // changed. Update every 1s at most - if (!hal.util->get_soft_armed() && - gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + // attempt to update home position and baro calibration if not armed: + if (!hal.util->get_soft_armed()) { update_home(); } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 832354ef4c..b27be1cbe9 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -406,7 +406,6 @@ private: void update_mission(void); // commands.cpp - void update_home_from_EKF(); bool set_home_to_current_location(bool lock); bool set_home(const Location& loc, bool lock); void update_home(); diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index 4f2b762484..cd44c99adf 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -1,21 +1,8 @@ #include "Rover.h" -// checks if we should update ahrs home position from the EKF's position -void Rover::update_home_from_EKF() -{ - // exit immediately if home already set - if (ahrs.home_is_set()) { - return; - } - - // move home to current ekf location (this will set home_state to HOME_SET) - set_home_to_current_location(false); -} - -// set ahrs home to current location from EKF reported location or GPS +// set ahrs home to current location from inertial-nav location bool Rover::set_home_to_current_location(bool lock) { - // use position from EKF if available otherwise use GPS Location temp_loc; if (ahrs.have_inertial_nav() && ahrs.get_position(temp_loc)) { if (!set_home(temp_loc, lock)) { @@ -40,12 +27,6 @@ bool Rover::set_home(const Location& loc, bool lock) return false; } - // check if EKF origin has been set - Location ekf_origin; - if (!ahrs.get_origin(ekf_origin)) { - return false; - } - const bool home_was_set = ahrs.home_is_set(); // set ahrs home @@ -79,20 +60,27 @@ bool Rover::set_home(const Location& loc, bool lock) return true; } -/* - update home location from GPS - this is called as long as we have 3D lock and the arming switch is - not pushed -*/ +// called periodically while disarmed to update our home position to +// our current location void Rover::update_home() { - if (!ahrs.home_is_locked()) { - Location loc; - if (ahrs.get_position(loc)) { - if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) { - ahrs.set_home(loc); - } - } + if (ahrs.home_is_locked()) { + // we've been explicitly told our home location + return; } + + Location loc{}; + if (!ahrs.get_position(loc)) { + return; + } + barometer.update_calibration(); + + if (ahrs.home_is_set() && + get_distance(loc, ahrs.get_home()) < DISTANCE_HOME_MINCHANGE) { + // insufficiently moved from current home - don't change it + return; + } + + ahrs.set_home(loc); } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 7626f74bd1..8d4cd7619c 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -109,7 +109,7 @@ enum fs_ekf_action { FS_EFK_HOLD = 1 }; -#define DISTANCE_HOME_MAX 0.5f // Distance max to home location before changing it when disarm +#define DISTANCE_HOME_MINCHANGE 0.5f // minimum distance to adjust home location enum mode_reason_t { MODE_REASON_INITIALISED = 0,