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

autotest: fixed target altitude for RTL mission

Browse Source
This commit is contained in:
Andrew Tridgell 2011-11-10 12:56:20 +11:00
parent 6211cc4e50
commit 6263bb8936
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/autotest/arducopter.py
View File

@ -162,16 +162,18 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
return False return False
def wait_location(mav, loc, accuracy=5, timeout=30, height_accuracy=-1): def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
'''wait for arrival at a location''' '''wait for arrival at a location'''
tstart = time.time() tstart = time.time()
if target_altitude is None:
target_altitude = loc.alt
while time.time() < tstart + timeout: while time.time() < tstart + timeout:
m = mav.recv_match(type='GPS_RAW', blocking=True) m = mav.recv_match(type='GPS_RAW', blocking=True)
pos = current_location(mav) pos = current_location(mav)
delta = get_distance(loc, pos) delta = get_distance(loc, pos)
print("Distance %.2f meters" % delta) print("Distance %.2f meters" % delta)
if delta <= accuracy: if delta <= accuracy:
if height_accuracy != -1 and math.fabs(pos.alt - loc.alt) > height_accuracy: if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy:
continue continue
print("Reached location (%.2f meters)" % delta) print("Reached location (%.2f meters)" % delta)
return True return True
@ -238,7 +240,7 @@ def land(mavproxy, mav, timeout=60):
return False return False
def fly_mission(mavproxy, mav, filename, height_accuracy=-1): def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file''' '''fly a mission from a file'''
global homeloc global homeloc
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
@ -248,7 +250,7 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1):
mavproxy.send('switch 1\n') # auto mode mavproxy.send('switch 1\n') # auto mode
mavproxy.expect('AUTO>') mavproxy.expect('AUTO>')
wait_distance(mav, 30, timeout=120) wait_distance(mav, 30, timeout=120)
wait_location(mav, homeloc, timeout=600, height_accuracy=height_accuracy) wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy)
def setup_rc(mavproxy): def setup_rc(mavproxy):
@ -328,7 +330,7 @@ def fly_ArduCopter():
loiter(mavproxy, mav) loiter(mavproxy, mav)
land(mavproxy, mav) land(mavproxy, mav)
#fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2) #fly_mission(mavproxy, mav, os.path.join(testdir, "mission_ttt.txt"), height_accuracy=0.2)
fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.2) fly_mission(mavproxy, mav, os.path.join(testdir, "mission1.txt"), height_accuracy = 0.5, target_altitude=10)
land(mavproxy, mav) land(mavproxy, mav)
disarm_motors(mavproxy) disarm_motors(mavproxy)
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT, e: