From f2e3d377adc81098099397b087d59c4e54a2eeff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 7 Mar 2019 12:35:53 +1100 Subject: [PATCH] Tools: autotest: drain mav during RTL test to improve reliability --- Tools/autotest/apmrover2.py | 20 ++++++++++++++------ Tools/autotest/common.py | 17 ++++++++++------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 6c0766d69e..d18a0503fb 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -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") diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 9fbac3477d..41a1c0d801 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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))