From 2529c7fc40e634a3b8fd3987bc0867711d7a7bc8 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 14 Jul 2022 11:09:08 +1000
Subject: [PATCH] autotest: add test for RESET_MODE_SWITCH

---
 Tools/autotest/arduplane.py | 26 ++++++++++++++++++++++++++
 Tools/autotest/common.py    |  4 ++++
 2 files changed, 30 insertions(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 5b560493b6..22caf7f626 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -1296,6 +1296,31 @@ class AutoTestPlane(AutoTest):
             on_radius_start_heading = None
             circle_time_start = 0
 
+    def MODE_SWITCH_RESET(self):
+        '''test the MODE_SWITCH_RESET auxiliary function'''
+        self.set_parameters({
+            "RC9_OPTION": 96,
+        })
+
+        self.progress("Using RC to change modes")
+        self.set_rc(8, 1500)
+        self.wait_mode('FBWA')
+
+        self.progress("Killing RC to engage RC failsafe")
+        self.set_parameter('SIM_RC_FAIL', 1)
+        self.wait_mode('RTL')
+
+        self.progress("Reinstating RC")
+        self.set_parameter('SIM_RC_FAIL', 0)
+
+        self.progress("Ensuring we don't automatically revert mode")
+        self.delay_sim_time(2)
+        self.assert_mode_is('RTL')
+
+        self.progress("Ensuring MODE_SWITCH_RESET switch resets to pre-failsafe mode")
+        self.set_rc(9, 2000)
+        self.wait_mode('FBWA')
+
     def FenceStatic(self):
         '''Test Basic Fence Functionality'''
         ex = None
@@ -4572,6 +4597,7 @@ class AutoTestPlane(AutoTest):
             self.GCSFailsafe,
             self.SDCardWPTest,
             self.NoArmWithoutMissionItems,
+            self.MODE_SWITCH_RESET,
         ])
         return ret
 
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 9d96f89861..ae12722a59 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -7213,6 +7213,10 @@ class AutoTest(ABC):
                 raise WaitModeTimeout("Did not change mode")
         self.progress("Got mode %s" % mode)
 
+    def assert_mode_is(self, mode):
+        if not self.mode_is(mode):
+            raise NotAchievedException("Expected mode %s" % str(mode))
+
     def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30):
         self.progress("Waiting for GPS health")
         tstart = self.get_sim_time_cached()