diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index f3d0c520c1..186a17d0a0 100644 --- a/Tools/autotest/antennatracker.py +++ b/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: diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 333e8efd59..745441328d 100644 --- a/Tools/autotest/apmrover2.py +++ b/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: diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index fef184ccda..7b44ef6162 100644 --- a/Tools/autotest/arducopter.py +++ b/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") diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 0e78f70b25..dd6e72e314 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -724,11 +724,9 @@ 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) - 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) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Giving receiver time to recover") + for i in range(1, 10): + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Testing receiver enabled") if (not (m.onboard_control_sensors_enabled & receiver_bit)): raise NotAchievedException("Receiver not enabled") @@ -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): diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 83c5db255b..98ddb0e8bc 100644 --- a/Tools/autotest/common.py +++ b/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