From 40cd0cd17fe4f4c4ab1a31120242b569100ec575 Mon Sep 17 00:00:00 2001 From: Karthik Desai Date: Fri, 27 Apr 2018 20:21:53 +0200 Subject: [PATCH] Tools: autotest: Use the generic run_test. If any of the tests throws an exception, it will be caught by the generic test function that collects all errors and displays them before passing the result to the vehicle testcode. Hence the name of the test and the exception that got raised get printed in a single location. This nicely reduces code duplication. --- Tools/autotest/apmrover2.py | 160 ++++------- Tools/autotest/arducopter.py | 540 +++++++++-------------------------- Tools/autotest/arduplane.py | 231 ++++++--------- Tools/autotest/ardusub.py | 60 ++-- Tools/autotest/quadplane.py | 24 +- 5 files changed, 293 insertions(+), 722 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index f6cd2b9258..f4835c670b 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -10,6 +10,10 @@ import time from common import AutoTest +from common import MsgRcvTimeoutException +from common import NotAchievedException +from common import PreconditionFailedException + from pysim import util from pymavlink import mavutil @@ -194,7 +198,6 @@ class AutoTestRover(AutoTest): def drive_square(self, side=50): """Drive a square, Driving N then E .""" self.progress("TEST SQUARE") - success = True # use LEARNING Mode self.mavproxy.send('switch 5\n') @@ -202,57 +205,42 @@ class AutoTestRover(AutoTest): # first aim north self.progress("\nTurn right towards north") - if not self.reach_heading_manual(10): - success = False - + self.reach_heading_manual(10) # save bottom left corner of box as waypoint self.progress("Save WP 1 & 2") self.save_wp() # pitch forward to fly north self.progress("\nGoing north %u meters" % side) - if not self.reach_distance_manual(side): - success = False - + self.reach_distance_manual(side) # save top left corner of square as waypoint self.progress("Save WP 3") self.save_wp() # roll right to fly east self.progress("\nGoing east %u meters" % side) - if not self.reach_heading_manual(100): - success = False - if not self.reach_distance_manual(side): - success = False - + self.reach_heading_manual(100) + self.reach_distance_manual(side) # save top right corner of square as waypoint self.progress("Save WP 4") self.save_wp() # pitch back to fly south self.progress("\nGoing south %u meters" % side) - if not self.reach_heading_manual(190): - success = False - if not self.reach_distance_manual(side): - success = False - + self.reach_heading_manual(190) + self.reach_distance_manual(side) # save bottom right corner of square as waypoint self.progress("Save WP 5") self.save_wp() # roll left to fly west self.progress("\nGoing west %u meters" % side) - if not self.reach_heading_manual(280): - success = False - if not self.reach_distance_manual(side): - success = False - + self.reach_heading_manual(280) + self.reach_distance_manual(side) # save bottom left corner of square (should be near home) as waypoint self.progress("Save WP 6") self.save_wp() - return success - def drive_left_circuit(self): """Drive a left circuit, 50m on a side.""" self.mavproxy.send('switch 6\n') @@ -265,15 +253,12 @@ class AutoTestRover(AutoTest): # hard left self.progress("Starting turn %u" % i) self.set_rc(1, 1000) - if not self.wait_heading(270 - (90*i), accuracy=10): - return False + self.wait_heading(270 - (90*i), accuracy=10) self.set_rc(1, 1500) self.progress("Starting leg %u" % i) - if not self.wait_distance(50, accuracy=7): - return False + self.wait_distance(50, accuracy=7) self.set_rc(3, 1500) self.progress("Circuit complete") - return True # def test_throttle_failsafe(self, home, distance_min=10, side=60, # timeout=300): @@ -338,11 +323,12 @@ class AutoTestRover(AutoTest): self.mavproxy.send('switch 4\n') # auto mode self.set_rc(3, 1500) self.wait_mode('AUTO') - if not self.wait_waypoint(1, 4, max_dist=5): - return False + self.wait_waypoint(1, 4, max_dist=5) self.wait_mode('HOLD') self.progress("Mission OK") - return True + + def drive_mission_rover1(self): + self.drive_mission(os.path.join(testdir, "rover1.txt")) def do_get_banner(self): self.mavproxy.send("long DO_SEND_BANNER 1\n") @@ -353,13 +339,12 @@ class AutoTestRover(AutoTest): timeout=1) if m is not None and "ArduRover" in m.text: self.progress("banner received: %s" % m.text) - return True + return if time.time() - start > 10: break self.progress("banner not received") - - return False + raise MsgRcvTimeoutException() def drive_brake_get_stopping_distance(self, speed): # measure our stopping distance: @@ -421,13 +406,11 @@ class AutoTestRover(AutoTest): (distance_with_brakes, distance_without_brakes, delta)) - return False - else: - self.progress( - "Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % - (distance_with_brakes, distance_without_brakes, delta)) + raise NotAchievedException() - return True + self.progress( + "Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % + (distance_with_brakes, distance_without_brakes, delta)) def drive_rtl_mission(self): mission_filepath = os.path.join(testdir, @@ -445,12 +428,12 @@ class AutoTestRover(AutoTest): timeout=0.1) if m is None: self.progress("Did not receive NAV_CONTROLLER_OUTPUT message") - return False + raise MsgRcvTimeoutException() wp_dist_min = 5 if m.wp_dist < wp_dist_min: self.progress("Did not start at least 5 metres from destination") - return False + raise PreconditionFailedException() self.progress("NAV_CONTROLLER_OUTPUT.wp_dist looks good (%u >= %u)" % (m.wp_dist, wp_dist_min,)) @@ -463,11 +446,10 @@ class AutoTestRover(AutoTest): if home_distance > home_distance_max: self.progress("Did not get home (%u metres distant > %u)" % (home_distance, home_distance_max)) - return False + raise NotAchievedException() self.mavproxy.send('switch 6\n') self.wait_mode('MANUAL') self.progress("RTL Mission OK") - return True def test_servorelayevents(self): self.mavproxy.send("relay set 0 0\n") @@ -476,9 +458,8 @@ class AutoTestRover(AutoTest): on = self.get_parameter("SIM_PIN_MASK") if on == off: self.progress("Pin mask unchanged after relay command") - return False + raise NotAchievedException() self.progress("Pin mask changed after relay command") - return True def autotest(self): """Autotest APMrover2 in SITL.""" @@ -486,8 +467,7 @@ class AutoTestRover(AutoTest): self.init() self.progress("Started simulator") - failed = False - e = 'None' + self.fail_list = [] try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) @@ -503,75 +483,33 @@ class AutoTestRover(AutoTest): self.wait_mode('MANUAL') self.progress("Waiting reading for arm") self.wait_ready_to_arm() - if not self.arm_vehicle(): - self.progress("Failed to ARM") - failed = True + self.arm_vehicle() - self.progress("#") - self.progress("########## Drive an RTL mission ##########") - self.progress("#") - # Drive a square in learning mode - # self.reset_and_arm() - if not self.drive_rtl_mission(): - self.progress("Failed RTL mission") - failed = True + self.run_test("Drive an RTL Mission", self.drive_rtl_mission) - self.progress("#") - self.progress("########## Drive a square and save WPs with CH7" - "switch ##########") - self.progress("#") - # Drive a square in learning mode - # self.reset_and_arm() - if not self.drive_square(): - self.progress("Failed drive square") - failed = True + self.run_test("Learn/Drive Square with Ch7 option", + self.drive_square) - if not self.drive_mission(os.path.join(testdir, "rover1.txt")): - self.progress("Failed mission") - failed = True + self.run_test("Drive Mission %s" % "rover1.txt", + self.drive_mission_rover1) - if not self.drive_brake(): - self.progress("Failed brake") - failed = True + self.run_test("Drive Brake", self.drive_brake) - if not self.disarm_vehicle(): - self.progress("Failed to DISARM") - failed = True + self.run_test("Disarm Vehicle", self.disarm_vehicle) - # do not move this to be the first test. MAVProxy's dedupe - # function may bite you. - self.progress("Getting banner") - if not self.do_get_banner(): - self.progress("FAILED: get banner") - failed = True + self.run_test("Get Banner", self.do_get_banner) - self.progress("Getting autopilot capabilities") - if not self.do_get_autopilot_capabilities(): - self.progress("FAILED: get capabilities") - failed = True + self.run_test("Get Capabilities", + self.do_get_autopilot_capabilities) - self.progress("Setting mode via MAV_COMMAND_DO_SET_MODE") - if not self.do_set_mode_via_command_long(): - failed = True + self.run_test("Set mode via MAV_COMMAND_DO_SET_MODE", + self.do_set_mode_via_command_long) - # test ServoRelayEvents: - self.progress("########## Test ServoRelayEvents ##########") - if not self.test_servorelayevents(): - self.progress("Failed servo relay events") - failed = True + self.run_test("Test ServoRelayEvents", + self.test_servorelayevents) - # Throttle Failsafe - self.progress("#") - self.progress("########## Test Failsafe ##########") - self.progress("#") - # self.reset_and_arm() - # if not self.test_throttle_failsafe(HOME, distance_min=4): - # self.progress("Throttle failsafe failed") - # sucess = False - - if not self.log_download(self.buildlogs_path("APMrover2-log.bin")): - self.progress("Failed log download") - failed = True + self.run_test("Download logs", lambda: + self.log_download(self.buildlogs_path("APMrover2-log.bin"))) # if not drive_left_circuit(self): # self.progress("Failed left circuit") # failed = True @@ -581,11 +519,11 @@ class AutoTestRover(AutoTest): except pexpect.TIMEOUT as e: self.progress("Failed with timeout") - failed = True + self.fail_list.append( ("*timeout*", None) ) self.close() - if failed: - self.progress("FAILED: %s" % e) + if len(self.fail_list): + self.progress("FAILED STEPS: %s" % self.fail_list) return False return True diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8d99b4a746..8a388be0e6 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -12,6 +12,7 @@ from pymavlink import mavutil from pysim import util from common import AutoTest +from common import NotAchievedException, AutoTestTimeoutException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) @@ -153,7 +154,6 @@ class AutoTestCopter(AutoTest): self.wait_altitude(alt_min, (alt_min + 5)) self.hover() self.progress("TAKEOFF COMPLETE") - return True def land(self, timeout=60): """Land the quad.""" @@ -161,13 +161,11 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('switch 2\n') # land mode self.wait_mode('LAND') self.progress("Entered Landing Mode") - ret = self.wait_altitude(-5, 1) - self.progress("LANDING: ok= %s" % ret) - return ret + self.wait_altitude(-5, 1) + self.progress("LANDING: ok!") def hover(self, hover_throttle=1500): self.set_rc(3, hover_throttle) - return True # loiter - fly south west, then loiter within 5m position and altitude def loiter(self, holdtime=10, maxaltchange=5, maxdistchange=5): @@ -178,21 +176,17 @@ class AutoTestCopter(AutoTest): # first aim south east self.progress("turn south east") self.set_rc(4, 1580) - if not self.wait_heading(170): - return False + self.wait_heading(170) self.set_rc(4, 1500) # fly south east 50m self.set_rc(2, 1100) - if not self.wait_distance(50): - return False + self.wait_distance(50) self.set_rc(2, 1500) # wait for copter to slow moving - if not self.wait_groundspeed(0, 2): - return False + self.wait_groundspeed(0, 2) - success = True m = self.mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = self.mav.location() @@ -208,16 +202,12 @@ class AutoTestCopter(AutoTest): if alt_delta > maxaltchange: self.progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) - success = False + raise NotAchievedException() if delta > maxdistchange: self.progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) - success = False - if success: - self.progress("Loiter OK for %u seconds" % holdtime) - else: - self.progress("Loiter FAILED") - return success + raise NotAchievedException() + self.progress("Loiter OK for %u seconds" % holdtime) def change_alt(self, alt_min, climb_throttle=1920, descend_throttle=1080): """Change altitude.""" @@ -231,7 +221,6 @@ class AutoTestCopter(AutoTest): self.set_rc(3, descend_throttle) self.wait_altitude((alt_min - 5), alt_min) self.hover() - return True ################################################# # TESTS FLY @@ -241,7 +230,6 @@ class AutoTestCopter(AutoTest): def fly_square(self, side=50, timeout=300): """Fly a square, flying N then E .""" tstart = self.get_sim_time() - success = True # ensure all sticks in the middle self.set_rc(1, 1500) @@ -256,9 +244,7 @@ class AutoTestCopter(AutoTest): # first aim north self.progress("turn right towards north") self.set_rc(4, 1580) - if not self.wait_heading(10): - self.progress("Failed to reach heading") - success = False + self.wait_heading(10) self.set_rc(4, 1500) self.mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True) @@ -275,9 +261,7 @@ class AutoTestCopter(AutoTest): # pitch forward to fly north self.progress("Going north %u meters" % side) self.set_rc(2, 1300) - if not self.wait_distance(side): - self.progress("Failed to reach distance of %u" % side) - success = False + self.wait_distance(side) self.set_rc(2, 1500) # save top left corner of square as waypoint @@ -287,9 +271,7 @@ class AutoTestCopter(AutoTest): # roll right to fly east self.progress("Going east %u meters" % side) self.set_rc(1, 1700) - if not self.wait_distance(side): - self.progress("Failed to reach distance of %u" % side) - success = False + self.wait_distance(side) self.set_rc(1, 1500) # save top right corner of square as waypoint @@ -299,9 +281,7 @@ class AutoTestCopter(AutoTest): # pitch back to fly south self.progress("Going south %u meters" % side) self.set_rc(2, 1700) - if not self.wait_distance(side): - self.progress("Failed to reach distance of %u" % side) - success = False + self.wait_distance(side) self.set_rc(2, 1500) # save bottom right corner of square as waypoint @@ -311,9 +291,7 @@ class AutoTestCopter(AutoTest): # roll left to fly west self.progress("Going west %u meters" % side) self.set_rc(1, 1300) - if not self.wait_distance(side): - self.progress("Failed to reach distance of %u" % side) - success = False + self.wait_distance(side) self.set_rc(1, 1500) # save bottom left corner of square (should be near home) as waypoint @@ -329,13 +307,9 @@ class AutoTestCopter(AutoTest): self.progress("timeleft = %u" % time_left) if time_left < 20: time_left = 20 - if not self.wait_altitude(-10, 10, time_left): - self.progress("Failed to reach alt of 10m") - success = False + self.wait_altitude(-10, 10, time_left) self.save_wp() - return success - def fly_RTL(self, side=60, timeout=250): """Return, land.""" self.progress("# Enter RTL") @@ -347,8 +321,8 @@ class AutoTestCopter(AutoTest): home_distance = self.get_distance(HOME, pos) self.progress("Alt: %u HomeDist: %.0f" % (m.alt, home_distance)) if m.alt <= 1 and home_distance < 10: - return True - return False + return + raise AutoTestTimeoutException() def fly_throttle_failsafe(self, side=60, timeout=180): """Fly east, Failsafe, return, land.""" @@ -360,8 +334,7 @@ class AutoTestCopter(AutoTest): # first aim east self.progress("turn east") self.set_rc(4, 1580) - if not self.wait_heading(135): - return False + self.wait_heading(135) self.set_rc(4, 1500) # raise throttle slightly to avoid hitting the ground @@ -375,8 +348,7 @@ class AutoTestCopter(AutoTest): # fly east 60 meters self.progress("# Going forward %u meters" % side) self.set_rc(2, 1350) - if not self.wait_distance(side, 5, 60): - return False + self.wait_distance(side, 5, 60) self.set_rc(2, 1500) # pull throttle low @@ -402,10 +374,8 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') self.set_rc(3, 1000) - if not self.arm_vehicle(): - self.progress("Failed to re-arm") - return False - return True + self.arm_vehicle() + return self.progress("Failed to land on failsafe RTL - " "timed out after %u seconds" % timeout) # reduce throttle @@ -415,12 +385,9 @@ class AutoTestCopter(AutoTest): self.wait_mode('LAND') self.mavproxy.send('switch 6\n') # stabilize mode self.wait_mode('STABILIZE') - return False + raise AutoTestTimeoutException() def fly_battery_failsafe(self, timeout=30): - # assume failure - success = False - # switch to loiter mode so that we hold position self.mavproxy.send('switch 5\n') self.wait_mode('LOITER') @@ -432,21 +399,13 @@ class AutoTestCopter(AutoTest): # trigger low voltage self.set_parameter('SIM_BATT_VOLTAGE', 10) - # wait for LAND mode - new_mode = self.wait_mode('LAND', 300) - if new_mode == 'LAND': - success = True + # wait for LAND mode. If unsuccessful an exception will be raised + self.wait_mode('LAND', 300) # disable battery failsafe self.set_parameter('BATT_FS_LOW_ACT', 0) - # return status - if success: - self.progress("Successfully entered LAND after battery failsafe") - else: - self.progress("Failed to enter LAND mode after battery failsafe") - - return success + self.progress("Successfully entered LAND after battery failsafe") # fly_stability_patch - fly south, then hold loiter within 5m # position and altitude and reduce 1 motor to 60% efficiency @@ -461,21 +420,17 @@ class AutoTestCopter(AutoTest): # first south self.progress("turn south") self.set_rc(4, 1580) - if not self.wait_heading(180): - return False + self.wait_heading(180) self.set_rc(4, 1500) # fly west 80m self.set_rc(2, 1100) - if not self.wait_distance(80): - return False + self.wait_distance(80) self.set_rc(2, 1500) # wait for copter to slow moving - if not self.wait_groundspeed(0, 2): - return False + self.wait_groundspeed(0, 2) - success = True m = self.mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = self.mav.location() @@ -496,22 +451,16 @@ class AutoTestCopter(AutoTest): if alt_delta > maxaltchange: self.progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) - success = False + raise NotAchievedException() if delta > maxdistchange: self.progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) - success = False + raise NotAchievedException() # restore motor 1 to 100% efficiency self.mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') - if success: - self.progress("Stability patch and Loiter OK for %u seconds" % - holdtime) - else: - self.progress("Stability Patch FAILED") - - return success + self.progress("Stability patch and Loiter OK for %u seconds" % holdtime) # fly_fence_test - fly east until you hit the horizontal circular fence def fly_fence_test(self, timeout=180): @@ -526,15 +475,13 @@ class AutoTestCopter(AutoTest): # first east self.progress("turn east") self.set_rc(4, 1580) - if not self.wait_heading(160): - return False + self.wait_heading(160) self.set_rc(4, 1500) # fly forward (east) at least 20m pitching_forward = True self.set_rc(2, 1100) - if not self.wait_distance(20): - return False + self.wait_distance(20) # start timer tstart = self.get_sim_time() @@ -564,15 +511,11 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1000) # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm uncheck all\n') - if not self.arm_vehicle(): - self.progress("Failed to re-arm") - # remove if we ever clear battery failsafe flag on disarm: - self.mavproxy.send('arm check all\n') - return False + self.arm_vehicle() # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm check all\n') self.progress("Reached home OK") - return True + return # disable fence, enable avoidance self.mavproxy.send('param set FENCE_ENABLE 0\n') @@ -587,7 +530,7 @@ class AutoTestCopter(AutoTest): self.wait_mode('STABILIZE') self.progress("Fence test failed to reach home - " "timed out after %u seconds" % timeout) - return False + raise AutoTestTimeoutException() # fly_alt_fence_test - fly up until you hit the fence def fly_alt_max_fence_test(self, timeout=180): @@ -600,22 +543,17 @@ class AutoTestCopter(AutoTest): self.set_parameter('AVOID_ENABLE', 0) self.set_parameter('FENCE_TYPE', 1) - if not self.change_alt(10): - failed_test_msg = "change_alt climb failed" - self.progress(failed_test_msg) - return False + self.change_alt(10) # first east self.progress("turn east") self.set_rc(4, 1580) - if not self.wait_heading(160): - return False + self.wait_heading(160) self.set_rc(4, 1500) # fly forward (east) at least 20m self.set_rc(2, 1100) - if not self.wait_distance(20): - return False + self.wait_distance(20) # stop flying forward and start flying up: self.set_rc(2, 1500) @@ -633,20 +571,12 @@ class AutoTestCopter(AutoTest): self.wait_mode('STABILIZE') # remove if we ever clear battery failsafe flag on disarm self.mavproxy.send('arm uncheck all\n') - if not self.arm_vehicle(): - self.progress("Failed to re-arm") - # remove if we ever clear battery failsafe flag on disarm: - self.mavproxy.send('arm check all\n') - return False + self.arm_vehicle() # remove if we ever clear battery failsafe flag on disarm: self.mavproxy.send('arm check all\n') - return True - def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20): - """fly_gps_glitch_loiter_test. - - Fly south east in loiter and test reaction to gps glitch.""" + """fly_gps_glitch_loiter_test. Fly south east in loiter and test reaction to gps glitch.""" self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') @@ -678,25 +608,18 @@ class AutoTestCopter(AutoTest): # turn south east self.progress("turn south east") self.set_rc(4, 1580) - if not self.wait_heading(150): + try: + self.wait_heading(150) + self.set_rc(4, 1500) + # fly forward (south east) at least 60m + self.set_rc(2, 1100) + self.wait_distance(60) + self.set_rc(2, 1500) + # wait for copter to slow down + except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) - return False - self.set_rc(4, 1500) - - # fly forward (south east) at least 60m - self.set_rc(2, 1100) - if not self.wait_distance(60): - if self.use_map: - self.show_gps_and_sim_positions(False) - return False - self.set_rc(2, 1500) - - # wait for copter to slow down - if not self.wait_groundspeed(0, 1): - if self.use_map: - self.show_gps_and_sim_positions(False) - return False + raise e # record time and position tstart = self.get_sim_time() @@ -741,7 +664,7 @@ class AutoTestCopter(AutoTest): if moved_distance > max_distance: self.progress("Moved over %u meters, Failed!" % max_distance) - success = False + raise NotAchievedException() # disable gps glitch if glitch_current != -1: @@ -751,17 +674,12 @@ class AutoTestCopter(AutoTest): if self.use_map: self.show_gps_and_sim_positions(False) - if success: - self.progress("GPS glitch test passed!" - " stayed within %u meters for %u seconds" % - (max_distance, timeout)) - else: - self.progress("GPS glitch test FAILED!") - return success + self.progress("GPS glitch test passed!" + " stayed within %u meters for %u seconds" % + (max_distance, timeout)) # fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch def fly_gps_glitch_auto_test(self, timeout=120): - # set-up gps glitch array glitch_lat = [0.0002996, 0.0006958, @@ -790,7 +708,7 @@ class AutoTestCopter(AutoTest): num_wp = self.load_mission("copter_glitch_mission.txt") if not num_wp: self.progress("load copter_glitch_mission failed") - return False + raise NotAchievedException() # turn on simulator display of gps and actual position if self.use_map: @@ -805,14 +723,15 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1500) # wait until 100m from home - if not self.wait_distance(100, 5, 60): + try: + self.wait_distance(100, 5, 60) + except Exception as e: if self.use_map: self.show_gps_and_sim_positions(False) - return False + raise e # record time and position tstart = self.get_sim_time() - tnow = tstart # initialise current glitch glitch_current = 0 @@ -842,7 +761,7 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') # continue with the mission - ret = self.wait_waypoint(0, num_wp-1, timeout=500) + self.wait_waypoint(0, num_wp-1, timeout=500) # wait for arrival back home self.mav.recv_match(type='VFR_HUD', blocking=True) @@ -852,8 +771,8 @@ class AutoTestCopter(AutoTest): if self.get_sim_time() > (tstart + timeout): self.progress("GPS Glitch testing failed" "- exceeded timeout %u seconds" % timeout) - ret = False - break + raise AutoTestTimeoutException() + self.mav.recv_match(type='VFR_HUD', blocking=True) pos = self.mav.location() dist_to_home = self.get_distance(HOME, pos) @@ -863,17 +782,13 @@ class AutoTestCopter(AutoTest): if self.use_map: self.show_gps_and_sim_positions(False) - self.progress("GPS Glitch test Auto completed: passed=%s" % ret) - - return ret + self.progress("GPS Glitch test Auto completed: passed!") # fly_simple - assumes the simple bearing is initialised to be # directly north flies a box with 100m west, 15 seconds north, # 50 seconds east, 15 seconds south def fly_simple(self, side=50, timeout=120): - failed = False - # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') @@ -889,8 +804,7 @@ class AutoTestCopter(AutoTest): # fly south 50m self.progress("# Flying south %u meters" % side) self.set_rc(1, 1300) - if not self.wait_distance(side, 5, 60): - failed = True + self.wait_distance(side, 5, 60) self.set_rc(1, 1500) # fly west 8 seconds @@ -904,8 +818,7 @@ class AutoTestCopter(AutoTest): # fly north 25 meters self.progress("# Flying north %u meters" % (side/2.0)) self.set_rc(1, 1700) - if not self.wait_distance(side/2, 5, 60): - failed = True + self.wait_distance(side/2, 5, 60) self.set_rc(1, 1500) # fly east 8 seconds @@ -921,13 +834,10 @@ class AutoTestCopter(AutoTest): # hover in place self.hover() - return not failed # fly_super_simple - flies a circle around home for 45 seconds def fly_super_simple(self, timeout=45): - failed = False - # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') @@ -935,8 +845,7 @@ class AutoTestCopter(AutoTest): # fly forward 20m self.progress("# Flying forward 20 meters") self.set_rc(2, 1300) - if not self.wait_distance(20, 5, 60): - failed = True + self.wait_distance(20, 5, 60) self.set_rc(2, 1500) # set SUPER SIMPLE mode for all flight modes @@ -967,11 +876,9 @@ class AutoTestCopter(AutoTest): # hover in place self.hover() - return not failed # fly_circle - flies a circle with 20m radius def fly_circle(self, maxaltchange=10, holdtime=36): - # hold position in loiter self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') @@ -979,8 +886,7 @@ class AutoTestCopter(AutoTest): # face west self.progress("turn west") self.set_rc(4, 1580) - if not self.wait_heading(270): - return False + self.wait_heading(270) self.set_rc(4, 1500) # set CIRCLE radius @@ -988,9 +894,7 @@ class AutoTestCopter(AutoTest): # fly forward (east) at least 100m self.set_rc(2, 1100) - if not self.wait_distance(100): - return False - + self.wait_distance(100) # return pitch stick back to middle self.set_rc(2, 1500) @@ -1009,11 +913,9 @@ class AutoTestCopter(AutoTest): self.progress("heading %u" % m.heading) self.progress("CIRCLE OK for %u seconds" % holdtime) - return True # fly_auto_test - fly mission which tests a significant number of commands def fly_auto_test(self): - # Fly mission #1 self.progress("# Load copter_mission") # load the waypoint count @@ -1021,7 +923,7 @@ class AutoTestCopter(AutoTest): num_wp = self.load_mission("copter_mission.txt") if not num_wp: self.progress("load copter_mission failed") - return False + raise NotAchievedException() self.progress("test: Fly a mission from 1 to %u" % num_wp) self.mavproxy.send('wp set 1\n') @@ -1032,10 +934,7 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1500) # fly the mission - ret = self.wait_waypoint(0, num_wp-1, timeout=500) - - # land if mission failed - if ret is False: + if not self.wait_waypoint(0, num_wp-1, timeout=500): self.land() # set throttle to minimum @@ -1045,9 +944,7 @@ class AutoTestCopter(AutoTest): self.mav.motors_disarmed_wait() self.progress("MOTORS DISARMED OK") - self.progress("Auto mission completed: passed=%s" % ret) - - return ret + self.progress("Auto mission completed: passed!") def load_mission(self, mission): path = os.path.join(testdir, mission) @@ -1055,7 +952,6 @@ class AutoTestCopter(AutoTest): # fly_avc_test - fly AVC mission def fly_avc_test(self): - # upload mission from file self.progress("# Load copter_AVC2013_mission") # load the waypoint count @@ -1063,7 +959,7 @@ class AutoTestCopter(AutoTest): num_wp = self.load_mission("copter_AVC2013_mission.txt") if not num_wp: self.progress("load copter_AVC2013_mission failed") - return False + raise NotAchievedException() self.progress("Fly AVC mission from 1 to %u" % num_wp) self.mavproxy.send('wp set 1\n') @@ -1077,7 +973,7 @@ class AutoTestCopter(AutoTest): self.set_rc(3, 1500) # fly the mission - ret = self.wait_waypoint(0, num_wp-1, timeout=500) + self.wait_waypoint(0, num_wp-1, timeout=500) # set throttle to minimum self.set_rc(3, 1000) @@ -1086,9 +982,7 @@ class AutoTestCopter(AutoTest): self.mav.motors_disarmed_wait() self.progress("MOTORS DISARMED OK") - self.progress("AVC mission completed: passed=%s" % ret) - - return ret + self.progress("AVC mission completed: passed!") def fly_mission(self, height_accuracy=-1.0, target_altitude=None): """Fly a mission from a file.""" @@ -1097,20 +991,18 @@ class AutoTestCopter(AutoTest): self.mavproxy.send('wp set 1\n') self.mavproxy.send('switch 4\n') # auto mode self.wait_mode('AUTO') - ret = self.wait_waypoint(0, num_wp-1, timeout=500) - self.progress("test: MISSION COMPLETE: passed=%s" % ret) + self.wait_waypoint(0, num_wp-1, timeout=500) + self.progress("test: MISSION COMPLETE: passed!") # wait here until ready self.mavproxy.send('switch 5\n') # loiter mode self.wait_mode('LOITER') - return ret def autotest(self): """Autotest ArduCopter in SITL.""" if not self.hasInit: self.init() - failed = False - failed_test_msg = "None" + self.fail_list = [] try: self.progress("Waiting for a heartbeat with mavlink protocol %s" @@ -1128,278 +1020,121 @@ class AutoTestCopter(AutoTest): self.wait_ready_to_arm() # Arm - self.progress("# Arm motors") - if not self.arm_vehicle(): - failed_test_msg = "arm_motors failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Arm motors", self.arm_vehicle) - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + # Takeoff + self.run_test("Takeoff to test fly Square", lambda: self.takeoff(10)) # Fly a square in Stabilize mode - self.progress("#") - self.progress("########## Fly a square and save WPs with CH7" - " switch ##########") - self.progress("#") - if not self.fly_square(): - failed_test_msg = "fly_square failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly a square and save WPs with CH7", self.fly_square) # save the stored mission to file - self.progress("# Save out the CH7 mission to file") global num_wp - num_wp = self.save_mission_to_file(os.path.join(testdir, - "ch7_mission.txt")) + num_wp = self.save_mission_to_file(os.path.join(testdir, "ch7_mission.txt")) if not num_wp: - failed_test_msg = "save_mission_to_file failed" - self.progress(failed_test_msg) - failed = True + self.fail_list.append("save_mission_to_file") + self.progress("save_mission_to_file failed") # fly the stored mission - self.progress("# Fly CH7 saved mission") - if not self.fly_mission(height_accuracy=0.5, target_altitude=10): - failed_test_msg = "fly ch7_mission failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly CH7 saved mission", lambda: self.fly_mission(height_accuracy=0.5, target_altitude=10)) # Throttle Failsafe - self.progress("#") - self.progress("########## Test Failsafe ##########") - self.progress("#") - if not self.fly_throttle_failsafe(): - failed_test_msg = "fly_throttle_failsafe failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Test Failsafe", lambda: self.fly_throttle_failsafe) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test battery failsafe", lambda: self.takeoff(10)) # Battery failsafe - if not self.fly_battery_failsafe(): - failed_test_msg = "fly_battery_failsafe failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly Battery Failsafe", lambda: self.fly_battery_failsafe) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test stability patch", lambda: self.takeoff(10)) # Stability patch - self.progress("#") - self.progress("########## Test Stability Patch ##########") - self.progress("#") - if not self.fly_stability_patch(30): - failed_test_msg = "fly_stability_patch failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly stability patch", lambda: self.fly_stability_patch(30)) # RTL - self.progress("# RTL #") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL after stab patch failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after stab patch", lambda: self.fly_RTL) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test horizontal fence", lambda: self.takeoff(10)) # Fence test - self.progress("#") - self.progress("########## Test Horizontal Fence ##########") - self.progress("#") - if not self.fly_fence_test(180): - failed_test_msg = "fly_fence_test failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Test horizontal fence", lambda: self.fly_fence_test(180)) # Fence test - self.progress("#") - self.progress("########## Test Max Alt Fence ##########") - self.progress("#") - if not self.fly_alt_max_fence_test(180): - failed_test_msg = "fly_alt_max_fence_test failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Test Max Alt Fence", lambda: self.fly_alt_max_fence_test(180)) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test GPS glitch loiter", lambda: self.takeoff(10)) # Fly GPS Glitch Loiter test - self.progress("# GPS Glitch Loiter Test") - if not self.fly_gps_glitch_loiter_test(): - failed_test_msg = "fly_gps_glitch_loiter_test failed" - self.progress(failed_test_msg) - failed = True + self.run_test("GPS Glitch Loiter Test", self.fly_gps_glitch_loiter_test) # RTL after GPS Glitch Loiter test - self.progress("# RTL #") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after GPS Glitch Loiter test", self.fly_RTL) # Fly GPS Glitch test in auto mode - self.progress("# GPS Glitch Auto Test") - if not self.fly_gps_glitch_auto_test(): - failed_test_msg = "fly_gps_glitch_auto_test failed" - self.progress(failed_test_msg) - failed = True + self.run_test("GPS Glitch Auto Test", self.fly_gps_glitch_auto_test) - # take-off ahead of next test - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + # Takeoff + self.run_test("Takeoff to test loiter", lambda: self.takeoff(10)) # Loiter for 10 seconds - self.progress("#") - self.progress("########## Test Loiter for 10 seconds ##########") - self.progress("#") - if not self.loiter(): - failed_test_msg = "loiter failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Test Loiter for 10 seconds", self.loiter) # Loiter Climb - self.progress("#") - self.progress("# Loiter - climb to 30m") - self.progress("#") - if not self.change_alt(30): - failed_test_msg = "change_alt climb failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Loiter - climb to 30m", lambda: self.change_alt(30)) # Loiter Descend - self.progress("#") - self.progress("# Loiter - descend to 20m") - self.progress("#") - if not self.change_alt(20): - failed_test_msg = "change_alt descend failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Loiter - descend to 20m", lambda: self.change_alt(20)) # RTL - self.progress("#") - self.progress("########## Test RTL ##########") - self.progress("#") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL after Loiter climb/descend failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after Loiter climb/descend", self.fly_RTL) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test fly SIMPLE mode", lambda: self.takeoff(10)) # Simple mode - self.progress("# Fly in SIMPLE mode") - if not self.fly_simple(): - failed_test_msg = "fly_simple failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly in SIMPLE mode", self.fly_simple) # RTL - self.progress("#") - self.progress("########## Test RTL ##########") - self.progress("#") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL after simple mode failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after SIMPLE mode", self.fly_RTL) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test circle in SUPER SIMPLE mode", lambda: self.takeoff(10)) # Fly a circle in super simple mode - self.progress("# Fly a circle in SUPER SIMPLE mode") - if not self.fly_super_simple(): - failed_test_msg = "fly_super_simple failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly a circle in SUPER SIMPLE mode", self.fly_super_simple) # RTL - self.progress("# RTL #") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL after super simple mode failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after SUPER SIMPLE mode", self.fly_RTL) # Takeoff - self.progress("# Takeoff") - if not self.takeoff(10): - failed_test_msg = "takeoff failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Takeoff to test CIRCLE mode", lambda: self.takeoff(10)) # Circle mode - self.progress("# Fly CIRCLE mode") - if not self.fly_circle(): - failed_test_msg = "fly_circle failed" - self.progress(failed_test_msg) - failed = True + self.run_test("Fly CIRCLE mode", self.fly_circle) # RTL - self.progress("#") - self.progress("########## Test RTL ##########") - self.progress("#") - if not self.fly_RTL(): - failed_test_msg = "fly_RTL after circle failed" - self.progress(failed_test_msg) - failed = True + self.run_test("RTL after CIRCLE mode", self.fly_RTL) - self.progress("# Fly copter mission") - if not self.fly_auto_test(): - failed_test_msg = "fly_auto_test failed" - self.progress(failed_test_msg) - failed = True - else: - self.progress("Flew copter mission OK") + # Fly auto test + self.run_test("Fly copter mission", self.fly_auto_test) # wait for disarm self.mav.motors_disarmed_wait() - log_filepath = self.buildlogs_path("ArduCopter-log.bin") - if not self.log_download(log_filepath): - failed_test_msg = "log_download failed" - self.progress(failed_test_msg) - failed = True + # Download logs + self.run_test("log download", lambda: self.log_download(self.buildlogs_path("ArduCopter-log.bin"))) except pexpect.TIMEOUT as e: self.progress("Failed with timeout") - failed = True - + self.fail_list.append("Failed with timeout") self.close() - if failed: - self.progress("FAILED: %s" % failed_test_msg) + if len(self.fail_list): + self.progress("FAILED : %s" % self.fail_list) return False return True @@ -1409,8 +1144,7 @@ class AutoTestCopter(AutoTest): if not self.hasInit: self.init() - failed = False - failed_test_msg = "None" + self.fail_list = [] try: self.mav.wait_heartbeat() @@ -1425,40 +1159,24 @@ class AutoTestCopter(AutoTest): self.wait_ready_to_arm() # Arm - self.progress("# Arm motors") - if not self.arm_vehicle(): - failed_test_msg = "arm_motors failed" - self.progress(failed_test_msg) - failed = True - + self.run_test("Arm motors", self.arm_vehicle) self.progress("Raising rotor speed") self.set_rc(8, 2000) - self.progress("# Fly AVC mission") - if not self.fly_avc_test(): - failed_test_msg = "fly_avc_test failed" - self.progress(failed_test_msg) - failed = True - else: - self.progress("Flew AVC mission OK") + self.run_test("Fly AVC mission", self.fly_avc_test) self.progress("Lowering rotor speed") self.set_rc(8, 1000) # mission ends with disarm so should be ok to download logs now - log_path = self.buildlogs_path("Helicopter-log.bin") - if not self.log_download(log_path): - failed_test_msg = "log_download failed" - self.progress(failed_test_msg) - failed = True + self.run_test("log download", lambda: self.log_download(self.buildlogs_path("Helicopter-log.bin"))) - except pexpect.TIMEOUT as failed_test_msg: - failed_test_msg = "Timeout" - failed = True + except pexpect.TIMEOUT as e: + self.fail_list.append("Failed with timeout") self.close() - if failed: - self.progress("FAILED: %s" % failed_test_msg) + if len(self.fail_list): + self.progress("FAILED: %s" % self.fail_list) return False return True diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 9926db1f6d..14a3a9912e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -11,6 +11,7 @@ from pymavlink import mavutil from pysim import util from common import AutoTest +from common import NotAchievedException # get location of scripts testdir = os.path.dirname(os.path.realpath(__file__)) @@ -104,7 +105,6 @@ class AutoTestPlane(AutoTest): def takeoff(self): """Takeoff get to 30m altitude.""" - self.wait_ready_to_arm() self.arm_vehicle() @@ -131,24 +131,21 @@ class AutoTestPlane(AutoTest): self.set_rc(3, 2000) # gain a bit of altitude - if not self.wait_altitude(self.homeloc.alt+150, + self.wait_altitude(self.homeloc.alt+150, self.homeloc.alt+180, - timeout=30): - return False + timeout=30) # level off self.set_rc(2, 1500) self.progress("TAKEOFF COMPLETE") - return True def fly_left_circuit(self): """Fly a left circuit, 200m on a side.""" self.mavproxy.send('switch 4\n') self.wait_mode('FBWA') self.set_rc(3, 2000) - if not self.wait_level_flight(): - return False + self.wait_level_flight() self.progress("Flying left circuit") # do 4 turns @@ -156,28 +153,23 @@ class AutoTestPlane(AutoTest): # hard left self.progress("Starting turn %u" % i) self.set_rc(1, 1000) - if not self.wait_heading(270 - (90*i), accuracy=10): - return False + self.wait_heading(270 - (90*i), accuracy=10) self.set_rc(1, 1500) self.progress("Starting leg %u" % i) - if not self.wait_distance(100, accuracy=20): - return False + self.wait_distance(100, accuracy=20) self.progress("Circuit complete") - return True def fly_RTL(self): """Fly to home.""" self.progress("Flying home in RTL") self.mavproxy.send('switch 2\n') self.wait_mode('RTL') - if not self.wait_location(self.homeloc, + self.wait_location(self.homeloc, accuracy=120, target_altitude=self.homeloc.alt+100, height_accuracy=20, - timeout=180): - return False + timeout=180) self.progress("RTL Complete") - return True def fly_LOITER(self, num_circles=4): """Loiter where we are.""" @@ -190,10 +182,8 @@ class AutoTestPlane(AutoTest): self.progress("Initial altitude %u\n" % initial_alt) while num_circles > 0: - if not self.wait_heading(0, accuracy=10, timeout=60): - return False - if not self.wait_heading(180, accuracy=10, timeout=60): - return False + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) num_circles -= 1 self.progress("Loiter %u circles left" % num_circles) @@ -207,10 +197,9 @@ class AutoTestPlane(AutoTest): if abs(final_alt - initial_alt) > 20: self.progress("Failed to maintain altitude") - return False + raise NotAchievedException() self.progress("Completed Loiter OK") - return True def fly_CIRCLE(self, num_circles=1): """Circle where we are.""" @@ -223,10 +212,8 @@ class AutoTestPlane(AutoTest): self.progress("Initial altitude %u\n" % initial_alt) while num_circles > 0: - if not self.wait_heading(0, accuracy=10, timeout=60): - return False - if not self.wait_heading(180, accuracy=10, timeout=60): - return False + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) num_circles -= 1 self.progress("CIRCLE %u circles left" % num_circles) @@ -240,10 +227,9 @@ class AutoTestPlane(AutoTest): if abs(final_alt - initial_alt) > 20: self.progress("Failed to maintain altitude") - return False + raise NotAchievedException() self.progress("Completed CIRCLE OK") - return True def wait_level_flight(self, accuracy=5, timeout=30): """Wait for level flight.""" @@ -259,9 +245,9 @@ class AutoTestPlane(AutoTest): self.progress("Roll=%.1f Pitch=%.1f" % (roll, pitch)) if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: self.progress("Attained level flight") - return True + return self.progress("Failed to attain level flight") - return False + raise NotAchievedException() def change_altitude(self, altitude, accuracy=30): """Get to a given altitude.""" @@ -272,8 +258,7 @@ class AutoTestPlane(AutoTest): self.set_rc(2, 2000) else: self.set_rc(2, 1000) - if not self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2): - return False + self.wait_altitude(altitude-accuracy/2, altitude+accuracy/2) self.set_rc(2, 1500) self.progress("Reached target altitude at %u" % self.mav.messages['VFR_HUD'].alt) @@ -283,8 +268,7 @@ class AutoTestPlane(AutoTest): """Fly a left axial roll.""" # full throttle! self.set_rc(3, 2000) - if not self.change_altitude(self.homeloc.alt+300): - return False + self.change_altitude(self.homeloc.alt+300) # fly the roll in manual self.mavproxy.send('switch 6\n') @@ -293,15 +277,13 @@ class AutoTestPlane(AutoTest): while count > 0: self.progress("Starting roll") self.set_rc(1, 1000) - if not self.wait_roll(-150, accuracy=90): - self.set_rc(1, 1500) - return False - if not self.wait_roll(150, accuracy=90): - self.set_rc(1, 1500) - return False - if not self.wait_roll(0, accuracy=90): - self.set_rc(1, 1500) - return False + try: + self.wait_roll(-150, accuracy=90) + self.wait_roll(150, accuracy=90) + self.wait_roll(0, accuracy=90) + except Exception as e: + self.set_rc(1,1500) + raise e count -= 1 # back to FBWA @@ -315,9 +297,7 @@ class AutoTestPlane(AutoTest): """Fly a inside loop.""" # full throttle! self.set_rc(3, 2000) - if not self.change_altitude(self.homeloc.alt+300): - return False - + self.change_altitude(self.homeloc.alt+300) # fly the loop in manual self.mavproxy.send('switch 6\n') self.wait_mode('MANUAL') @@ -325,10 +305,8 @@ class AutoTestPlane(AutoTest): while count > 0: self.progress("Starting loop") self.set_rc(2, 1000) - if not self.wait_pitch(-60, accuracy=20): - return False - if not self.wait_pitch(0, accuracy=20): - return False + self.wait_pitch(-60, accuracy=20) + self.wait_pitch(0, accuracy=20) count -= 1 # back to FBWA @@ -343,8 +321,7 @@ class AutoTestPlane(AutoTest): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - if not self.change_altitude(self.homeloc.alt+300): - return False + self.change_altitude(self.homeloc.alt+300) self.set_rc(2, 1500) self.mavproxy.send("mode STABILIZE\n") @@ -354,17 +331,13 @@ class AutoTestPlane(AutoTest): while count > 0: self.progress("Starting roll") self.set_rc(1, 2000) - if not self.wait_roll(-150, accuracy=90): - return False - if not self.wait_roll(150, accuracy=90): - return False - if not self.wait_roll(0, accuracy=90): - return False + self.wait_roll(-150, accuracy=90) + self.wait_roll(150, accuracy=90) + self.wait_roll(0, accuracy=90) count -= 1 self.set_rc(1, 1500) - if not self.wait_roll(0, accuracy=5): - return False + self.wait_roll(0, accuracy=5) # back to FBWA self.mavproxy.send('mode FBWA\n') @@ -377,8 +350,7 @@ class AutoTestPlane(AutoTest): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - if not self.change_altitude(self.homeloc.alt+300): - return False + self.change_altitude(self.homeloc.alt+300) self.set_rc(2, 1500) self.mavproxy.send("mode ACRO\n") @@ -388,12 +360,9 @@ class AutoTestPlane(AutoTest): while count > 0: self.progress("Starting roll") self.set_rc(1, 1000) - if not self.wait_roll(-150, accuracy=90): - return False - if not self.wait_roll(150, accuracy=90): - return False - if not self.wait_roll(0, accuracy=90): - return False + self.wait_roll(-150, accuracy=90) + self.wait_roll(150, accuracy=90) + self.wait_roll(0, accuracy=90) count -= 1 self.set_rc(1, 1500) @@ -410,10 +379,8 @@ class AutoTestPlane(AutoTest): while count > 0: self.progress("Starting loop") self.set_rc(2, 1000) - if not self.wait_pitch(-60, accuracy=20): - return False - if not self.wait_pitch(0, accuracy=20): - return False + self.wait_pitch(-60, accuracy=20) + self.wait_pitch(0, accuracy=20) count -= 1 self.set_rc(2, 1500) @@ -447,13 +414,14 @@ class AutoTestPlane(AutoTest): # hard left self.progress("Starting turn %u" % i) self.set_rc(1, 1800) - if not self.wait_heading(0 + (90*i), accuracy=20, timeout=60): + try: + self.wait_heading(0 + (90*i), accuracy=20, timeout=60) + except Exception as e: self.set_rc(1, 1500) - return False + raise e self.set_rc(1, 1500) self.progress("Starting leg %u" % i) - if not self.wait_distance(100, accuracy=20): - return False + self.wait_distance(100, accuracy=20) self.progress("Circuit complete") self.progress("Flying rudder left circuit") @@ -462,13 +430,14 @@ class AutoTestPlane(AutoTest): # hard left self.progress("Starting turn %u" % i) self.set_rc(4, 1900) - if not self.wait_heading(360 - (90*i), accuracy=20, timeout=60): + try: + self.wait_heading(360 - (90*i), accuracy=20, timeout=60) + except Exception as e: self.set_rc(4, 1500) - return False + raise e self.set_rc(4, 1500) self.progress("Starting leg %u" % i) - if not self.wait_distance(100, accuracy=20): - return False + self.wait_distance(100, accuracy=20) self.progress("Circuit complete") m = self.mav.recv_match(type='VFR_HUD', blocking=True) @@ -482,7 +451,7 @@ class AutoTestPlane(AutoTest): if abs(final_alt - initial_alt) > 20: self.progress("Failed to maintain altitude") - return False + raise NotAchievedException() return self.wait_level_flight() @@ -495,22 +464,17 @@ class AutoTestPlane(AutoTest): self.mavproxy.expect('Requesting [0-9]+ waypoints') self.mavproxy.send('switch 1\n') # auto mode self.wait_mode('AUTO') - if not self.wait_waypoint(1, 7, max_dist=60): - return False - if not self.wait_groundspeed(0, 0.5, timeout=60): - return False + self.wait_waypoint(1, 7, max_dist=60) + self.wait_groundspeed(0, 0.5, timeout=60) self.mavproxy.expect("Auto disarmed") self.progress("Mission OK") - return True def autotest(self): """Autotest ArduPlane in SITL.""" if not self.hasInit: self.init() - failed = False - fail_list = [] - e = 'None' + self.fail_list = [] try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) @@ -526,69 +490,42 @@ class AutoTestPlane(AutoTest): self.mav.wait_gps_fix() self.homeloc = self.mav.location() self.progress("Home location: %s" % self.homeloc) - if not self.takeoff(): - self.progress("Failed takeoff") - failed = True - fail_list.append("takeoff") - if not self.fly_left_circuit(): - self.progress("Failed left circuit") - failed = True - fail_list.append("left_circuit") - if not self.axial_left_roll(1): - self.progress("Failed left roll") - failed = True - fail_list.append("left_roll") - if not self.inside_loop(): - self.progress("Failed inside loop") - failed = True - fail_list.append("inside_loop") - if not self.test_stabilize(): - self.progress("Failed stabilize test") - failed = True - fail_list.append("stabilize") - if not self.test_acro(): - self.progress("Failed ACRO test") - failed = True - fail_list.append("acro") - if not self.test_FBWB(): - self.progress("Failed FBWB test") - failed = True - fail_list.append("fbwb") - if not self.test_FBWB(mode='CRUISE'): - self.progress("Failed CRUISE test") - failed = True - fail_list.append("cruise") - if not self.fly_RTL(): - self.progress("Failed RTL") - failed = True - fail_list.append("RTL") - if not self.fly_LOITER(): - self.progress("Failed LOITER") - failed = True - fail_list.append("LOITER") - if not self.fly_CIRCLE(): - self.progress("Failed CIRCLE") - failed = True - fail_list.append("LOITER") - if not self.fly_mission(os.path.join(testdir, "ap1.txt"), + + self.run_test("Takeoff", self.takeoff) + + self.run_test("Fly left circuit", self.fly_left_circuit) + + self.run_test("Left roll", lambda: self.axial_left_roll(1)) + + self.run_test("Inside loop", self.inside_loop) + + self.run_test("Stablize test", self.test_stabilize) + + self.run_test("ACRO test", self.test_acro) + + self.run_test("FBWB test", self.test_FBWB) + + self.run_test("CRUISE test", lambda: self.test_FBWB(mode='CRUISE')) + + self.run_test("RTL test", self.fly_RTL) + + self.run_test("LOITER test", self.fly_LOITER) + + self.run_test("CIRCLE test", self.fly_CIRCLE) + + self.run_test("Mission test", lambda: self.fly_mission(os.path.join(testdir, "ap1.txt"), height_accuracy=10, - target_altitude=self.homeloc.alt+100): - self.progress("Failed mission") - failed = True - fail_list.append("mission") - if not self.log_download(self.buildlogs_path("ArduPlane-log.bin")): - self.progress("Failed log download") - failed = True - fail_list.append("log_download") + target_altitude=self.homeloc.alt+100)) + + self.run_test("Log download", lambda: self.log_download(self.buildlogs_path("ArduPlane-log.bin"))) + except pexpect.TIMEOUT as e: self.progress("Failed with timeout") - failed = True - fail_list.append("timeout") + self.fail_list.append("timeout") self.close() - if failed: - self.progress("FAILED: %s" % e) - self.progress("Fail list: %s" % fail_list) + if len(self.fail_list): + self.progress("FAILED: %s" % self.fail_list) return False return True diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index dc163cdb53..0299e59ef3 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -103,39 +103,27 @@ class AutoTestSub(AutoTest): self.set_rc(5, 1600) self.set_rc(6, 1550) - if not self.wait_distance(50, accuracy=7, timeout=200): - return False - + self.wait_distance(50, accuracy=7, timeout=200) self.set_rc(4, 1550) - if not self.wait_heading(0): - return False - + self.wait_heading(0) self.set_rc(4, 1500) - if not self.wait_distance(50, accuracy=7, timeout=100): - return False - + self.wait_distance(50, accuracy=7, timeout=100) self.set_rc(4, 1550) - if not self.wait_heading(0): - return False - + self.wait_heading(0) self.set_rc(4, 1500) self.set_rc(5, 1500) self.set_rc(6, 1100) - if not self.wait_distance(75, accuracy=7, timeout=100): - return False - + self.wait_distance(75, accuracy=7, timeout=100) self.set_rc_default() self.disarm_vehicle() self.progress("Manual dive OK") - return True def dive_mission(self, filename): - self.progress("Executing mission %s" % filename) self.mavproxy.send('wp load %s\n' % filename) self.mavproxy.expect('Flight plan received') @@ -143,28 +131,23 @@ class AutoTestSub(AutoTest): self.mavproxy.expect('Saved [0-9]+ waypoints') self.set_rc_default() - if not self.arm_vehicle(): - self.progress("Failed to ARM") - return False + self.arm_vehicle() self.mavproxy.send('mode auto\n') self.wait_mode('AUTO') - if not self.wait_waypoint(1, 5, max_dist=5): - return False + self.wait_waypoint(1, 5, max_dist=5) self.disarm_vehicle() self.progress("Mission OK") - return True def autotest(self): """Autotest ArduSub in SITL.""" if not self.hasInit: self.init() - failed = False - e = 'None' + self.fail_list = [] try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) @@ -179,25 +162,22 @@ class AutoTestSub(AutoTest): self.homeloc = self.mav.location() self.progress("Home location: %s" % self.homeloc) self.set_rc_default() - if not self.arm_vehicle(): - self.progress("Failed to ARM") - failed = True - if not self.dive_manual(): - self.progress("Failed manual dive") - failed = True - if not self.dive_mission(os.path.join(testdir, "sub_mission.txt")): - self.progress("Failed auto mission") - failed = True - if not self.log_download(self.buildlogs_path("ArduSub-log.bin")): - self.progress("Failed log download") - failed = True + + self.run_test("Arm vehicle", self.arm_vehicle) + + self.run_test("Dive manual", self.dive_manual) + + self.run_test("Dive mission", lambda: self.dive_mission(os.path.join(testdir, "sub_mission.txt"))) + + self.run_test("Log download", lambda: self.log_download(self.buildlogs_path("ArduSub-log.bin"))) + except pexpect.TIMEOUT as e: self.progress("Failed with timeout") - failed = True + self.fail_list.append("Failed with timeout") self.close() - if failed: - self.progress("FAILED: %s" % e) + if len(self.fail_list): + self.progress("FAILED: %s" % self.fail_list) return False return True diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 273a74b94c..8a97c18d9d 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -134,25 +134,24 @@ class AutoTestQuadPlane(AutoTest): self.mavproxy.expect('Requesting [0-9]+ waypoints') self.mavproxy.send('mode AUTO\n') self.wait_mode('AUTO') - if not self.wait_waypoint(1, 19, max_dist=60, timeout=1200): - return False + self.wait_waypoint(1, 19, max_dist=60, timeout=1200) + self.mavproxy.expect('DISARMED') # wait for blood sample here self.mavproxy.send('wp set 20\n') self.arm_vehicle() - if not self.wait_waypoint(20, 34, max_dist=60, timeout=1200): - return False + self.wait_waypoint(20, 34, max_dist=60, timeout=1200) + self.mavproxy.expect('DISARMED') self.progress("Mission OK") - return True def autotest(self): """Autotest QuadPlane in SITL.""" if not self.hasInit: self.init() - failed = False - e = 'None' + self.fail_list = [] + try: self.progress("Waiting for a heartbeat with mavlink protocol %s" % self.mav.WIRE_PROTOCOL_VERSION) @@ -174,16 +173,15 @@ class AutoTestQuadPlane(AutoTest): m = os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt") f = os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt") - if not self.fly_mission(m, f): - self.progress("Failed mission") - failed = True + + self.run_test("Mission", lambda: self.fly_mission(m, f)) except pexpect.TIMEOUT as e: self.progress("Failed with timeout") - failed = True + self.fail_list.append("Failed with timeout") self.close() - if failed: - self.progress("FAILED: %s" % e) + if len(self.fail_list): + self.progress("FAILED: %s" % self.fail_list) return False return True