From 8bee74b8f83cd09454d04d448deab6d9f88ba84b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 21 May 2024 10:12:31 +1000
Subject: [PATCH] autotest: add test showing throttle saturation problem

autotest: verbose minimum_duration in wait_message_field_values
---
 Tools/autotest/arduplane.py          | 18 ++++++++++++++++++
 Tools/autotest/vehicle_test_suite.py |  9 ++++++++-
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index b05f565a5a2980..ba7a2eed20db4a 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -5340,6 +5340,22 @@ def MinThrottle(self):
         self.drain_mav()
         self.assert_servo_channel_value(3, servo_min)
 
+    def ClimbThrottleSaturation(self):
+        '''check what happens when throttle is saturated in GUIDED'''
+        self.set_parameters({
+            "TECS_CLMB_MAX": 30,
+            "TKOFF_ALT": 1000,
+        })
+
+        self.change_mode("TAKEOFF")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.wait_message_field_values('VFR_HUD', {
+            "throttle": 100,
+        }, minimum_duration=30, timeout=90)
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -5449,6 +5465,7 @@ def tests(self):
             self.MAV_CMD_NAV_LOITER_UNLIM,
             self.MAV_CMD_NAV_RETURN_TO_LAUNCH,
             self.MinThrottle,
+            self.ClimbThrottleSaturation,
         ])
         return ret
 
@@ -5456,4 +5473,5 @@ def disabled_tests(self):
         return {
             "LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054",
             "InteractTest": "requires user interaction",
+            "ClimbThrottleSaturation": "requires https://github.com/ArduPilot/ardupilot/pull/27106 to pass",
         }
diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py
index ebc16cf3b2f53f..b20e4dc37b4e33 100644
--- a/Tools/autotest/vehicle_test_suite.py
+++ b/Tools/autotest/vehicle_test_suite.py
@@ -4225,6 +4225,7 @@ def wait_message_field_values(self,
 
         tstart = self.get_sim_time_cached()
         pass_start = None
+        last_debug = 0
         while True:
             now = self.get_sim_time_cached()
             if now - tstart > timeout:
@@ -4240,8 +4241,14 @@ def wait_message_field_values(self,
                     if pass_start is None:
                         pass_start = now
                         continue
-                    if now - pass_start < minimum_duration:
+                    delta = now - pass_start
+                    if now - last_debug >= 1:
+                        last_debug = now
+                        self.progress(f"Good field values ({delta:.2f}s/{minimum_duration}s)")
+                    if delta < minimum_duration:
                         continue
+                else:
+                    self.progress("Reached field values")
                 return m
             pass_start = None