From d8c53bbd9c71783a78d4022efe26b9528af85987 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir@azurdrones.com>
Date: Mon, 6 Aug 2018 10:46:25 +0200
Subject: [PATCH] Tools: remove dependency on mavproxy for cmd_long based
 functions

---
 Tools/autotest/apmrover2.py |  4 +++
 Tools/autotest/common.py    | 57 ++++++++++++++++++++++++++++++-------
 2 files changed, 50 insertions(+), 11 deletions(-)

diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index 099a7e805c..8867011e14 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -747,6 +747,10 @@ class AutoTestRover(AutoTest):
 
             self.run_test("Set mode via MAV_COMMAND_DO_SET_MODE",
                           self.do_set_mode_via_command_long)
+            self.mavproxy.send('switch 6\n')  # Manual mode
+            self.wait_mode('MANUAL')
+            self.run_test("Set mode via MAV_COMMAND_DO_SET_MODE with MAVProxy",
+                          self.mavproxy_do_set_mode_via_command_long())
 
             self.run_test("Test ServoRelayEvents",
                           self.test_servorelayevents)
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 0c58fe01c8..3cf49888a6 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -673,20 +673,56 @@ class AutoTest(ABC):
         return bearing
 
     def do_get_autopilot_capabilities(self):
-        self.mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n")
-        m = self.mav.recv_match(type='AUTOPILOT_VERSION',
-                                blocking=True,
-                                timeout=10)
-        if m is None:
-            self.progress("AUTOPILOT_VERSION not received")
-            raise NotAchievedException()
-        self.progress("AUTOPILOT_VERSION received")
+        tstart = self.get_sim_time()
+        while self.get_sim_time() - tstart < 10:
+            # Cannot use run_cmd otherwise the respond is lost during the wait for ACK
+            self.mav.mav.command_long_send(1,
+                                           1,
+                                           mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
+                                           0,  # confirmation
+                                           1,  # 1: Request autopilot version
+                                           0,
+                                           0,
+                                           0,
+                                           0,
+                                           0,
+                                           0)
+            m = self.mav.recv_match(type='AUTOPILOT_VERSION',
+                                    blocking=True,
+                                    timeout=10)
+            if m is not None:
+                self.progress("AUTOPILOT_VERSION received")
+                return
+        raise AutoTestTimeoutException()
 
     def do_set_mode_via_command_long(self):
         base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
         custom_mode = 4  # hold
-        start = time.time()
-        while time.time() - start < 5:
+        tstart = self.get_sim_time()
+        while self.get_sim_time() - tstart < 5:
+            self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_MODE,
+                         base_mode,
+                         custom_mode,
+                         0,
+                         0,
+                         0,
+                         0,
+                         0,
+                         )
+            m = self.mav.recv_match(type='HEARTBEAT',
+                                    blocking=True,
+                                    timeout=10)
+            if m is None:
+                raise ErrorException()
+            if m.custom_mode == custom_mode:
+                return
+        raise AutoTestTimeoutException()
+
+    def mavproxy_do_set_mode_via_command_long(self):
+        base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
+        custom_mode = 4  # hold
+        start = self.get_sim_time()
+        while self.get_sim_time() - start < 5:
             self.mavproxy.send("long DO_SET_MODE %u %u\n" %
                                (base_mode, custom_mode))
             m = self.mav.recv_match(type='HEARTBEAT',
@@ -696,7 +732,6 @@ class AutoTest(ABC):
                 raise ErrorException()
             if m.custom_mode == custom_mode:
                 return
-            time.sleep(0.1)
         raise AutoTestTimeoutException()
 
     def reach_heading_manual(self, heading):