From 58ac9de94b169e41755d1257549bb4b2c20bfc74 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 5 Jan 2015 13:51:20 +0900
Subject: [PATCH] Copter: update home position when disarmed

This resolves the issue in which the vehicle's position jumped back to
it's home location when disarmed and using the EKF.  This also makes
copter consistent with plane.
---
 ArduCopter/ArduCopter.pde |  5 +++++
 ArduCopter/commands.pde   | 14 ++++++++++++++
 2 files changed, 19 insertions(+)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 0edd1eef40..691c33c238 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1211,6 +1211,11 @@ static void update_GPS(void)
                     // start again if we lose 3d lock
                     ground_start_count = 10;
                 }
+            } else {
+                // update home position when not armed
+                if (!motors.armed()) {
+                    update_home();
+                }
             }
 
             //If we are not currently armed, and we're ready to 
diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index 2481a0b7d0..3dc10b5e71 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -26,5 +26,19 @@ static void init_home()
     scaleLongUp   = 1.0f/scaleLongDown;
 }
 
+// update_home - reset home to current location
+//  should be called only when vehicle is disarmed
+static void update_home()
+{
+    // copter uses 0 home altitude
+    Location loc = gps.location();
+
+    // set ahrs object's home position
+    ahrs.set_home(loc);
+
+    // update navigation scalers.  used to offset the shrinking longitude as we go towards the poles
+    scaleLongDown = longitude_scale(loc);
+    scaleLongUp   = 1.0f/scaleLongDown;
+}