Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: shorten GPS glitch duration for copter

Browse Source
This commit is contained in:
Randy Mackay 2015-04-15 17:04:32 +09:00
parent dffa2e19bf
commit 90b02a2685
1 changed files with 28 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
Tools/autotest/arducopter.py
View File

@ -487,12 +487,13 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=20):
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# wait for copter to slow down # wait for copter to slow down
if not wait_groundspeed(mav, 0, 2): if not wait_groundspeed(mav, 0, 1):
show_gps_and_sim_positions(mavproxy, False) show_gps_and_sim_positions(mavproxy, False)
return False return False
# record time and position # record time and position
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
tnow = tstart
start_pos = sim_location(mav) start_pos = sim_location(mav)
success = True success = True
@ -503,28 +504,32 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, timeout=30, max_distance=20):
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
# record position for 30 seconds # record position for 30 seconds
while get_sim_time(mav) < tstart + timeout: while tnow < tstart + timeout:
tnow = get_sim_time(mav)
time_in_sec = int(get_sim_time(mav) - tstart); desired_glitch_num = int((tnow - tstart) * 2.2)
if time_in_sec > glitch_current and glitch_current != -1: if desired_glitch_num > glitch_current and glitch_current != -1:
glitch_current = time_in_sec glitch_current = desired_glitch_num
# turn off glitching if we've reached the end of the glitch list # turn off glitching if we've reached the end of the glitch list
if glitch_current >= glitch_num: if glitch_current >= glitch_num:
glitch_current = -1 glitch_current = -1
print("Completed Glitches")
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
else: else:
print("Applying glitch %u" % glitch_current)
#move onto the next glitch #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_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
m = mav.recv_match(type='VFR_HUD', blocking=True) # start displaying distance moved after all glitches applied
curr_pos = sim_location(mav) if (glitch_current == -1):
moved_distance = get_distance(curr_pos, start_pos) m = mav.recv_match(type='VFR_HUD', blocking=True)
print("Alt: %u Moved: %.0f" % (m.alt, moved_distance)) curr_pos = sim_location(mav)
if moved_distance > max_distance: moved_distance = get_distance(curr_pos, start_pos)
print("Moved over %u meters, Failed!" % max_distance) print("Alt: %u Moved: %.0f" % (m.alt, moved_distance))
success = False if moved_distance > max_distance:
print("Moved over %u meters, Failed!" % max_distance)
success = False
# disable gps glitch # disable gps glitch
if glitch_current != -1: if glitch_current != -1:
@ -577,6 +582,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, timeout=30, max_distance=100):
# record time and position # record time and position
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
tnow = tstart
start_pos = sim_location(mav) start_pos = sim_location(mav)
# initialise current glitch # initialise current glitch
@ -587,15 +593,18 @@ def fly_gps_glitch_auto_test(mavproxy, mav, timeout=30, max_distance=100):
# record position for 30 seconds # record position for 30 seconds
while glitch_current < glitch_num: while glitch_current < glitch_num:
time_in_sec = int(get_sim_time(mav) - tstart); tnow = get_sim_time(mav)
if (time_in_sec * 2) > glitch_current and glitch_current != -1: desired_glitch_num = int((tnow - tstart) * 2)
glitch_current = (time_in_sec * 2) if desired_glitch_num > glitch_current and glitch_current != -1:
glitch_current = desired_glitch_num
# apply next glitch # apply next glitch
if glitch_current < glitch_num: if glitch_current < glitch_num:
print("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_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
# turn off glitching # turn off glitching
print("Completed Glitches")
mavproxy.send('param set SIM_GPS_GLITCH_X 0\n') mavproxy.send('param set SIM_GPS_GLITCH_X 0\n')
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
@ -1229,6 +1238,9 @@ def fly_ArduCopter(viewerip=None, map=False):
else: else:
print("Flew copter mission OK") print("Flew copter mission OK")
# wait for disarm
mav.motors_disarmed_wait()
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")): if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduCopter-log.bin")):
failed_test_msg = "log_download failed" failed_test_msg = "log_download failed"
print(failed_test_msg) print(failed_test_msg)