Tools: autotest: cope with being unable to reboot while armed

Tools/autotest/antennatracker.py

@@ -59,6 +59,9 @@ class AutoTestTracker(AutoTest):
 
     def reboot_sitl(self):
         """Reboot SITL instance and wait it to reconnect."""
+        self.wait_heartbeat()
+        if self.armed():
+            self.disarm_vehicle()
         self.mavproxy.send("reboot\n")
         self.mavproxy.expect("Initialising APM")
         # empty mav to avoid getting old timestamps:

Tools/autotest/apmrover2.py

@@ -269,8 +269,10 @@ class AutoTestRover(AutoTest):
 
             self.progress("Sprayer OK")
         except Exception as e:
+            self.progress("Caught exception: %s" % str(e))
             ex = e
         self.context_pop()
+        self.disarm_vehicle(force=True)
         self.reboot_sitl()
         if ex:
             raise ex
@@ -490,6 +492,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             ex = e
         self.context_pop()
         self.mavproxy.send("fence clear\n")
+        self.disarm_vehicle(force=True)
         self.reboot_sitl()
         if ex:
             raise ex
@@ -757,6 +760,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             ex = e
 
         self.context_pop()
+        self.disarm_vehicle()
         self.reboot_sitl()
 
         if ex is not None:

Tools/autotest/arducopter.py

@@ -482,6 +482,7 @@ class AutoTestCopter(AutoTest):
         self.wait_mode('LAND', timeout=timeout)
 
         self.progress("Successfully entered LAND after battery failsafe")
+        self.mav.motors_disarmed_wait()
         self.reboot_sitl()
 
     # fly_stability_patch - fly south, then hold loiter within 5m
@@ -810,6 +811,7 @@ class AutoTestCopter(AutoTest):
         self.progress("GPS glitch test passed!"
                       "  stayed within %u meters for %u seconds" %
                       (max_distance, timeout))
+        self.do_RTL()
         # re-arming is problematic because the GPS is glitching!
         self.reboot_sitl()
 
@@ -917,6 +919,7 @@ class AutoTestCopter(AutoTest):
             self.show_gps_and_sim_positions(False)
 
         self.progress("GPS Glitch test Auto completed: passed!")
+        self.mav.motors_disarmed_wait()
         # re-arming is problematic because the GPS is glitching!
         self.reboot_sitl()
 
@@ -1164,6 +1167,7 @@ class AutoTestCopter(AutoTest):
 
         self.set_rc(2, 1500)
         self.context_pop()
+        self.disarm_vehicle(force=True)
         self.reboot_sitl()
 
         if ex is not None:
@@ -2640,6 +2644,7 @@ class AutoTestCopter(AutoTest):
             ex = e
         self.context_pop()
 
+        self.disarm_vehicle(force=True)
         self.reboot_sitl() # to handle MNT_TYPE changing
 
         if ex is not None:
@@ -2734,6 +2739,7 @@ class AutoTestCopter(AutoTest):
 
         self.context_pop()
         self.zero_throttle()
+        self.disarm_vehicle(force=True)
         self.reboot_sitl()
         self.progress("All done")
 

Tools/autotest/arduplane.py

@@ -724,10 +724,8 @@ class AutoTestPlane(AutoTest):
             raise NotAchievedException("Sensor healthy when it shouldn't be")
         self.progress("Making RC work again")
         self.set_parameter("SIM_RC_FAIL", 0)
-        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        self.progress("Giving receiver time to recover")
-        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        for i in range(1, 10):
-        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
-        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
             m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
         self.progress("Testing receiver enabled")
         if (not (m.onboard_control_sensors_enabled & receiver_bit)):
@@ -773,6 +771,7 @@ class AutoTestPlane(AutoTest):
         self.wait_ready_to_arm()
         self.arm_vehicle()
         self.mavproxy.expect("BANG")
+        self.disarm_vehicle(force=True)
         self.reboot_sitl()
 
     def run_subtest(self, desc, func):

Tools/autotest/common.py

@@ -291,10 +291,35 @@ class AutoTest(ABC):
         self.mavproxy.send("param fetch\n")
         self.mavproxy.expect("Received [0-9]+ parameters")
 
+    def reboot_sitl_mav(self):
+        """Reboot SITL instance using mavlink and wait for it to reconnect."""
+        old_bootcount = self.get_parameter('STAT_BOOTCNT')
+        # ardupilot SITL may actually NAK the reboot; replace with
+        # run_cmd when we don't do that.
+        self.mav.mav.command_long_send(self.sysid_thismav(),
+                                       1,
+                                       mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
+                                       1,  # confirmation
+                                       1, # reboot autopilot
+                                       0,
+                                       0,
+                                       0,
+                                       0,
+                                       0,
+                                       0)
+        self.detect_and_handle_reboot(old_bootcount)
+
     def reboot_sitl(self):
-        """Reboot SITL instance and wait it to reconnect."""
+        """Reboot SITL instance and wait for it to reconnect."""
+        self.reboot_sitl_mav()
+
+    def reboot_sitl_mavproxy(self):
+        """Reboot SITL instance using MAVProxy and wait for it to reconnect."""
         old_bootcount = self.get_parameter('STAT_BOOTCNT')
         self.mavproxy.send("reboot\n")
+        self.detect_and_handle_reboot(old_bootcount)
+
+    def detect_and_handle_reboot(self, old_bootcount):
         tstart = time.time()
         while True:
             if time.time() - tstart > 10:
@@ -439,7 +464,7 @@ class AutoTest(ABC):
         return m.time_boot_ms * 1.0e-3
 
     def get_sim_time_cached(self):
-        """Get SITL time."""
+        """Get SITL time in seconds."""
         x = self.mav.messages.get("SYSTEM_TIME", None)
         if x is None:
             return self.get_sim_time()
@@ -836,12 +861,15 @@ class AutoTest(ABC):
                 return True
         raise AutoTestTimeoutException("Failed to ARM with mavlink")
 
-    def disarm_vehicle(self, timeout=20):
+    def disarm_vehicle(self, timeout=60, force=False):
         """Disarm vehicle with mavlink disarm message."""
         self.progress("Disarm motors with MAVLink cmd")
+        p2 = 0
+        if force:
+            p2 = 21196 # magic force disarm value
         self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
                      0,  # DISARM
-                     0,
+                     p2,
                      0,
                      0,
                      0,
@@ -1098,7 +1126,7 @@ class AutoTest(ABC):
                 raise AutoTestTimeoutException("Did not get good COMMAND_ACK within %fs" % timeout)
             m = self.mav.recv_match(type='COMMAND_ACK',
                                     blocking=True,
-                                    timeout=1)
+                                    timeout=0.1)
             if m is None:
                 continue
             if not quiet:
@@ -1703,6 +1731,8 @@ class AutoTest(ABC):
     def check_sitl_reset(self):
         self.wait_heartbeat()
         if self.armed():
+            self.progress("Armed at end of test; force-rebooting SITL")
+            self.disarm_vehicle(force=True)
             self.forced_post_test_sitl_reboots += 1
             self.progress("Force-resetting SITL")
             self.reboot_sitl() # that'll learn it
@@ -1897,6 +1927,9 @@ class AutoTest(ABC):
                           (m.groundspeed, want))
 
     def fly_test_set_home(self):
+        if self.is_tracker():
+            # tracker starts armed...
+            self.disarm_vehicle(force=True)
         self.reboot_sitl()
 
         # HOME_POSITION is used as a surrogate for origin until we