From 8335399525991376402ac5549f0e0d10e21117b6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Apr 2015 12:15:14 +0900 Subject: [PATCH] SITL: fix copter test Allow more time for fly_square Allow more movement in loiter_glitch test --- Tools/autotest/arducopter.py | 107 +++++++++++++++++++++-------------- 1 file changed, 63 insertions(+), 44 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index cc16c5814a..f357210be7 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -89,6 +89,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): if not wait_groundspeed(mav, 0, 2): return False + success = True m = mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = mav.location() @@ -99,16 +100,19 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): 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)) - if math.fabs(m.alt - start_altitude) > maxaltchange: - tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails + if alt_delta > maxaltchange: + print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + success = False if delta > maxdistchange: - tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails - if get_sim_time(mav) - tholdstart > holdtime: - print("Loiter OK for %u seconds" % holdtime) - return True - print("Loiter FAILED") - return False + print("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange)) + success = False + if success: + print("Loiter OK for %u seconds" % holdtime) + else: + print("Loiter FAILED") + return success def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080): '''change altitude''' @@ -125,10 +129,10 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108 return True # fly a square in stabilize mode -def fly_square(mavproxy, mav, side=50, timeout=120): +def fly_square(mavproxy, mav, side=50, timeout=300): '''fly a square, flying N then E''' tstart = get_sim_time(mav) - failed = False + success = True # ensure all sticks in the middle mavproxy.send('rc 1 1500\n') @@ -144,7 +148,8 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("turn right towards north") mavproxy.send('rc 4 1580\n') if not wait_heading(mav, 10): - return False + print("Failed to reach heading") + success = False mavproxy.send('rc 4 1500\n') mav.recv_match(condition='RC_CHANNELS_RAW.chan4_raw==1500', blocking=True) @@ -161,7 +166,8 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("Going north %u meters" % side) mavproxy.send('rc 2 1300\n') if not wait_distance(mav, side): - failed = True + print("Failed to reach distance of %u") % side + success = False mavproxy.send('rc 2 1500\n') # save top left corner of square as waypoint @@ -172,7 +178,8 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("Going east %u meters" % side) mavproxy.send('rc 1 1700\n') if not wait_distance(mav, side): - failed = True + print("Failed to reach distance of %u") % side + success = False mavproxy.send('rc 1 1500\n') # save top right corner of square as waypoint @@ -183,7 +190,8 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("Going south %u meters" % side) mavproxy.send('rc 2 1700\n') if not wait_distance(mav, side): - failed = True + print("Failed to reach distance of %u") % side + success = False mavproxy.send('rc 2 1500\n') # save bottom right corner of square as waypoint @@ -194,14 +202,29 @@ def fly_square(mavproxy, mav, side=50, timeout=120): print("Going west %u meters" % side) mavproxy.send('rc 1 1300\n') if not wait_distance(mav, side): - failed = True + print("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") save_wp(mavproxy, mav) - return not failed + # descend to 10m + print("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) + if time_left < 20: + time_left = 20 + if not wait_altitude(mav, -10, 10, time_left): + print("Failed to reach alt of 10m") + success = False + save_wp(mavproxy, mav) + + return success def fly_RTL(mavproxy, mav, side=60, timeout=250): '''Return, land''' @@ -329,6 +352,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang if not wait_groundspeed(mav, 0, 2): return False + success = True m = mav.recv_match(type='VFR_HUD', blocking=True) start_altitude = m.alt start = mav.location() @@ -344,20 +368,24 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang 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)) - if math.fabs(m.alt - start_altitude) > maxaltchange: - tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails + if alt_delta > maxaltchange: + print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange)) + success = False if delta > maxdistchange: - tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails - if get_sim_time(mav) - tholdstart > holdtime: - print("Stability patch and Loiter OK for %u seconds" % holdtime) - # restore motor 1 to 100% efficiency - mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') - return True - print("Stability Patch FAILED") + print("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') - return False + + if success: + print("Stability patch and Loiter OK for %u seconds" % holdtime) + else: + print("Stability Patch FAILED") + + return success # fly_fence_test - fly east until you hit the horizontal circular fence def fly_fence_test(mavproxy, mav, timeout=180): @@ -427,7 +455,7 @@ def show_gps_and_sim_positions(mavproxy, on_off): mavproxy.send('map set showsimpos 0\n') # fly_gps_glitch_loiter_test - fly south east in loiter and test reaction to gps glitch -def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=10): +def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=20): '''hold loiter position''' mavproxy.send('switch 5\n') # loiter mode wait_mode(mav, 'LOITER') @@ -466,6 +494,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=10): # record time and position tstart = get_sim_time(mav) start_pos = sim_location(mav) + success = True # initialise current glitch glitch_current = 0; @@ -495,11 +524,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=10): print("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) if moved_distance > max_distance: print("Moved over %u meters, Failed!" % max_distance) - # disable gps glitch - mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') - mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') - show_gps_and_sim_positions(mavproxy, False) - return False + success = False # disable gps glitch if glitch_current != -1: @@ -508,9 +533,11 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=10): mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') show_gps_and_sim_positions(mavproxy, False) - # if we've gotten this far then we've succeeded - print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) - return True + if success: + print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) + else: + print("GPS glitch test FAILED!") + return success # fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch def fly_gps_glitch_auto_test(mavproxy, mav, timeout=30, max_distance=100): @@ -975,15 +1002,6 @@ def fly_ArduCopter(viewerip=None, map=False): print(failed_test_msg) failed = True - print("# Land") - if not land(mavproxy, mav): - failed_test_msg = "land failed" - print(failed_test_msg) - failed = True - - print("Save landing WP") - save_wp(mavproxy, mav) - # save the stored mission to file print("# Save out the CH7 mission to file") if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")): @@ -1121,6 +1139,7 @@ def fly_ArduCopter(viewerip=None, map=False): print(failed_test_msg) failed = True + # takeoff if not takeoff(mavproxy, mav, 10): failed_test_msg = "takeoff failed" print(failed_test_msg)