Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fix copter test

Browse Source 
Allow more time for fly_square
Allow more movement in loiter_glitch test
This commit is contained in:
Randy Mackay 2015-04-08 12:15:14 +09:00
parent 7eb710ed3a
commit 8335399525
1 changed files with 63 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
Tools/autotest/arducopter.py
View File

@ -89,6 +89,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
if not wait_groundspeed(mav, 0, 2): if not wait_groundspeed(mav, 0, 2):
return False return False
success = True
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt start_altitude = m.alt
start = mav.location() 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) m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location() pos = mav.location()
delta = get_distance(start, pos) delta = get_distance(start, pos)
alt_delta = math.fabs(m.alt - start_altitude)
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
if math.fabs(m.alt - start_altitude) > maxaltchange: if alt_delta > maxaltchange:
tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange))
success = False
if delta > maxdistchange: if delta > maxdistchange:
tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails print("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange))
if get_sim_time(mav) - tholdstart > holdtime: success = False
print("Loiter OK for %u seconds" % holdtime) if success:
return True print("Loiter OK for %u seconds" % holdtime)
print("Loiter FAILED") else:
return False print("Loiter FAILED")
return success
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080): def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
'''change altitude''' '''change altitude'''
@ -125,10 +129,10 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
return True return True
# fly a square in stabilize mode # 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''' '''fly a square, flying N then E'''
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
failed = False success = True
# ensure all sticks in the middle # ensure all sticks in the middle
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
@ -144,7 +148,8 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
print("turn right towards north") print("turn right towards north")
mavproxy.send('rc 4 1580\n') mavproxy.send('rc 4 1580\n')
if not wait_heading(mav, 10): if not wait_heading(mav, 10):
return False print("Failed to reach heading")
success = False
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan4_raw==1500', blocking=True) 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) print("Going north %u meters" % side)
mavproxy.send('rc 2 1300\n') mavproxy.send('rc 2 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True print("Failed to reach distance of %u") % side
success = False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# save top left corner of square as waypoint # 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) print("Going east %u meters" % side)
mavproxy.send('rc 1 1700\n') mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True print("Failed to reach distance of %u") % side
success = False
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
# save top right corner of square as waypoint # 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) print("Going south %u meters" % side)
mavproxy.send('rc 2 1700\n') mavproxy.send('rc 2 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True print("Failed to reach distance of %u") % side
success = False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# save bottom right corner of square as waypoint # 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) print("Going west %u meters" % side)
mavproxy.send('rc 1 1300\n') mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
failed = True print("Failed to reach distance of %u") % side
success = False
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
# save bottom left corner of square (should be near home) as waypoint # save bottom left corner of square (should be near home) as waypoint
print("Save WP 6") print("Save WP 6")
save_wp(mavproxy, mav) 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): def fly_RTL(mavproxy, mav, side=60, timeout=250):
'''Return, land''' '''Return, land'''
@ -329,6 +352,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
if not wait_groundspeed(mav, 0, 2): if not wait_groundspeed(mav, 0, 2):
return False return False
success = True
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt start_altitude = m.alt
start = mav.location() 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) m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = mav.location() pos = mav.location()
delta = get_distance(start, pos) delta = get_distance(start, pos)
alt_delta = math.fabs(m.alt - start_altitude)
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt)) print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
if math.fabs(m.alt - start_altitude) > maxaltchange: if alt_delta > maxaltchange:
tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails print("Loiter alt shifted %u meters (> limit of %u)" % (alt_delta, maxaltchange))
success = False
if delta > maxdistchange: if delta > maxdistchange:
tholdstart = get_sim_time(mav) # this will cause this test to timeout and fails print("Loiter shifted %u meters (> limit of %u)" % (delta, maxdistchange))
if get_sim_time(mav) - tholdstart > holdtime: success = False
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")
# restore motor 1 to 100% efficiency # restore motor 1 to 100% efficiency
mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') 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 # fly_fence_test - fly east until you hit the horizontal circular fence
def fly_fence_test(mavproxy, mav, timeout=180): 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') mavproxy.send('map set showsimpos 0\n')
# 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
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''' '''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') 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 # record time and position
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
start_pos = sim_location(mav) start_pos = sim_location(mav)
success = True
# initialise current glitch # initialise current glitch
glitch_current = 0; 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)) print("Alt: %u Moved: %.0f" % (m.alt, moved_distance))
if moved_distance > max_distance: if moved_distance > max_distance:
print("Moved over %u meters, Failed!" % max_distance) print("Moved over %u meters, Failed!" % max_distance)
# disable gps glitch success = False
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
# disable gps glitch # disable gps glitch
if glitch_current != -1: 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') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
show_gps_and_sim_positions(mavproxy, False) show_gps_and_sim_positions(mavproxy, False)
# if we've gotten this far then we've succeeded if success:
print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout)) print("GPS glitch test passed! stayed within %u meters for %u seconds" % (max_distance, timeout))
return True else:
print("GPS glitch test FAILED!")
return success
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch # 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): 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) print(failed_test_msg)
failed = True 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 # save the stored mission to file
print("# Save out the CH7 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")): 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) print(failed_test_msg)
failed = True failed = True
# takeoff
if not takeoff(mavproxy, mav, 10): if not takeoff(mavproxy, mav, 10):
failed_test_msg = "takeoff failed" failed_test_msg = "takeoff failed"
print(failed_test_msg) print(failed_test_msg)