mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Tools: autotest: cope with being unable to reboot while armed
This commit is contained in:
parent
778b88cba2
commit
6bf600c587
@ -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:
|
||||
|
@ -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:
|
||||
|
@ -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")
|
||||
|
||||
|
@ -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):
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user