From f3d6d8e23647446c47f3abef7e925d611769cacd Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 10 May 2018 17:15:38 +1000
Subject: [PATCH] Tools: autotest: make Copter tests more reliable

RTL may disarm the vehicle on completion.  We RTL at several times in
the testing, and the subsequent tests were not rearming.  This means we
had a race condition.

We now explicitly wait to be disarmed by the RTL mode, and rearm the
vehicle.

This is an interim patch until we decide whether to make each "test"
self-contained, and have a precondition of "on ground and disarmed".
---
 Tools/autotest/arducopter.py | 31 ++++++++++++++++++++++++++-----
 Tools/autotest/common.py     |  4 ++++
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 0b4be36fb7..186877a440 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -144,10 +144,13 @@ class AutoTestCopter(AutoTest):
         if self.copy_tlog:
             shutil.copy(self.logfile, self.buildlog)
 
-    def takeoff(self, alt_min=30, takeoff_throttle=1700):
+    def takeoff(self, alt_min=30, takeoff_throttle=1700, arm=False):
         """Takeoff get to 30m altitude."""
         self.mavproxy.send('switch 6\n')  # stabilize mode
         self.wait_mode('STABILIZE')
+        if arm:
+            self.set_rc(3, 1000)
+            self.arm_vehicle()
         self.set_rc(3, takeoff_throttle)
         m = self.mav.recv_match(type='VFR_HUD', blocking=True)
         if m.alt < alt_min:
@@ -310,6 +313,7 @@ class AutoTestCopter(AutoTest):
         self.wait_altitude(-10, 10, time_left)
         self.save_wp()
 
+    # enter RTL mode and wait for the vehicle to disarm
     def fly_RTL(self, side=60, timeout=250):
         """Return, land."""
         self.progress("# Enter RTL")
@@ -319,8 +323,13 @@ class AutoTestCopter(AutoTest):
             m = self.mav.recv_match(type='VFR_HUD', blocking=True)
             pos = self.mav.location()
             home_distance = self.get_distance(HOME, pos)
-            self.progress("Alt: %u  HomeDist: %.0f" % (m.alt, home_distance))
+            home = ""
             if m.alt <= 1 and home_distance < 10:
+                home = "HOME"
+            self.progress("Alt: %u  HomeDist: %.0f %s" %
+                          (m.alt, home_distance, home))
+            # our post-condition is that we are disarmed:
+            if not self.armed():
                 return
         raise AutoTestTimeoutException()
 
@@ -1088,6 +1097,12 @@ class AutoTestCopter(AutoTest):
             # RTL after GPS Glitch Loiter test
             self.run_test("RTL after GPS Glitch Loiter test", self.fly_RTL)
 
+            # Arm
+            self.mavproxy.send('mode stabilize\n')  # stabilize mode
+            self.wait_mode('STABILIZE')
+            self.set_rc(3, 1000)
+            self.run_test("Arm motors", self.arm_vehicle)
+
             # Fly GPS Glitch test in auto mode
             self.run_test("GPS Glitch Auto Test",
                           self.fly_gps_glitch_auto_test)
@@ -1110,7 +1125,7 @@ class AutoTestCopter(AutoTest):
 
             # Takeoff
             self.run_test("Takeoff to test fly SIMPLE mode",
-                          lambda: self.takeoff(10))
+                          lambda: self.takeoff(10, arm=True))
 
             # Simple mode
             self.run_test("Fly in SIMPLE mode", self.fly_simple)
@@ -1120,7 +1135,7 @@ class AutoTestCopter(AutoTest):
 
             # Takeoff
             self.run_test("Takeoff to test circle in SUPER SIMPLE mode",
-                          lambda: self.takeoff(10))
+                          lambda: self.takeoff(10, arm=True))
 
             # Fly a circle in super simple mode
             self.run_test("Fly a circle in SUPER SIMPLE mode",
@@ -1131,7 +1146,7 @@ class AutoTestCopter(AutoTest):
 
             # Takeoff
             self.run_test("Takeoff to test CIRCLE mode",
-                          lambda: self.takeoff(10))
+                          lambda: self.takeoff(10, arm=True))
 
             # Circle mode
             self.run_test("Fly CIRCLE mode", self.fly_circle)
@@ -1139,6 +1154,12 @@ class AutoTestCopter(AutoTest):
             # RTL
             self.run_test("RTL after CIRCLE mode", self.fly_RTL)
 
+            # Arm
+            self.set_rc(3, 1000)
+            self.mavproxy.send('mode stabilize\n')  # stabilize mode
+            self.wait_mode('STABILIZE')
+            self.run_test("Arm motors", self.arm_vehicle)
+
             # Fly auto test
             self.run_test("Fly copter mission", self.fly_auto_test)
 
diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py
index 0f6d6ecbd3..88fc9b4f7b 100644
--- a/Tools/autotest/common.py
+++ b/Tools/autotest/common.py
@@ -327,6 +327,10 @@ class AutoTest(ABC):
         self.progress("Failed to send RC commands to channel %s" % str(chan))
         raise SetRCTimeout()
 
+    def armed(self):
+        '''Return true if vehicle is armed and safetyoff'''
+        return self.mav.motors_armed();
+
     def arm_vehicle(self):
         """Arm vehicle with mavlink arm message."""
         self.mavproxy.send('arm throttle\n')