SITL: fix copter test
Allow more time for fly_square Allow more movement in loiter_glitch test
This commit is contained in:
Randy Mackay
parent
7eb710ed3a
commit
8335399525
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user