Tools: autotest: drain mav during RTL test to improve reliability

This commit is contained in:
Peter Barker 2019-03-07 12:35:53 +11:00 committed by Peter Barker
parent c04d2145a1
commit f2e3d377ad
2 changed files with 24 additions and 13 deletions

View File

@ -449,11 +449,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
mission_filepath = os.path.join("ArduRover-Missions", "rtl.txt")
self.load_mission(mission_filepath)
self.mavproxy.send('switch 4\n') # auto mode
self.set_rc(3, 1500)
self.wait_mode('AUTO')
self.change_mode("AUTO")
self.mavproxy.expect('Executing RTL')
self.drain_mav();
m = self.mav.recv_match(type='NAV_CONTROLLER_OUTPUT',
blocking=True,
timeout=0.1)
@ -470,7 +470,17 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" %
(m.wp_dist, wp_dist_min,))
self.wait_mode('HOLD', timeout=600) # balancebot can take a long time!
tstart = self.get_sim_time()
while True:
if self.get_sim_time_cached() - tstart > 600:
raise NotAchievedException("Did not get home")
self.progress("Distance home: %f (mode=%s)" %
(self.distance_to_home(), self.mav.flightmode))
if self.mode_is('HOLD'):
break
# the EKF doesn't pull us down to 0 speed:
self.wait_groundspeed(0, 0.5, timeout=600)
home_distance = self.distance_to_home()
home_distance_max = 5
@ -478,8 +488,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
raise NotAchievedException(
"Did not get home (%f metres distant > %f)" %
(home_distance, home_distance_max))
self.mavproxy.send('switch 6\n')
self.wait_mode('MANUAL')
self.disarm_vehicle()
self.progress("RTL Mission OK")

View File

@ -1045,14 +1045,15 @@ class AutoTest(ABC):
def run_cmd_get_ack(self, command, want_result, timeout):
tstart = self.get_sim_time_cached()
while True:
if self.get_sim_time_cached() - tstart > timeout:
raise AutoTestTimeoutException("Did not get good COMMAND_ACK")
delta_time = self.get_sim_time_cached() - tstart
if delta_time > timeout:
raise AutoTestTimeoutException("Did not get good COMMAND_ACK within %fs" % timeout)
m = self.mav.recv_match(type='COMMAND_ACK',
blocking=True,
timeout=1)
if m is None:
continue
self.progress("ACK received: %s" % str(m))
self.progress("ACK received: %s (%fs)" % (str(m), delta_time))
if m.command == command:
if m.result != want_result:
raise ValueError("Expected %s got %s" % (want_result,
@ -1306,7 +1307,7 @@ class AutoTest(ABC):
while self.get_sim_time_cached() < tstart + timeout:
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
if self.get_sim_time_cached() - last_print > 1:
self.progress("Wait groundspeed %.1f, target:%.1f" %
self.progress("Wait groundspeed %.3f, target:%.3f" %
(m.groundspeed, gs_min))
last_print = self.get_sim_time_cached()
if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
@ -1519,13 +1520,15 @@ class AutoTest(ABC):
raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" %
(wpnum_end, wpnum_end))
def mode_is(self, mode):
self.wait_heartbeat()
return self.get_mode_from_mode_mapping(self.mav.flightmode) == self.get_mode_from_mode_mapping(mode)
def wait_mode(self, mode, timeout=60):
"""Wait for mode to change."""
self.get_mode_from_mode_mapping(mode)
self.progress("Waiting for mode %s" % mode)
tstart = self.get_sim_time()
self.wait_heartbeat()
while self.mav.flightmode != mode:
while not self.mode_is(mode):
custom_num = self.mav.messages['HEARTBEAT'].custom_mode
self.progress("mav.flightmode=%s Want=%s custom=%u" % (
self.mav.flightmode, mode, custom_num))