From dcc04ccd09c60c5d812e830418a97398e536677b Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 18 Feb 2021 15:12:25 +1100
Subject: [PATCH] autotest: stop using MAVProxy to change modes

---
 Tools/autotest/arducopter.py |  7 +++++--
 Tools/autotest/arduplane.py  | 12 ++++--------
 Tools/autotest/ardusub.py    |  8 +++-----
 Tools/autotest/common.py     | 15 +++++++++------
 4 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index a9d62c45e2..b06d344d4d 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -4768,8 +4768,11 @@ class AutoTestCopter(AutoTest):
         # arm in Stabilize and attempt to switch to Loiter
         self.change_mode('STABILIZE')
         self.arm_vehicle()
-        self.mavproxy.send("mode LOITER\n")
-        self.mavproxy.expect("requires position")
+        self.context_collect('STATUSTEXT')
+        self.run_cmd_do_set_mode(
+            "LOITER",
+            want_result=mavutil.mavlink.MAV_RESULT_FAILED)
+        self.wait_statustext("requires position", check_context=True)
         self.disarm_vehicle()
         self.context_pop()
         self.reboot_sitl()
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 2a636644d9..90966d9349 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -408,8 +408,7 @@ class AutoTestPlane(AutoTest):
         self.change_altitude(self.homeloc.alt+300)
         self.set_rc(2, 1500)
 
-        self.mavproxy.send("mode STABILIZE\n")
-        self.wait_mode('STABILIZE')
+        self.change_mode('STABILIZE')
 
         while count > 0:
             self.progress("Starting roll")
@@ -436,8 +435,7 @@ class AutoTestPlane(AutoTest):
         self.change_altitude(self.homeloc.alt+300)
         self.set_rc(2, 1500)
 
-        self.mavproxy.send("mode ACRO\n")
-        self.wait_mode('ACRO')
+        self.change_mode('ACRO')
 
         while count > 0:
             self.progress("Starting roll")
@@ -454,8 +452,7 @@ class AutoTestPlane(AutoTest):
 
         self.wait_level_flight()
 
-        self.mavproxy.send("mode ACRO\n")
-        self.wait_mode('ACRO')
+        self.change_mode('ACRO')
 
         count = 2
         while count > 0:
@@ -475,8 +472,7 @@ class AutoTestPlane(AutoTest):
 
     def test_FBWB(self, mode='FBWB'):
         """Fly FBWB or CRUISE mode."""
-        self.mavproxy.send("mode %s\n" % mode)
-        self.wait_mode(mode)
+        self.change_mode(mode)
         self.set_rc(3, 1700)
         self.set_rc(2, 1500)
 
diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index 705eb7b30b..0e10adeba2 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -106,8 +106,7 @@ class AutoTestSub(AutoTest):
         """
         self.wait_ready_to_arm()
         self.arm_vehicle()
-        self.mavproxy.send('mode ALT_HOLD\n')
-        self.wait_mode('ALT_HOLD')
+        self.change_mode('ALT_HOLD')
 
         msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
         if msg is None:
@@ -160,8 +159,7 @@ class AutoTestSub(AutoTest):
         self.arm_vehicle()
         # point North
         self.reach_heading_manual(0)
-        self.mavproxy.send('mode POSHOLD\n')
-        self.wait_mode('POSHOLD')
+        self.change_mode('POSHOLD')
 
         # dive a little
         self.set_rc(Joystick.Throttle, 1300)
@@ -278,7 +276,7 @@ class AutoTestSub(AutoTest):
                 return
 
             self.load_mission("sub-gripper-mission.txt")
-            self.mavproxy.send('mode loiter\n')
+            self.change_mode('LOITER')
             self.wait_ready_to_arm()
             self.arm_vehicle()
             self.change_mode('AUTO')
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 32662ca8e4..9520714850 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -4181,10 +4181,7 @@ class AutoTest(ABC):
             bearing += 360.00
         return bearing
 
-    def change_mode(self, mode, timeout=60):
-        '''change vehicle flightmode'''
-        self.wait_heartbeat()
-        self.progress("Changing mode to %s" % mode)
+    def send_cmd_do_set_mode(self, mode):
         self.send_cmd(
             mavutil.mavlink.MAV_CMD_DO_SET_MODE,
             mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
@@ -4193,8 +4190,14 @@ class AutoTest(ABC):
             0,
             0,
             0,
-            0,
+            0
         )
+
+    def change_mode(self, mode, timeout=60):
+        '''change vehicle flightmode'''
+        self.wait_heartbeat()
+        self.progress("Changing mode to %s" % mode)
+        self.send_cmd_do_set_mode(mode)
         tstart = self.get_sim_time()
         while not self.mode_is(mode):
             custom_num = self.mav.messages['HEARTBEAT'].custom_mode
@@ -4203,7 +4206,7 @@ class AutoTest(ABC):
             if (timeout is not None and
                     self.get_sim_time_cached() > tstart + timeout):
                 raise WaitModeTimeout("Did not change mode")
-            self.mavproxy.send('mode %s\n' % mode)
+            self.send_cmd_do_set_mode(mode)
         self.progress("Got mode %s" % mode)
 
     def capable(self, capability):