diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 10550f49d3..d8cac07ce9 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -27,7 +27,7 @@ def arm_rover(mavproxy, mav): mavproxy.send('arm throttle\n') mavproxy.expect('ARMED') - print("ROVER ARMED") + progress("ROVER ARMED") return True @@ -35,7 +35,7 @@ def disarm_rover(mavproxy, mav): mavproxy.send('disarm\n') mavproxy.expect('DISARMED') - print("ROVER DISARMED") + progress("ROVER DISARMED") return True @@ -45,30 +45,30 @@ def drive_left_circuit(mavproxy, mav): wait_mode(mav, 'MANUAL') mavproxy.send('rc 3 2000\n') - print("Driving left circuit") + progress("Driving left circuit") # do 4 turns for i in range(0, 4): # hard left - print("Starting turn %u" % i) + progress("Starting turn %u" % i) mavproxy.send('rc 1 1000\n') if not wait_heading(mav, 270 - (90*i), accuracy=10): return False mavproxy.send('rc 1 1500\n') - print("Starting leg %u" % i) + progress("Starting leg %u" % i) if not wait_distance(mav, 50, accuracy=7): return False mavproxy.send('rc 3 1500\n') - print("Circuit complete") + progress("Circuit complete") return True def drive_RTL(mavproxy, mav): """Drive to home.""" - print("Driving home in RTL") + progress("Driving home in RTL") mavproxy.send('switch 3\n') if not wait_location(mav, homeloc, accuracy=22, timeout=90): return False - print("RTL Complete") + progress("RTL Complete") return True @@ -82,7 +82,7 @@ def setup_rc(mavproxy): def drive_mission(mavproxy, mav, filename): """Drive a mission from a file.""" global homeloc - print("Driving mission %s" % filename) + progress("Driving mission %s" % filename) mavproxy.send('wp load %s\n' % filename) mavproxy.expect('Flight plan received') mavproxy.send('wp list\n') @@ -93,7 +93,7 @@ def drive_mission(mavproxy, mav, filename): if not wait_waypoint(mav, 1, 4, max_dist=5): return False wait_mode(mav, 'HOLD') - print("Mission OK") + progress("Mission OK") return True def do_get_banner(mavproxy, mav): @@ -102,12 +102,12 @@ def do_get_banner(mavproxy, mav): while True: m = mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) if m is not None and "APM:Rover" in m.text: - print("banner received: %s" % (m.text)) + progress("banner received: %s" % (m.text)) return True if time.time() - start > 10: break - print("banner not received") + progress("banner not received") return False @@ -115,9 +115,9 @@ def do_get_autopilot_capabilities(mavproxy, mav): mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n") m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10) if m is None: - print("AUTOPILOT_VERSION not received") + progress("AUTOPILOT_VERSION not received") return False - print("AUTOPILOT_VERSION received") + progress("AUTOPILOT_VERSION received") return True; def do_set_mode_via_command_long(mavproxy, mav): @@ -189,10 +189,10 @@ def drive_brake(mavproxy, mav): delta = distance_without_brakes - distance_with_brakes if delta < distance_without_brakes*0.05: # 5% isn't asking for much - print("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) + progress("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) return False else: - print("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) + progress("Brakes work (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta)) return True @@ -219,7 +219,7 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup) mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options) - print("WAITING FOR PARAMETERS") + progress("WAITING FOR PARAMETERS") mavproxy.expect('Received [0-9]+ parameters') # setup test parameters @@ -241,10 +241,10 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/APMrover2-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) try: @@ -259,13 +259,13 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa expect_list_clear() expect_list_extend([sitl, mavproxy]) - print("Started simulator") + progress("Started simulator") # get a mavlink connection going try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -273,54 +273,54 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa failed = False e = 'None' try: - print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) + progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) mav.wait_heartbeat() - print("Setting up RC parameters") + progress("Setting up RC parameters") setup_rc(mavproxy) - print("Waiting for GPS fix") + progress("Waiting for GPS fix") mav.wait_gps_fix() homeloc = mav.location() - print("Home location: %s" % homeloc) + progress("Home location: %s" % homeloc) if not arm_rover(mavproxy, mav): - print("Failed to ARM") + progress("Failed to ARM") failed = True if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")): - print("Failed mission") + progress("Failed mission") failed = True if not drive_brake(mavproxy, mav): - print("Failed brake") + progress("Failed brake") failed = True if not disarm_rover(mavproxy, mav): - print("Failed to DISARM") + progress("Failed to DISARM") failed = True if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/APMrover2-log.bin")): - print("Failed log download") + progress("Failed log download") failed = True # if not drive_left_circuit(mavproxy, mav): -# print("Failed left circuit") +# progress("Failed left circuit") # failed = True # if not drive_RTL(mavproxy, mav): -# print("Failed RTL") +# progress("Failed RTL") # failed = True # do not move this to be the first test. MAVProxy's dedupe # function may bite you. - print("Getting banner") + progress("Getting banner") if not do_get_banner(mavproxy, mav): - print("FAILED: get banner") + progress("FAILED: get banner") failed = True - print("Getting autopilot capabilities") + progress("Getting autopilot capabilities") if not do_get_autopilot_capabilities(mavproxy, mav): - print("FAILED: get capabilities") + progress("FAILED: get capabilities") failed = True - print("Setting mode via MAV_COMMAND_DO_SET_MODE") + progress("Setting mode via MAV_COMMAND_DO_SET_MODE") if not do_set_mode_via_command_long(mavproxy, mav): failed = True except pexpect.TIMEOUT as e: - print("Failed with timeout") + progress("Failed with timeout") failed = True mav.close() @@ -333,6 +333,6 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) if failed: - print("FAILED: %s" % e) + progress("FAILED: %s" % e) return False return True diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1cd2950763..8e026b3911 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -33,7 +33,7 @@ def hover(mavproxy, mav, hover_throttle=1500): def arm_motors(mavproxy, mav): """Arm motors.""" - print("Arming motors") + progress("Arming motors") mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1000\n') @@ -41,13 +41,13 @@ def arm_motors(mavproxy, mav): mavproxy.expect('APM: Arming motors') mavproxy.send('rc 4 1500\n') mav.motors_armed_wait() - print("MOTORS ARMED OK") + progress("MOTORS ARMED OK") return True def disarm_motors(mavproxy, mav): """Disarm motors.""" - print("Disarming motors") + progress("Disarming motors") mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') mavproxy.send('rc 3 1000\n') @@ -55,7 +55,7 @@ def disarm_motors(mavproxy, mav): mavproxy.expect('APM: Disarming motors') mavproxy.send('rc 4 1500\n') mav.motors_disarmed_wait() - print("MOTORS DISARMED OK") + progress("MOTORS DISARMED OK") return True @@ -68,7 +68,7 @@ def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700): if (m.alt < alt_min): wait_altitude(mav, alt_min, (alt_min + 5)) hover(mavproxy, mav) - print("TAKEOFF COMPLETE") + progress("TAKEOFF COMPLETE") return True @@ -79,7 +79,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): wait_mode(mav, 'LOITER') # first aim south east - print("turn south east") + progress("turn south east") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 170): return False @@ -101,23 +101,23 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): start = mav.location() tstart = get_sim_time(mav) tholdstart = get_sim_time(mav) - print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) + progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) while get_sim_time(mav) < tstart + holdtime: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = mav.location() delta = get_distance(start, pos) alt_delta = math.fabs(m.alt - start_altitude) - print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) + progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) if alt_delta > maxaltchange: - print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) success = False if delta > maxdistchange: - print("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) + progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) success = False if success: - print("Loiter OK for %u seconds" % holdtime) + progress("Loiter OK for %u seconds" % holdtime) else: - print("Loiter FAILED") + progress("Loiter FAILED") return success @@ -125,11 +125,11 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108 """Change altitude.""" m = mav.recv_match(type='VFR_HUD', blocking=True) if(m.alt < alt_min): - print("Rise to alt:%u from %u" % (alt_min, m.alt)) + progress("Rise to alt:%u from %u" % (alt_min, m.alt)) mavproxy.send('rc 3 %u\n' % climb_throttle) wait_altitude(mav, alt_min, (alt_min + 5)) else: - print("Lower to alt:%u from %u" % (alt_min, m.alt)) + progress("Lower to alt:%u from %u" % (alt_min, m.alt)) mavproxy.send('rc 3 %u\n' % descend_throttle) wait_altitude(mav, (alt_min - 5), alt_min) hover(mavproxy, mav) @@ -153,16 +153,16 @@ def fly_square(mavproxy, mav, side=50, timeout=300): wait_mode(mav, 'LOITER') # first aim north - print("turn right towards north") + progress("turn right towards north") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 10): - print("Failed to reach heading") + progress("Failed to reach heading") success = False mavproxy.send('rc 4 1500\n') mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True) # save bottom left corner of box as waypoint - print("Save WP 1 & 2") + progress("Save WP 1 & 2") save_wp(mavproxy, mav) # switch back to stabilize mode @@ -171,64 +171,64 @@ def fly_square(mavproxy, mav, side=50, timeout=300): wait_mode(mav, 'STABILIZE') # pitch forward to fly north - print("Going north %u meters" % side) + progress("Going north %u meters" % side) mavproxy.send('rc 2 1300\n') if not wait_distance(mav, side): - print("Failed to reach distance of %u" % side) + progress("Failed to reach distance of %u" % side) success = False mavproxy.send('rc 2 1500\n') # save top left corner of square as waypoint - print("Save WP 3") + progress("Save WP 3") save_wp(mavproxy, mav) # roll right to fly east - print("Going east %u meters" % side) + progress("Going east %u meters" % side) mavproxy.send('rc 1 1700\n') if not wait_distance(mav, side): - print("Failed to reach distance of %u" % side) + progress("Failed to reach distance of %u" % side) success = False mavproxy.send('rc 1 1500\n') # save top right corner of square as waypoint - print("Save WP 4") + progress("Save WP 4") save_wp(mavproxy, mav) # pitch back to fly south - print("Going south %u meters" % side) + progress("Going south %u meters" % side) mavproxy.send('rc 2 1700\n') if not wait_distance(mav, side): - print("Failed to reach distance of %u" % side) + progress("Failed to reach distance of %u" % side) success = False mavproxy.send('rc 2 1500\n') # save bottom right corner of square as waypoint - print("Save WP 5") + progress("Save WP 5") save_wp(mavproxy, mav) # roll left to fly west - print("Going west %u meters" % side) + progress("Going west %u meters" % side) mavproxy.send('rc 1 1300\n') if not wait_distance(mav, side): - print("Failed to reach distance of %u" % side) + progress("Failed to reach distance of %u" % side) success = False mavproxy.send('rc 1 1500\n') # save bottom left corner of square (should be near home) as waypoint - print("Save WP 6") + progress("Save WP 6") save_wp(mavproxy, mav) # descend to 10m - print("Descend to 10m in Loiter") + progress("Descend to 10m in Loiter") mavproxy.send('switch 5\n') # loiter mode wait_mode(mav, 'LOITER') mavproxy.send('rc 3 1300\n') time_left = timeout - (get_sim_time(mav) - tstart) - print("timeleft = %u" % time_left) + progress("timeleft = %u" % time_left) if time_left < 20: time_left = 20 if not wait_altitude(mav, -10, 10, time_left): - print("Failed to reach alt of 10m") + progress("Failed to reach alt of 10m") success = False save_wp(mavproxy, mav) @@ -237,14 +237,14 @@ def fly_square(mavproxy, mav, side=50, timeout=300): def fly_RTL(mavproxy, mav, side=60, timeout=250): """Return, land.""" - print("# Enter RTL") + progress("# Enter RTL") mavproxy.send('switch 3\n') tstart = get_sim_time(mav) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = mav.location() home_distance = get_distance(HOME, pos) - print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) if(m.alt <= 1 and home_distance < 10): return True return False @@ -258,7 +258,7 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): wait_mode(mav, 'LOITER') # first aim east - print("turn east") + progress("turn east") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 135): return False @@ -273,14 +273,14 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): hover(mavproxy, mav) # fly east 60 meters - print("# Going forward %u meters" % side) + progress("# Going forward %u meters" % side) mavproxy.send('rc 2 1350\n') if not wait_distance(mav, side, 5, 60): return False mavproxy.send('rc 2 1500\n') # pull throttle low - print("# Enter Failsafe") + progress("# Enter Failsafe") mavproxy.send('rc 3 900\n') tstart = get_sim_time(mav) @@ -288,7 +288,7 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): m = mav.recv_match(type='VFR_HUD', blocking=True) pos = mav.location() home_distance = get_distance(HOME, pos) - print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) # check if we've reached home if m.alt <= 1 and home_distance < 10: # reduce throttle @@ -296,16 +296,16 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): # switch back to stabilize mavproxy.send('switch 2\n') # land mode wait_mode(mav, 'LAND') - print("Waiting for disarm") + progress("Waiting for disarm") mav.motors_disarmed_wait() - print("Reached failsafe home OK") + progress("Reached failsafe home OK") mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') if not arm_motors(mavproxy, mav): - print("Failed to re-arm") + progress("Failed to re-arm") return False return True - print("Failed to land on failsafe RTL - timed out after %u seconds" % timeout) + progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout) # reduce throttle mavproxy.send('rc 3 1100\n') # switch back to stabilize mode @@ -341,9 +341,9 @@ def fly_battery_failsafe(mavproxy, mav, timeout=30): # return status if success: - print("Successfully entered LAND mode after battery failsafe") + progress("Successfully entered LAND mode after battery failsafe") else: - print("Failed to enter LAND mode after battery failsafe") + progress("Failed to enter LAND mode after battery failsafe") return success @@ -355,7 +355,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang wait_mode(mav, 'LOITER') # first south - print("turn south") + progress("turn south") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 180): return False @@ -377,10 +377,10 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang start = mav.location() tstart = get_sim_time(mav) tholdstart = get_sim_time(mav) - print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) + progress("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime)) # cut motor 1 to 55% efficiency - print("Cutting motor 1 to 60% efficiency") + progress("Cutting motor 1 to 60% efficiency") mavproxy.send('param set SIM_ENGINE_MUL 0.60\n') while get_sim_time(mav) < tstart + holdtime: @@ -388,21 +388,21 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang pos = mav.location() delta = get_distance(start, pos) alt_delta = math.fabs(m.alt - start_altitude) - print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) + progress("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) if alt_delta > maxaltchange: - print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + progress("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) success = False if delta > maxdistchange: - print("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) + progress("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) success = False # restore motor 1 to 100% efficiency mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') if success: - print("Stability patch and Loiter OK for %u seconds" % holdtime) + progress("Stability patch and Loiter OK for %u seconds" % holdtime) else: - print("Stability Patch FAILED") + progress("Stability Patch FAILED") return success @@ -418,7 +418,7 @@ def fly_fence_test(mavproxy, mav, timeout=180): mavproxy.send('param set AVOID_ENABLE 0\n') # first east - print("turn east") + progress("turn east") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 160): return False @@ -436,7 +436,7 @@ def fly_fence_test(mavproxy, mav, timeout=180): m = mav.recv_match(type='VFR_HUD', blocking=True) pos = mav.location() home_distance = get_distance(HOME, pos) - print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) + progress("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) # recenter pitch sticks once we reach home so we don't fly off again if pitching_forward and home_distance < 10: pitching_forward = False @@ -449,14 +449,14 @@ def fly_fence_test(mavproxy, mav, timeout=180): # switch mode to stabilize mavproxy.send('switch 2\n') # land mode wait_mode(mav, 'LAND') - print("Waiting for disarm") + progress("Waiting for disarm") mav.motors_disarmed_wait() - print("Reached home OK") + progress("Reached home OK") mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm if not arm_motors(mavproxy, mav): - print("Failed to re-arm") + progress("Failed to re-arm") mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm return False mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm @@ -473,7 +473,7 @@ def fly_fence_test(mavproxy, mav, timeout=180): wait_mode(mav, 'LAND') mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') - print("Fence test failed to reach home - timed out after %u seconds" % timeout) + progress("Fence test failed to reach home - timed out after %u seconds" % timeout) return False @@ -503,12 +503,12 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] glitch_num = len(glitch_lat) - print("GPS Glitches:") + progress("GPS Glitches:") for i in range(1, glitch_num): - print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # turn south east - print("turn south east") + progress("turn south east") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 150): if (use_map): @@ -538,7 +538,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis # initialise current glitch glitch_current = 0 - print("Apply first glitch") + progress("Apply first glitch") mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) @@ -551,11 +551,11 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis # turn off glitching if we've reached the end of the glitch list if glitch_current >= glitch_num: glitch_current = -1 - print("Completed Glitches") + progress("Completed Glitches") mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') else: - print("Applying glitch %u" % glitch_current) + progress("Applying glitch %u" % glitch_current) # move onto the next glitch mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) @@ -565,9 +565,9 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis m = mav.recv_match(type='VFR_HUD', blocking=True) curr_pos = sim_location(mav) moved_distance = get_distance(curr_pos, start_pos) - print("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) + progress("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) if moved_distance > max_distance: - print("Moved over %u meters, Failed!" % max_distance) + progress("Moved over %u meters, Failed!" % max_distance) success = False # disable gps glitch @@ -579,9 +579,9 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis show_gps_and_sim_positions(mavproxy, False) if success: - print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) + progress("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) else: - print("GPS glitch test FAILED!") + progress("GPS glitch test FAILED!") return success @@ -592,14 +592,14 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304] glitch_num = len(glitch_lat) - print("GPS Glitches:") + progress("GPS Glitches:") for i in range(1, glitch_num): - print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) + progress("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i])) # Fly mission #1 - print("# Load copter_glitch_mission") + progress("# Load copter_glitch_mission") if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_glitch_mission.txt")): - print("load copter_glitch_mission failed") + progress("load copter_glitch_mission failed") return False # turn on simulator display of gps and actual position @@ -609,7 +609,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): # load the waypoint count global homeloc global num_wp - print("test: Fly a mission from 1 to %u" % num_wp) + progress("test: Fly a mission from 1 to %u" % num_wp) mavproxy.send('wp set 1\n') # switch into AUTO mode and raise throttle @@ -629,7 +629,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): # initialise current glitch glitch_current = 0 - print("Apply first glitch") + progress("Apply first glitch") mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) @@ -641,12 +641,12 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): glitch_current = desired_glitch_num # apply next glitch if glitch_current < glitch_num: - print("Applying glitch %u" % glitch_current) + progress("Applying glitch %u" % glitch_current) mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) # turn off glitching - print("Completed Glitches") + progress("Completed Glitches") mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') @@ -659,19 +659,19 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): dist_to_home = get_distance(HOME, pos) while dist_to_home > 5: if get_sim_time(mav) > (tstart + timeout): - print("GPS Glitch testing failed - exceeded timeout %u seconds" % timeout) + progress("GPS Glitch testing failed - exceeded timeout %u seconds" % timeout) ret = False break m = mav.recv_match(type='VFR_HUD', blocking=True) pos = mav.location() dist_to_home = get_distance(HOME, pos) - print("Dist from home: %u" % dist_to_home) + progress("Dist from home: %u" % dist_to_home) # turn off simulator display of gps and actual position if (use_map): show_gps_and_sim_positions(mavproxy, False) - print("GPS Glitch test Auto completed: passed=%s" % ret) + progress("GPS Glitch test Auto completed: passed=%s" % ret) return ret @@ -695,37 +695,37 @@ def fly_simple(mavproxy, mav, side=50, timeout=120): mavproxy.send('rc 3 1500\n') # fly south 50m - print("# Flying south %u meters" % side) + progress("# Flying south %u meters" % side) mavproxy.send('rc 1 1300\n') if not wait_distance(mav, side, 5, 60): failed = True mavproxy.send('rc 1 1500\n') # fly west 8 seconds - print("# Flying west for 8 seconds") + progress("# Flying west for 8 seconds") mavproxy.send('rc 2 1300\n') tstart = get_sim_time(mav) while get_sim_time(mav) < (tstart + 8): m = mav.recv_match(type='VFR_HUD', blocking=True) delta = (get_sim_time(mav) - tstart) - # print("%u" % delta) + # progress("%u" % delta) mavproxy.send('rc 2 1500\n') # fly north 25 meters - print("# Flying north %u meters" % (side/2.0)) + progress("# Flying north %u meters" % (side/2.0)) mavproxy.send('rc 1 1700\n') if not wait_distance(mav, side/2, 5, 60): failed = True mavproxy.send('rc 1 1500\n') # fly east 8 seconds - print("# Flying east for 8 seconds") + progress("# Flying east for 8 seconds") mavproxy.send('rc 2 1700\n') tstart = get_sim_time(mav) while get_sim_time(mav) < (tstart + 8): m = mav.recv_match(type='VFR_HUD', blocking=True) delta = (get_sim_time(mav) - tstart) - # print("%u" % delta) + # progress("%u" % delta) mavproxy.send('rc 2 1500\n') # restore to default @@ -746,7 +746,7 @@ def fly_super_simple(mavproxy, mav, timeout=45): wait_mode(mav, 'LOITER') # fly forward 20m - print("# Flying forward 20 meters") + progress("# Flying forward 20 meters") mavproxy.send('rc 2 1300\n') if not wait_distance(mav, 20, 5, 60): failed = True @@ -764,7 +764,7 @@ def fly_super_simple(mavproxy, mav, timeout=45): mavproxy.send('rc 4 1550\n') # roll left for timeout seconds - print("# rolling left from pilot's point of view for %u seconds" % timeout) + progress("# rolling left from pilot's point of view for %u seconds" % timeout) mavproxy.send('rc 1 1300\n') tstart = get_sim_time(mav) while get_sim_time(mav) < (tstart + timeout): @@ -791,7 +791,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36): wait_mode(mav, 'LOITER') # face west - print("turn west") + progress("turn west") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 270): return False @@ -817,12 +817,12 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36): start_altitude = m.alt tstart = get_sim_time(mav) tholdstart = get_sim_time(mav) - print("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) + progress("Circle at %u meters for %u seconds" % (start_altitude, holdtime)) while get_sim_time(mav) < tstart + holdtime: m = mav.recv_match(type='VFR_HUD', blocking=True) - print("heading %u" % m.heading) + progress("heading %u" % m.heading) - print("CIRCLE OK for %u seconds" % holdtime) + progress("CIRCLE OK for %u seconds" % holdtime) return True @@ -830,15 +830,15 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36): def fly_auto_test(mavproxy, mav): # Fly mission #1 - print("# Load copter_mission") + progress("# Load copter_mission") if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_mission.txt")): - print("load copter_mission failed") + progress("load copter_mission failed") return False # load the waypoint count global homeloc global num_wp - print("test: Fly a mission from 1 to %u" % num_wp) + progress("test: Fly a mission from 1 to %u" % num_wp) mavproxy.send('wp set 1\n') # switch into AUTO mode and raise throttle @@ -858,9 +858,9 @@ def fly_auto_test(mavproxy, mav): # wait for disarm mav.motors_disarmed_wait() - print("MOTORS DISARMED OK") + progress("MOTORS DISARMED OK") - print("Auto mission completed: passed=%s" % ret) + progress("Auto mission completed: passed=%s" % ret) return ret @@ -869,15 +869,15 @@ def fly_auto_test(mavproxy, mav): def fly_avc_test(mavproxy, mav): # upload mission from file - print("# Load copter_AVC2013_mission") + progress("# Load copter_AVC2013_mission") if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")): - print("load copter_AVC2013_mission failed") + progress("load copter_AVC2013_mission failed") return False # load the waypoint count global homeloc global num_wp - print("Fly AVC mission from 1 to %u" % num_wp) + progress("Fly AVC mission from 1 to %u" % num_wp) mavproxy.send('wp set 1\n') # wait for motor runup @@ -896,21 +896,21 @@ def fly_avc_test(mavproxy, mav): # wait for disarm mav.motors_disarmed_wait() - print("MOTORS DISARMED OK") + progress("MOTORS DISARMED OK") - print("AVC mission completed: passed=%s" % ret) + progress("AVC mission completed: passed=%s" % ret) return ret def land(mavproxy, mav, timeout=60): """Land the quad.""" - print("STARTING LANDING") + progress("STARTING LANDING") mavproxy.send('switch 2\n') # land mode wait_mode(mav, 'LAND') - print("Entered Landing Mode") + progress("Entered Landing Mode") ret = wait_altitude(mav, -5, 1) - print("LANDING: ok= %s" % ret) + progress("LANDING: ok= %s" % ret) return ret @@ -918,7 +918,7 @@ def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None): """Fly a mission from a file.""" global homeloc global num_wp - print("test: Fly a mission from 1 to %u" % num_wp) + progress("test: Fly a mission from 1 to %u" % num_wp) mavproxy.send('wp set 1\n') mavproxy.send('switch 4\n') # auto mode wait_mode(mav, 'AUTO') @@ -926,7 +926,7 @@ def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None): expect_msg = "Reached command #%u" % (num_wp-1) if (ret): mavproxy.expect(expect_msg) - print("test: MISSION COMPLETE: passed=%s" % ret) + progress("test: MISSION COMPLETE: passed=%s" % ret) # wait here until ready mavproxy.send('switch 5\n') # loiter mode wait_mode(mav, 'LOITER') @@ -953,7 +953,7 @@ def save_mission_to_file(mavproxy, mav, filename): mavproxy.send('wp save %s\n' % filename) mavproxy.expect('Saved ([0-9]+) waypoints') num_wp = int(mavproxy.match.group(1)) - print("num_wp: %d" % num_wp) + progress("num_wp: %d" % num_wp) return True @@ -1006,17 +1006,17 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) copy_tlog = False if os.path.exists(buildlog): os.unlink(buildlog) try: os.link(logfile, buildlog) except Exception: - print("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location") + progress("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location") copy_tlog = True # the received parameters can come before or after the ready to fly message @@ -1032,7 +1032,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -1048,254 +1048,254 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal wait_ready_to_arm(mav) # Arm - print("# Arm motors") + progress("# Arm motors") if not arm_motors(mavproxy, mav): failed_test_msg = "arm_motors failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Fly a square in Stabilize mode - print("#") - print("########## Fly a square and save WPs with CH7 switch ##########") - print("#") + progress("#") + progress("########## Fly a square and save WPs with CH7 switch ##########") + progress("#") if not fly_square(mavproxy, mav): failed_test_msg = "fly_square failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # save the stored mission to file - print("# Save out the CH7 mission to file") + progress("# Save out the CH7 mission to file") if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")): failed_test_msg = "save_mission_to_file failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # fly the stored mission - print("# Fly CH7 saved mission") + progress("# Fly CH7 saved mission") if not fly_mission(mavproxy, mav, height_accuracy=0.5, target_altitude=10): failed_test_msg = "fly ch7_mission failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Throttle Failsafe - print("#") - print("########## Test Failsafe ##########") - print("#") + progress("#") + progress("########## Test Failsafe ##########") + progress("#") if not fly_throttle_failsafe(mavproxy, mav): failed_test_msg = "fly_throttle_failsafe failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Battery failsafe if not fly_battery_failsafe(mavproxy, mav): failed_test_msg = "fly_battery_failsafe failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Stability patch - print("#") - print("########## Test Stability Patch ##########") - print("#") + progress("#") + progress("########## Test Stability Patch ##########") + progress("#") if not fly_stability_patch(mavproxy, mav, 30): failed_test_msg = "fly_stability_patch failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL - print("# RTL #") + progress("# RTL #") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL after stab patch failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Fence test - print("#") - print("########## Test Horizontal Fence ##########") - print("#") + progress("#") + progress("########## Test Horizontal Fence ##########") + progress("#") if not fly_fence_test(mavproxy, mav, 180): failed_test_msg = "fly_fence_test failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Fly GPS Glitch Loiter test - print("# GPS Glitch Loiter Test") + progress("# GPS Glitch Loiter Test") if not fly_gps_glitch_loiter_test(mavproxy, mav, use_map): failed_test_msg = "fly_gps_glitch_loiter_test failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL after GPS Glitch Loiter test - print("# RTL #") + progress("# RTL #") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Fly GPS Glitch test in auto mode - print("# GPS Glitch Auto Test") + progress("# GPS Glitch Auto Test") if not fly_gps_glitch_auto_test(mavproxy, mav, use_map): failed_test_msg = "fly_gps_glitch_auto_test failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # take-off ahead of next test - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Loiter for 10 seconds - print("#") - print("########## Test Loiter for 10 seconds ##########") - print("#") + progress("#") + progress("########## Test Loiter for 10 seconds ##########") + progress("#") if not loiter(mavproxy, mav): failed_test_msg = "loiter failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Loiter Climb - print("#") - print("# Loiter - climb to 30m") - print("#") + progress("#") + progress("# Loiter - climb to 30m") + progress("#") if not change_alt(mavproxy, mav, 30): failed_test_msg = "change_alt climb failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Loiter Descend - print("#") - print("# Loiter - descend to 20m") - print("#") + progress("#") + progress("# Loiter - descend to 20m") + progress("#") if not change_alt(mavproxy, mav, 20): failed_test_msg = "change_alt descend failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL - print("#") - print("########## Test RTL ##########") - print("#") + progress("#") + progress("########## Test RTL ##########") + progress("#") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL after Loiter climb/descend failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Simple mode - print("# Fly in SIMPLE mode") + progress("# Fly in SIMPLE mode") if not fly_simple(mavproxy, mav): failed_test_msg = "fly_simple failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL - print("#") - print("########## Test RTL ##########") - print("#") + progress("#") + progress("########## Test RTL ##########") + progress("#") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL after simple mode failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Fly a circle in super simple mode - print("# Fly a circle in SUPER SIMPLE mode") + progress("# Fly a circle in SUPER SIMPLE mode") if not fly_super_simple(mavproxy, mav): failed_test_msg = "fly_super_simple failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL - print("# RTL #") + progress("# RTL #") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL after super simple mode failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Takeoff - print("# Takeoff") + progress("# Takeoff") if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # Circle mode - print("# Fly CIRCLE mode") + progress("# Fly CIRCLE mode") if not fly_circle(mavproxy, mav): failed_test_msg = "fly_circle failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True # RTL - print("#") - print("########## Test RTL ##########") - print("#") + progress("#") + progress("########## Test RTL ##########") + progress("#") if not fly_RTL(mavproxy, mav): failed_test_msg = "fly_RTL after circle failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True - print("# Fly copter mission") + progress("# Fly copter mission") if not fly_auto_test(mavproxy, mav): failed_test_msg = "fly_auto_test failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True else: - print("Flew copter mission OK") + progress("Flew copter mission OK") # wait for disarm mav.motors_disarmed_wait() if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")): failed_test_msg = "log_download failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True except pexpect.TIMEOUT as failed_test_msg: @@ -1317,7 +1317,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal shutil.copy(logfile, buildlog) if failed: - print("FAILED: %s" % failed_test_msg) + progress("FAILED: %s" % failed_test_msg) return False return True @@ -1359,10 +1359,10 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) try: @@ -1387,7 +1387,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -1400,36 +1400,36 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals setup_rc(mavproxy) homeloc = mav.location() - print("Lowering rotor speed") + progress("Lowering rotor speed") mavproxy.send('rc 8 1000\n') wait_ready_to_arm(mav) # Arm - print("# Arm motors") + progress("# Arm motors") if not arm_motors(mavproxy, mav): failed_test_msg = "arm_motors failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True - print("Raising rotor speed") + progress("Raising rotor speed") mavproxy.send('rc 8 2000\n') - print("# Fly AVC mission") + progress("# Fly AVC mission") if not fly_avc_test(mavproxy, mav): failed_test_msg = "fly_avc_test failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True else: - print("Flew AVC mission OK") + progress("Flew AVC mission OK") - print("Lowering rotor speed") + progress("Lowering rotor speed") mavproxy.send('rc 8 1000\n') # mission includes disarm at end so should be ok to download logs now if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")): failed_test_msg = "log_download failed" - print(failed_test_msg) + progress(failed_test_msg) failed = True except pexpect.TIMEOUT as failed_test_msg: @@ -1446,6 +1446,6 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log")) if failed: - print("FAILED: %s" % failed_test_msg) + progress("FAILED: %s" % failed_test_msg) return False return True diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index f809a369c0..eb3badf98b 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -58,7 +58,7 @@ def takeoff(mavproxy, mav): # level off mavproxy.send('rc 2 1500\n') - print("TAKEOFF COMPLETE") + progress("TAKEOFF COMPLETE") return True @@ -70,44 +70,44 @@ def fly_left_circuit(mavproxy, mav): if not wait_level_flight(mavproxy, mav): return False - print("Flying left circuit") + progress("Flying left circuit") # do 4 turns for i in range(0, 4): # hard left - print("Starting turn %u" % i) + progress("Starting turn %u" % i) mavproxy.send('rc 1 1000\n') if not wait_heading(mav, 270 - (90*i), accuracy=10): return False mavproxy.send('rc 1 1500\n') - print("Starting leg %u" % i) + progress("Starting leg %u" % i) if not wait_distance(mav, 100, accuracy=20): return False - print("Circuit complete") + progress("Circuit complete") return True def fly_RTL(mavproxy, mav): """Fly to home.""" - print("Flying home in RTL") + progress("Flying home in RTL") mavproxy.send('switch 2\n') wait_mode(mav, 'RTL') if not wait_location(mav, homeloc, accuracy=120, target_altitude=homeloc.alt+100, height_accuracy=20, timeout=180): return False - print("RTL Complete") + progress("RTL Complete") return True def fly_LOITER(mavproxy, mav, num_circles=4): """Loiter where we are.""" - print("Testing LOITER for %u turns" % num_circles) + progress("Testing LOITER for %u turns" % num_circles) mavproxy.send('loiter\n') wait_mode(mav, 'LOITER') m = mav.recv_match(type='VFR_HUD', blocking=True) initial_alt = m.alt - print("Initial altitude %u\n" % initial_alt) + progress("Initial altitude %u\n" % initial_alt) while num_circles > 0: if not wait_heading(mav, 0, accuracy=10, timeout=60): @@ -115,32 +115,32 @@ def fly_LOITER(mavproxy, mav, num_circles=4): if not wait_heading(mav, 180, accuracy=10, timeout=60): return False num_circles -= 1 - print("Loiter %u circles left" % num_circles) + progress("Loiter %u circles left" % num_circles) m = mav.recv_match(type='VFR_HUD', blocking=True) final_alt = m.alt - print("Final altitude %u initial %u\n" % (final_alt, initial_alt)) + progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) mavproxy.send('mode FBWA\n') wait_mode(mav, 'FBWA') if abs(final_alt - initial_alt) > 20: - print("Failed to maintain altitude") + progress("Failed to maintain altitude") return False - print("Completed Loiter OK") + progress("Completed Loiter OK") return True def fly_CIRCLE(mavproxy, mav, num_circles=1): """Circle where we are.""" - print("Testing CIRCLE for %u turns" % num_circles) + progress("Testing CIRCLE for %u turns" % num_circles) mavproxy.send('mode CIRCLE\n') wait_mode(mav, 'CIRCLE') m = mav.recv_match(type='VFR_HUD', blocking=True) initial_alt = m.alt - print("Initial altitude %u\n" % initial_alt) + progress("Initial altitude %u\n" % initial_alt) while num_circles > 0: if not wait_heading(mav, 0, accuracy=10, timeout=60): @@ -148,27 +148,27 @@ def fly_CIRCLE(mavproxy, mav, num_circles=1): if not wait_heading(mav, 180, accuracy=10, timeout=60): return False num_circles -= 1 - print("CIRCLE %u circles left" % num_circles) + progress("CIRCLE %u circles left" % num_circles) m = mav.recv_match(type='VFR_HUD', blocking=True) final_alt = m.alt - print("Final altitude %u initial %u\n" % (final_alt, initial_alt)) + progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) mavproxy.send('mode FBWA\n') wait_mode(mav, 'FBWA') if abs(final_alt - initial_alt) > 20: - print("Failed to maintain altitude") + progress("Failed to maintain altitude") return False - print("Completed CIRCLE OK") + progress("Completed CIRCLE OK") return True def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30): """Wait for level flight.""" tstart = get_sim_time(mav) - print("Waiting for level flight") + progress("Waiting for level flight") mavproxy.send('rc 1 1500\n') mavproxy.send('rc 2 1500\n') mavproxy.send('rc 4 1500\n') @@ -176,11 +176,11 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30): m = mav.recv_match(type='ATTITUDE', blocking=True) roll = math.degrees(m.roll) pitch = math.degrees(m.pitch) - print("Roll=%.1f Pitch=%.1f" % (roll, pitch)) + progress("Roll=%.1f Pitch=%.1f" % (roll, pitch)) if math.fabs(roll) <= accuracy and math.fabs(pitch) <= accuracy: - print("Attained level flight") + progress("Attained level flight") return True - print("Failed to attain level flight") + progress("Failed to attain level flight") return False @@ -196,7 +196,7 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30): if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2): return False mavproxy.send('rc 2 1500\n') - print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt) + progress("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt) return wait_level_flight(mavproxy, mav) @@ -212,7 +212,7 @@ def axial_left_roll(mavproxy, mav, count=1): wait_mode(mav, 'MANUAL') while count > 0: - print("Starting roll") + progress("Starting roll") mavproxy.send('rc 1 1000\n') if not wait_roll(mav, -150, accuracy=90): mavproxy.send('rc 1 1500\n') @@ -245,7 +245,7 @@ def inside_loop(mavproxy, mav, count=1): wait_mode(mav, 'MANUAL') while count > 0: - print("Starting loop") + progress("Starting loop") mavproxy.send('rc 2 1000\n') if not wait_pitch(mav, -60, accuracy=20): return False @@ -275,7 +275,7 @@ def test_stabilize(mavproxy, mav, count=1): count = 1 while count > 0: - print("Starting roll") + progress("Starting roll") mavproxy.send('rc 1 2000\n') if not wait_roll(mav, -150, accuracy=90): return False @@ -310,7 +310,7 @@ def test_acro(mavproxy, mav, count=1): count = 1 while count > 0: - print("Starting roll") + progress("Starting roll") mavproxy.send('rc 1 1000\n') if not wait_roll(mav, -150, accuracy=90): return False @@ -332,7 +332,7 @@ def test_acro(mavproxy, mav, count=1): count = 2 while count > 0: - print("Starting loop") + progress("Starting loop") mavproxy.send('rc 2 1000\n') if not wait_pitch(mav, -60, accuracy=20): return False @@ -364,48 +364,48 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'): m = mav.recv_match(type='VFR_HUD', blocking=True) initial_alt = m.alt - print("Initial altitude %u\n" % initial_alt) + progress("Initial altitude %u\n" % initial_alt) - print("Flying right circuit") + progress("Flying right circuit") # do 4 turns for i in range(0, 4): # hard left - print("Starting turn %u" % i) + progress("Starting turn %u" % i) mavproxy.send('rc 1 1800\n') if not wait_heading(mav, 0 + (90*i), accuracy=20, timeout=60): mavproxy.send('rc 1 1500\n') return False mavproxy.send('rc 1 1500\n') - print("Starting leg %u" % i) + progress("Starting leg %u" % i) if not wait_distance(mav, 100, accuracy=20): return False - print("Circuit complete") + progress("Circuit complete") - print("Flying rudder left circuit") + progress("Flying rudder left circuit") # do 4 turns for i in range(0, 4): # hard left - print("Starting turn %u" % i) + progress("Starting turn %u" % i) mavproxy.send('rc 4 1900\n') if not wait_heading(mav, 360 - (90*i), accuracy=20, timeout=60): mavproxy.send('rc 4 1500\n') return False mavproxy.send('rc 4 1500\n') - print("Starting leg %u" % i) + progress("Starting leg %u" % i) if not wait_distance(mav, 100, accuracy=20): return False - print("Circuit complete") + progress("Circuit complete") m = mav.recv_match(type='VFR_HUD', blocking=True) final_alt = m.alt - print("Final altitude %u initial %u\n" % (final_alt, initial_alt)) + progress("Final altitude %u initial %u\n" % (final_alt, initial_alt)) # back to FBWA mavproxy.send('mode FBWA\n') wait_mode(mav, 'FBWA') if abs(final_alt - initial_alt) > 20: - print("Failed to maintain altitude") + progress("Failed to maintain altitude") return False return wait_level_flight(mavproxy, mav) @@ -422,7 +422,7 @@ def setup_rc(mavproxy): def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None): """Fly a mission from a file.""" global homeloc - print("Flying mission %s" % filename) + progress("Flying mission %s" % filename) mavproxy.send('wp load %s\n' % filename) mavproxy.expect('Flight plan received') mavproxy.send('wp list\n') @@ -434,7 +434,7 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non if not wait_groundspeed(mav, 0, 0.5, timeout=60): return False mavproxy.expect("Auto disarmed") - print("Mission OK") + progress("Mission OK") return True @@ -458,10 +458,10 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/ArduPlane-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) try: @@ -476,13 +476,13 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals expect_list_clear() expect_list_extend([sitl, mavproxy]) - print("Started simulator") + progress("Started simulator") # get a mavlink connection going try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -491,72 +491,72 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals fail_list = [] e = 'None' try: - print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) + progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) mav.wait_heartbeat() - print("Setting up RC parameters") + progress("Setting up RC parameters") setup_rc(mavproxy) - print("Waiting for GPS fix") + progress("Waiting for GPS fix") mav.recv_match(condition='VFR_HUD.alt>10', blocking=True) mav.wait_gps_fix() while mav.location().alt < 10: mav.wait_gps_fix() homeloc = mav.location() - print("Home location: %s" % homeloc) + progress("Home location: %s" % homeloc) if not takeoff(mavproxy, mav): - print("Failed takeoff") + progress("Failed takeoff") failed = True fail_list.append("takeoff") if not fly_left_circuit(mavproxy, mav): - print("Failed left circuit") + progress("Failed left circuit") failed = True fail_list.append("left_circuit") if not axial_left_roll(mavproxy, mav, 1): - print("Failed left roll") + progress("Failed left roll") failed = True fail_list.append("left_roll") if not inside_loop(mavproxy, mav): - print("Failed inside loop") + progress("Failed inside loop") failed = True fail_list.append("inside_loop") if not test_stabilize(mavproxy, mav): - print("Failed stabilize test") + progress("Failed stabilize test") failed = True fail_list.append("stabilize") if not test_acro(mavproxy, mav): - print("Failed ACRO test") + progress("Failed ACRO test") failed = True fail_list.append("acro") if not test_FBWB(mavproxy, mav): - print("Failed FBWB test") + progress("Failed FBWB test") failed = True fail_list.append("fbwb") if not test_FBWB(mavproxy, mav, mode='CRUISE'): - print("Failed CRUISE test") + progress("Failed CRUISE test") failed = True fail_list.append("cruise") if not fly_RTL(mavproxy, mav): - print("Failed RTL") + progress("Failed RTL") failed = True fail_list.append("RTL") if not fly_LOITER(mavproxy, mav): - print("Failed LOITER") + progress("Failed LOITER") failed = True fail_list.append("LOITER") if not fly_CIRCLE(mavproxy, mav): - print("Failed CIRCLE") + progress("Failed CIRCLE") failed = True fail_list.append("LOITER") if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10, target_altitude=homeloc.alt+100): - print("Failed mission") + progress("Failed mission") failed = True fail_list.append("mission") if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduPlane-log.bin")): - print("Failed log download") + progress("Failed log download") failed = True fail_list.append("log_download") except pexpect.TIMEOUT as e: - print("Failed with timeout") + progress("Failed with timeout") failed = True fail_list.append("timeout") @@ -570,6 +570,7 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log")) if failed: - print("FAILED: %s" % e, fail_list) + progress("FAILED: %s" % e) + progress("Fail list: %s" % fail_list) return False return True diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 2107d8b40e..d8ce6e4db9 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -26,7 +26,7 @@ def arm_sub(mavproxy, mav): mavproxy.send('arm throttle\n') mavproxy.expect('ARMED') - print("SUB ARMED") + progress("SUB ARMED") return True def dive_manual(mavproxy, mav): @@ -65,19 +65,19 @@ def dive_manual(mavproxy, mav): # wait for disarm mav.motors_disarmed_wait() - print("Manual dive OK") + progress("Manual dive OK") return True def dive_mission(mavproxy, mav, filename): - print("Executing mission %s" % filename) + progress("Executing mission %s" % filename) mavproxy.send('wp load %s\n' % filename) mavproxy.expect('Flight plan received') mavproxy.send('wp list\n') mavproxy.expect('Saved [0-9]+ waypoints') if not arm_sub(mavproxy, mav): - print("Failed to ARM") + progress("Failed to ARM") return False mavproxy.send('mode auto\n') @@ -91,7 +91,7 @@ def dive_mission(mavproxy, mav, filename): # wait for disarm mav.motors_disarmed_wait() - print("Mission OK") + progress("Mission OK") return True def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10): @@ -127,10 +127,10 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) try: @@ -145,13 +145,13 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False expect_list_clear() expect_list_extend([sitl, mavproxy]) - print("Started simulator") + progress("Started simulator") # get a mavlink connection going try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -159,30 +159,30 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False failed = False e = 'None' try: - print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) + progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) mav.wait_heartbeat() - print("Waiting for GPS fix") + progress("Waiting for GPS fix") mav.wait_gps_fix() # wait for EKF and GPS checks to pass mavproxy.expect('IMU0 is using GPS') homeloc = mav.location() - print("Home location: %s" % homeloc) + progress("Home location: %s" % homeloc) if not arm_sub(mavproxy, mav): - print("Failed to ARM") + progress("Failed to ARM") failed = True if not dive_manual(mavproxy, mav): - print("Failed manual dive") + progress("Failed manual dive") failed = True if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub_mission.txt")): - print("Failed auto mission") + progress("Failed auto mission") failed = True if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")): - print("Failed log download") + progress("Failed log download") failed = True except pexpect.TIMEOUT as e: - print("Failed with timeout") + progress("Failed with timeout") failed = True mav.close() @@ -195,6 +195,6 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) if failed: - print("FAILED: %s" % e) + progress("FAILED: %s" % e) return False return True diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 8ebf3f224c..f281b7105e 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -22,16 +22,16 @@ def wait_ekf_happy(mav, timeout=30): tstart = get_sim_time(mav) required_value = 831 - print("Waiting for EKF value %u" % (required_value)) + progress("Waiting for EKF value %u" % (required_value)) while timeout is None or get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True) current = m.flags if (tstart - get_sim_time(mav)) % 5 == 0: - print("Wait EKF.flags: required:%u current:%u" % (required_value, current)) + progress("Wait EKF.flags: required:%u current:%u" % (required_value, current)) if current == required_value: - print("EKF Flags OK") + progress("EKF Flags OK") return - print("Failed to get EKF.flags=%u" % required_value) + progress("Failed to get EKF.flags=%u" % required_value) raise AutoTestTimeoutException() def expect_list_clear(): @@ -68,6 +68,13 @@ def expect_callback(e): util.pexpect_drain(p) +''' +SIM UTILITIES +''' +def progress(text): + """Display autotest progress text.""" + print("AUTOTEST: " + text) + def get_distance(loc1, loc2): """Get ground distance between two locations.""" dlat = loc2.lat - loc1.lat @@ -105,7 +112,7 @@ def set_parameter(mavproxy, name ,value): if float(returned_value) == float(value): # yes, exactly equal. break - print("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value)) + progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value)) def get_parameter(mavproxy, name): mavproxy.send("param fetch %s\n" % (name)) @@ -118,75 +125,75 @@ def wait_altitude(mav, alt_min, alt_max, timeout=30): previous_alt = 0 tstart = get_sim_time(mav) - print("Waiting for altitude between %u and %u" % (alt_min, alt_max)) + progress("Waiting for altitude between %u and %u" % (alt_min, alt_max)) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) climb_rate = m.alt - previous_alt previous_alt = m.alt - print("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate)) + progress("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate)) if m.alt >= alt_min and m.alt <= alt_max: - print("Altitude OK") + progress("Altitude OK") return True - print("Failed to attain altitude range") + progress("Failed to attain altitude range") return False def wait_groundspeed(mav, gs_min, gs_max, timeout=30): """Wait for a given ground speed range.""" tstart = get_sim_time(mav) - print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max)) + progress("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max)) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) - print("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min)) + progress("Wait groundspeed %.1f, target:%.1f" % (m.groundspeed, gs_min)) if m.groundspeed >= gs_min and m.groundspeed <= gs_max: return True - print("Failed to attain groundspeed range") + progress("Failed to attain groundspeed range") return False def wait_roll(mav, roll, accuracy, timeout=30): """Wait for a given roll in degrees.""" tstart = get_sim_time(mav) - print("Waiting for roll of %d at %s" % (roll, time.ctime())) + progress("Waiting for roll of %d at %s" % (roll, time.ctime())) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='ATTITUDE', blocking=True) p = math.degrees(m.pitch) r = math.degrees(m.roll) - print("Roll %d Pitch %d" % (r, p)) + progress("Roll %d Pitch %d" % (r, p)) if math.fabs(r - roll) <= accuracy: - print("Attained roll %d" % roll) + progress("Attained roll %d" % roll) return True - print("Failed to attain roll %d" % roll) + progress("Failed to attain roll %d" % roll) return False def wait_pitch(mav, pitch, accuracy, timeout=30): """Wait for a given pitch in degrees.""" tstart = get_sim_time(mav) - print("Waiting for pitch of %u at %s" % (pitch, time.ctime())) + progress("Waiting for pitch of %u at %s" % (pitch, time.ctime())) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='ATTITUDE', blocking=True) p = math.degrees(m.pitch) r = math.degrees(m.roll) - print("Pitch %d Roll %d" % (p, r)) + progress("Pitch %d Roll %d" % (p, r)) if math.fabs(p - pitch) <= accuracy: - print("Attained pitch %d" % pitch) + progress("Attained pitch %d" % pitch) return True - print("Failed to attain pitch %d" % pitch) + progress("Failed to attain pitch %d" % pitch) return False def wait_heading(mav, heading, accuracy=5, timeout=30): """Wait for a given heading.""" tstart = get_sim_time(mav) - print("Waiting for heading %u with accuracy %u" % (heading, accuracy)) + progress("Waiting for heading %u with accuracy %u" % (heading, accuracy)) while get_sim_time(mav) < tstart + timeout: m = mav.recv_match(type='VFR_HUD', blocking=True) - print("Heading %u" % m.heading) + progress("Heading %u" % m.heading) if math.fabs(m.heading - heading) <= accuracy: - print("Attained heading %u" % heading) + progress("Attained heading %u" % heading) return True - print("Failed to attain heading %u" % heading) + progress("Failed to attain heading %u" % heading) return False @@ -197,14 +204,14 @@ def wait_distance(mav, distance, accuracy=5, timeout=30): while get_sim_time(mav) < tstart + timeout: pos = mav.location() delta = get_distance(start, pos) - print("Distance %.2f meters" % delta) + progress("Distance %.2f meters" % delta) if math.fabs(delta - distance) <= accuracy: - print("Attained distance %.2f meters OK" % delta) + progress("Attained distance %.2f meters OK" % delta) return True if delta > (distance + accuracy): - print("Failed distance - overshoot delta=%f distance=%f" % (delta, distance)) + progress("Failed distance - overshoot delta=%f distance=%f" % (delta, distance)) return False - print("Failed to attain distance %u" % distance) + progress("Failed to attain distance %u" % distance) return False @@ -213,18 +220,18 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height tstart = get_sim_time(mav) if target_altitude is None: target_altitude = loc.alt - print("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % ( + progress("Waiting for location %.4f,%.4f at altitude %.1f height_accuracy=%.1f" % ( loc.lat, loc.lng, target_altitude, height_accuracy)) while get_sim_time(mav) < tstart + timeout: pos = mav.location() delta = get_distance(loc, pos) - print("Distance %.2f meters alt %.1f" % (delta, pos.alt)) + progress("Distance %.2f meters alt %.1f" % (delta, pos.alt)) if delta <= accuracy: if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy: continue - print("Reached location (%.2f meters)" % delta) + progress("Reached location (%.2f meters)" % delta) return True - print("Failed to attain location") + progress("Failed to attain location") return False @@ -236,9 +243,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time current_wp = start_wp mode = mav.flightmode - print("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) + progress("\ntest: wait for waypoint ranges start=%u end=%u\n\n" % (wpnum_start, wpnum_end)) # if start_wp != wpnum_start: - # print("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp)) + # progress("test: Expected start waypoint %u but got %u" % (wpnum_start, start_wp)) # return False while get_sim_time(mav) < tstart + timeout: @@ -249,27 +256,27 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time # if we changed mode, fail if mav.flightmode != mode: - print('Exited %s mode' % mode) + progress('Exited %s mode' % mode) return False - print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) + progress("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): - print("test: Starting new waypoint %u" % seq) + progress("test: Starting new waypoint %u" % seq) tstart = get_sim_time(mav) current_wp = seq # the wp_dist check is a hack until we can sort out the right seqnum # for end of mission # if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2): if (current_wp == wpnum_end and wp_dist < max_dist): - print("Reached final waypoint %u" % seq) + progress("Reached final waypoint %u" % seq) return True if (seq >= 255): - print("Reached final waypoint %u" % seq) + progress("Reached final waypoint %u" % seq) return True if seq > current_wp+1: - print("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) + progress("Failed: Skipped waypoint! Got wp %u expected %u" % (seq, current_wp+1)) return False - print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) + progress("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) return False @@ -286,9 +293,9 @@ def save_wp(mavproxy, mav): def wait_mode(mav, mode, timeout=None): - print("Waiting for mode %s" % mode) + progress("Waiting for mode %s" % mode) mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True) - print("Got mode %s" % mode) + progress("Got mode %s" % mode) return mav.flightmode diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 818ff65e6f..4c38d7599c 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -24,7 +24,7 @@ homeloc = None def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): """Fly a mission from a file.""" - print("Flying mission %s" % filename) + progress("Flying mission %s" % filename) mavproxy.send('wp load %s\n' % filename) mavproxy.expect('Flight plan received') mavproxy.send('fence load %s\n' % fence) @@ -42,7 +42,7 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): if not wait_waypoint(mav, 20, 34, max_dist=60, timeout=1200): return False mavproxy.expect('DISARMED') - print("Mission OK") + progress("Mission OK") return True @@ -65,10 +65,10 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options) mavproxy.expect('Telemetry log: (\S+)') logfile = mavproxy.match.group(1) - print("LOGFILE %s" % logfile) + progress("LOGFILE %s" % logfile) buildlog = util.reltopdir("../buildlogs/QuadPlane-test.tlog") - print("buildlog=%s" % buildlog) + progress("buildlog=%s" % buildlog) if os.path.exists(buildlog): os.unlink(buildlog) try: @@ -83,13 +83,13 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals expect_list_clear() expect_list_extend([sitl, mavproxy]) - print("Started simulator") + progress("Started simulator") # get a mavlink connection going try: mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) except Exception as msg: - print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) + progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg) raise mav.message_hooks.append(message_hook) mav.idle_hooks.append(idle_hook) @@ -97,15 +97,15 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals failed = False e = 'None' try: - print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) + progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION) mav.wait_heartbeat() - print("Waiting for GPS fix") + progress("Waiting for GPS fix") mav.recv_match(condition='VFR_HUD.alt>10', blocking=True) mav.wait_gps_fix() while mav.location().alt < 10: mav.wait_gps_fix() homeloc = mav.location() - print("Home location: %s" % homeloc) + progress("Home location: %s" % homeloc) # wait for EKF and GPS checks to pass wait_seconds(mav, 30) @@ -116,10 +116,10 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals if not fly_mission(mavproxy, mav, os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"), os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")): - print("Failed mission") + progress("Failed mission") failed = True except pexpect.TIMEOUT as e: - print("Failed with timeout") + progress("Failed with timeout") failed = True mav.close() @@ -132,6 +132,6 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log")) if failed: - print("FAILED: %s" % e) + progress("FAILED: %s" % e) return False return True