TECS: Correctly handle Home altitude change

ArduPlane/altitude.cpp

@@ -44,7 +44,7 @@ void Plane::check_home_alt_change(void)
             next_WP_loc.alt += alt_change_cm;
         }
         // reset TECS to force the field elevation estimate to reset
-        TECS_controller.reset();
+        TECS_controller.offset_altitude(alt_change_cm * 0.01f);
     }
     auto_state.last_home_alt_cm = home_alt_cm;
 }

Tools/autotest/arduplane.py

@@ -5,6 +5,7 @@
 '''
 
 from __future__ import print_function
+import copy
 import math
 import os
 import signal
@@ -6190,10 +6191,43 @@ def SetHomeAltChange(self):
             higher_home = home
             higher_home.alt += 40
             self.set_home(higher_home)
+            self.change_mode(mode)
             self.wait_altitude(15, 25, relative=True, minimum_duration=10)
             self.disarm_vehicle(force=True)
             self.reboot_sitl()
 
+    def SetHomeAltChange2(self):
+        '''ensure TECS operates predictably as home altitude changes continuously'''
+        '''
+        This can happen when performing a ship landing, where the home
+        coordinates are continuously set by the ship GNSS RX.
+        '''
+        self.set_parameter('TRIM_THROTTLE', 70)
+        self.wait_ready_to_arm()
+        home = self.home_position_as_mav_location()
+        target_alt = 20
+        self.takeoff(target_alt, mode="TAKEOFF")
+        self.change_mode("LOITER")
+        self.delay_sim_time(20) # Let the plane settle.
+
+        tstart = self.get_sim_time()
+        test_time = 10 # Run the test for 10s.
+        pub_freq = 10
+        for i in range(test_time*pub_freq):
+            tnow = self.get_sim_time()
+            higher_home = copy.copy(home)
+            # Produce 1Hz sine waves in home altitude change.
+            higher_home.alt += 40*math.sin((tnow-tstart)*(2*math.pi))
+            self.set_home(higher_home)
+            if tnow-tstart > test_time:
+                break
+            self.delay_sim_time(1.0/pub_freq)
+
+        # Test if the altitude is still within bounds.
+        self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=1, timeout=2)
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def ForceArm(self):
         '''check force-arming functionality'''
         self.set_parameter("SIM_GPS_DISABLE", 1)
@@ -6462,6 +6496,7 @@ def tests1b(self):
             self.ScriptStats,
             self.GPSPreArms,
             self.SetHomeAltChange,
+            self.SetHomeAltChange2,
             self.ForceArm,
             self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
             self.GliderPullup,

libraries/AP_TECS/AP_TECS.cpp

@@ -1124,7 +1124,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
         _integKE              = 0.0f;
         _last_throttle_dem    = aparm.throttle_cruise * 0.01f;
         _last_pitch_dem       = _ahrs.get_pitch();
-        _hgt_afe              = hgt_afe;
         _hgt_dem_in_prev      = hgt_afe;
         _hgt_dem_lpf          = hgt_afe;
         _hgt_dem_rate_ltd     = hgt_afe;
@@ -1501,4 +1500,32 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
 
     // don't allow max pitch to go below min pitch
     _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
+}
+
+void AP_TECS::offset_altitude(const float alt_offset)
+{
+    // Convention: When alt_offset is positive it means that the altitude of
+    // home has increased. Thus, the relative altitude of the vehicle has
+    // decreased.
+    //
+    // Assumption: This method is called more often and before
+    // `update_pitch_throttle()`. This is necessary to ensure that new height
+    // demands which incorporate the home change are compatible with the
+    // (now updated) internal height state.
+
+    _flare_hgt_dem_ideal    -= alt_offset;
+    _flare_hgt_dem_adj      -= alt_offset;
+    _hgt_at_start_of_flare  -= alt_offset;
+    _hgt_dem_in_prev        -= alt_offset;
+    _hgt_dem_lpf            -= alt_offset;
+    _hgt_dem_rate_ltd       -= alt_offset;
+    _hgt_dem_prev           -= alt_offset;
+    _height_filter.height   -= alt_offset;
+
+    // The following variables are updated anew in every call of
+    // `update_pitch_throttle()`. There's no need to update those.
+    // _hgt_dem
+    // _hgt_dem_in_raw
+    // _hgt_dem_in
+    // Energies
 }

libraries/AP_TECS/AP_TECS.h

@@ -154,6 +154,9 @@ class AP_TECS {
         _need_reset = true;
     }
 
+    // Apply an altitude offset, to compensate for changes in home alt.
+    void offset_altitude(const float alt_offset);
+
     // this supports the TECS_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];