From b03203c61267868d17c30f6595f429643524cceb Mon Sep 17 00:00:00 2001
From: Bill Geyer <bnsgeyer@users.noreply.github.com>
Date: Mon, 22 Apr 2024 07:27:25 -0400
Subject: [PATCH] Tools: heli autotune autotest save gains and add tune check

---
 Tools/autotest/helicopter.py | 40 ++++++++++++++++++++++++++++++------
 1 file changed, 34 insertions(+), 6 deletions(-)

diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py
index b237a48827..279e33413b 100644
--- a/Tools/autotest/helicopter.py
+++ b/Tools/autotest/helicopter.py
@@ -876,7 +876,7 @@ class AutoTestHelicopter(AutoTestCopter):
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
-        self.land_and_disarm()
+        self.autotune_land_and_save_gains()
 
         # test pitch rate P and Rate D tuning
         self.set_parameters({
@@ -895,7 +895,7 @@ class AutoTestHelicopter(AutoTestCopter):
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
-        self.land_and_disarm()
+        self.autotune_land_and_save_gains()
 
         # test Roll rate P and Rate D tuning
         self.set_parameters({
@@ -914,7 +914,7 @@ class AutoTestHelicopter(AutoTestCopter):
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
-        self.land_and_disarm()
+        self.autotune_land_and_save_gains()
 
         # test Roll and pitch angle P tuning
         self.set_parameters({
@@ -935,7 +935,7 @@ class AutoTestHelicopter(AutoTestCopter):
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
-        self.land_and_disarm()
+        self.autotune_land_and_save_gains()
 
         # test yaw FF and rate P and Rate D
         self.set_parameters({
@@ -956,8 +956,7 @@ class AutoTestHelicopter(AutoTestCopter):
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
-        self.land_and_disarm()
-        self.set_rc(8, 1000)
+        self.autotune_land_and_save_gains()
 
         # test yaw angle P tuning
         self.set_parameters({
@@ -974,12 +973,41 @@ class AutoTestHelicopter(AutoTestCopter):
         # hold position in loiter
         self.change_mode('AUTOTUNE')
 
+        tstart = self.get_sim_time()
+        self.wait_statustext('AutoTune: Success', timeout=1000)
+        now = self.get_sim_time()
+        self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
+        self.autotune_land_and_save_gains()
+
+        # tune check
+        self.set_parameters({
+            "AUTOTUNE_AXES": 7,
+            "AUTOTUNE_SEQ": 16,
+            "AUTOTUNE_FRQ_MIN": 10,
+            "AUTOTUNE_FRQ_MAX": 80,
+            })
+
+        # Conduct testing from althold
+        self.takeoff(10, mode="ALT_HOLD")
+
+        # hold position in loiter
+        self.change_mode('AUTOTUNE')
+
         tstart = self.get_sim_time()
         self.wait_statustext('AutoTune: Success', timeout=1000)
         now = self.get_sim_time()
         self.progress("AUTOTUNE OK (%u seconds)" % (now - tstart))
         self.land_and_disarm()
 
+    def autotune_land_and_save_gains(self):
+        self.set_rc(3, 1000)
+        self.context_collect('STATUSTEXT')
+        self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
+                             check_context=True,
+                             regex=True)
+        self.set_rc(8, 1000)
+        self.wait_disarmed()
+
     def land_and_disarm(self, **kwargs):
         super(AutoTestHelicopter, self).land_and_disarm(**kwargs)
         self.progress("Killing rotor speed")