Plane: cope with home altitude change while navigating

this fixes a bug where a change of home altitude would cause a sudden
height demand change. This copes with 3 situations:

 - flying with AMSL alt demand. Changing home altitude makes for no change
 - flying with AGL alt demand. Changing home altitude requires update of next_WP_loc
 - flying with home relative alt demand. Changing home altitude changes demand at end of current navigation leg

ArduPlane/Plane.h

@@ -507,6 +507,9 @@ private:
 
         // how much correction have we added for terrain data
         float terrain_correction;
+
+        // last home altitude for detecting changes
+        int32_t last_home_alt_cm;
     } auto_state;
 
 #if AP_SCRIPTING_ENABLED
@@ -1036,6 +1039,7 @@ private:
     void loiter_angle_reset(void);
     void loiter_angle_update(void);
     void navigate();
+    void check_home_alt_change(void);
     void calc_airspeed_errors();
     float mode_auto_target_airspeed_cm();
     void calc_gndspeed_undershoot();

ArduPlane/altitude.cpp

@@ -28,6 +28,27 @@ void Plane::adjust_altitude_target()
     control_mode->update_target_altitude();
 }
 
+void Plane::check_home_alt_change(void)
+{
+    int32_t home_alt_cm = ahrs.get_home().alt;
+    if (home_alt_cm != auto_state.last_home_alt_cm && hal.util->get_soft_armed()) {
+        // cope with home altitude changing
+        const int32_t alt_change_cm = home_alt_cm - auto_state.last_home_alt_cm;
+        if (next_WP_loc.terrain_alt) {
+            /*
+              next_WP_loc for terrain alt WP are quite strange. They
+              have terrain_alt=1, but also have relative_alt=0 and
+              have been calculated to be relative to home. We need to
+              adjust for the change in home alt
+             */
+            next_WP_loc.alt += alt_change_cm;
+        }
+        // reset TECS to force the field elevation estimate to reset
+        TECS_controller.reset();
+    }
+    auto_state.last_home_alt_cm = home_alt_cm;
+}
+
 /*
   setup for a gradual glide slope to the next waypoint, if appropriate
  */

ArduPlane/navigation.cpp

@@ -95,6 +95,8 @@ void Plane::navigate()
         return;
     }
 
+    check_home_alt_change();
+
     // waypoint distance from plane
     // ----------------------------
     auto_state.wp_distance = current_loc.get_distance(next_WP_loc);