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

This commit is contained in:
Peter Barker 2019-02-26 15:29:12 +11:00 committed by Andrew Tridgell
parent 778b88cba2
commit 6bf600c587
5 changed files with 55 additions and 10 deletions

View File

@ -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:

View File

@ -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:

View File

@ -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")

View File

@ -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):

View File

@ -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