From 2bec00e1c528de10750d1323d955320e56d1695a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 25 Feb 2015 23:13:59 +0900
Subject: [PATCH] Copter: set ahrs home from ekf location

---
 ArduCopter/ArduCopter.pde |  2 +-
 ArduCopter/commands.pde   | 40 +++++++++++----------------------------
 2 files changed, 12 insertions(+), 30 deletions(-)

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