diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 23235fda06..48a0dfcb5e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1177,7 +1177,7 @@ static void update_GPS(void) set_system_time_from_GPS(); // update home from GPS location if necessary - update_home_from_GPS(); + update_home_from_EKF(); // check gps base position (used for RTK only) check_gps_base_pos(); diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 3b256662a1..ccedac07ad 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -7,44 +7,26 @@ * HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command */ -static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before accepting a GPS location as home location - -// checks if we should update ahrs/RTL home position from GPS -static void update_home_from_GPS() +// checks if we should update ahrs/RTL home position from the EKF +static void update_home_from_EKF() { - // exit immediately if counter has run down and home already set - if (ground_start_count == 0 && ap.home_state != HOME_UNSET) { + // exit immediately if home already set + if (ap.home_state != HOME_UNSET) { return; } - // if counter has not run down - if (ground_start_count > 0) { - - // reset counter if we do not have GPS lock - if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { - ground_start_count = 10; - - // count down for 10 consecutive locks - } else { - ground_start_count--; - } - - return; - } - - // move home to current gps location (this will set home_state to HOME_SET) - set_home(gps.location()); + // move home to current ekf location (this will set home_state to HOME_SET) + set_home_to_current_location(); } // set_home_to_current_location - set home to current GPS location static bool set_home_to_current_location() { - // exit with failure if we haven't had 10 good GPS position - if (ground_start_count > 0) { - return false; + // get current location from EKF + Location temp_loc; + if (inertial_nav.get_location(temp_loc)) { + return set_home(temp_loc); } - - // set home to latest gps location - return set_home(gps.location()); + return false; } // set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved