Copter: lock home altitude to origin altitude if home was set on arming

ArduCopter/ArduCopter.pde

@@ -382,7 +382,8 @@ static union {
         uint8_t system_time_set     : 1; // 20  // true if the system time has been set from the GPS
         uint8_t gps_base_pos_set    : 1; // 21  // true when the gps base position has been set (used for RTK gps only)
         enum HomeState home_state   : 2; // 22,23 - home status (unset, set, locked)
-        uint8_t in_ground_effect    : 1; // 24
+        uint8_t home_set_by_ap      : 1; // 24
+        uint8_t in_ground_effect    : 1; // 25
     };
     uint32_t value;
 } ap;

ArduCopter/commands.pde

@@ -33,7 +33,7 @@ static void update_home_from_GPS()
     }
 
     // move home to current gps location (this will set home_state to HOME_SET)
-    set_home(gps.location());
+    set_home_to_current_location();
 }
 
 // set_home_to_current_location - set home to current GPS location
@@ -44,7 +44,7 @@ static bool set_home_to_current_location() {
     }
 
     // set home to latest gps location
-    return set_home(gps.location());
+    return set_home(gps.location(), true);
 }
 
 // set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
@@ -61,8 +61,9 @@ static bool set_home_to_current_location_and_lock()
 //  unless this function is called again
 static bool set_home_and_lock(const Location& loc)
 {
-    if (set_home(loc)) {
+    if (set_home(loc, false)) {
         set_home_state(HOME_SET_AND_LOCKED);
+        ap.home_set_by_ap = false;
         return true;
     }
     return false;
@@ -71,13 +72,15 @@ static bool set_home_and_lock(const Location& loc)
 // 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
-static bool set_home(const Location& loc)
+static bool set_home(const Location& loc, bool set_by_ap)
 {
     // check location is valid
     if (loc.lat == 0 && loc.lng == 0) {
         return false;
     }
 
+    ap.home_set_by_ap = set_by_ap;
+
     // set ahrs home (used for RTL)
     ahrs.set_home(loc);
 

ArduCopter/commands_logic.pde

@@ -868,7 +868,7 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
         set_home_to_current_location();
     } else {
         if (!far_from_EKF_origin(cmd.content.location)) {
-            set_home(cmd.content.location);
+            set_home(cmd.content.location, false);
         }
     }
 }

ArduCopter/position_vector.pde

@@ -47,7 +47,14 @@ float pv_alt_above_origin(float alt_above_home_cm)
 float pv_alt_above_home(float alt_above_origin_cm)
 {
     const struct Location &origin = inertial_nav.get_origin();
-    return alt_above_origin_cm + (origin.alt - ahrs.get_home().alt);
+    struct Location home = ahrs.get_home();
+
+    if(ap.home_set_by_ap && origin.alt != home.alt) {
+        home.alt = origin.alt;
+        ahrs.set_home(home);
+    }
+
+    return alt_above_origin_cm + (origin.alt - home.alt);
 }
 
 // pv_get_bearing_cd - return bearing in centi-degrees between two positions