From 2024eba262480da9b72ea85aa69e22ffe09169e0 Mon Sep 17 00:00:00 2001
From: George Zogopoulos <tailwhipx4@gmail.com>
Date: Mon, 25 Nov 2024 16:05:15 +0100
Subject: [PATCH] autotest: Added TECS test for constantly changing home alt

---
 Tools/autotest/arduplane.py | 35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 0809d457d2..a724a9a45a 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -5,6 +5,7 @@ AP_FLAKE8_CLEAN
 '''
 
 from __future__ import print_function
+import copy
 import math
 import os
 import signal
@@ -6241,10 +6242,43 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
             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)
@@ -6513,6 +6547,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
             self.ScriptStats,
             self.GPSPreArms,
             self.SetHomeAltChange,
+            self.SetHomeAltChange2,
             self.ForceArm,
             self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
             self.GliderPullup,