mirror of https://github.com/ArduPilot/ardupilot
351 lines
11 KiB
Python
351 lines
11 KiB
Python
# fly ArduCopter in SIL
|
|
|
|
import util, pexpect, sys, time, math, shutil, os
|
|
|
|
# get location of scripts
|
|
testdir=os.path.dirname(os.path.realpath(__file__))
|
|
|
|
sys.path.insert(0, util.reltopdir('../pymavlink'))
|
|
import mavutil
|
|
|
|
HOME_LOCATION='-35.362938,149.165085,584,270'
|
|
|
|
homeloc = None
|
|
|
|
# a list of pexpect objects to read while waiting for
|
|
# messages. This keeps the output to stdout flowing
|
|
expect_list = []
|
|
|
|
def message_hook(mav, msg):
|
|
'''called as each mavlink msg is received'''
|
|
global expect_list
|
|
if msg.get_type() in [ 'NAV_CONTROLLER_OUTPUT', 'GPS_RAW' ]:
|
|
print(msg)
|
|
for p in expect_list:
|
|
try:
|
|
p.read_nonblocking(100, timeout=0)
|
|
except pexpect.TIMEOUT:
|
|
pass
|
|
|
|
def expect_callback(e):
|
|
'''called when waiting for a expect pattern'''
|
|
global expect_list
|
|
for p in expect_list:
|
|
if p == e:
|
|
continue
|
|
try:
|
|
while p.read_nonblocking(100, timeout=0):
|
|
pass
|
|
except pexpect.TIMEOUT:
|
|
pass
|
|
|
|
|
|
class location(object):
|
|
'''represent a GPS coordinate'''
|
|
def __init__(self, lat, lng, alt=0):
|
|
self.lat = lat
|
|
self.lng = lng
|
|
self.alt = alt
|
|
|
|
def get_distance(loc1, loc2):
|
|
'''get ground distance between two locations'''
|
|
dlat = loc2.lat - loc1.lat
|
|
dlong = loc2.lng - loc1.lng
|
|
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
|
|
|
|
def get_bearing(loc1, loc2):
|
|
'''get bearing from loc1 to loc2'''
|
|
off_x = loc2.lng - loc1.lng
|
|
off_y = loc2.lat - loc1.lat
|
|
bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
|
|
if bearing < 0:
|
|
bearing += 360.00
|
|
return bearing;
|
|
|
|
def current_location(mav):
|
|
'''return current location'''
|
|
return location(mav.messages['GPS_RAW'].lat,
|
|
mav.messages['GPS_RAW'].lon,
|
|
mav.messages['VFR_HUD'].alt)
|
|
|
|
def wait_altitude(mav, alt_min, alt_max, timeout=30):
|
|
'''wait for a given altitude range'''
|
|
tstart = time.time()
|
|
print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
print("Altitude %u" % m.alt)
|
|
if m.alt >= alt_min and m.alt <= alt_max:
|
|
return True
|
|
print("Failed to attain altitude range")
|
|
return False
|
|
|
|
|
|
def arm_motors(mavproxy):
|
|
'''arm motors'''
|
|
print("Arming motors")
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('rc 3 1000\n')
|
|
mavproxy.send('rc 4 2000\n')
|
|
mavproxy.expect('APM: ARMING MOTORS')
|
|
mavproxy.send('rc 4 1500\n')
|
|
print("MOTORS ARMED OK")
|
|
|
|
def disarm_motors(mavproxy):
|
|
'''disarm motors'''
|
|
print("Disarming motors")
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mavproxy.send('rc 3 1000\n')
|
|
mavproxy.send('rc 4 1000\n')
|
|
mavproxy.expect('APM: DISARMING MOTORS')
|
|
mavproxy.send('rc 4 1500\n')
|
|
print("MOTORS DISARMED OK")
|
|
|
|
|
|
def takeoff(mavproxy, mav):
|
|
'''takeoff get to 30m altitude'''
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('rc 3 1500\n')
|
|
wait_altitude(mav, 30, 40)
|
|
print("TAKEOFF COMPLETE")
|
|
|
|
|
|
def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
|
|
'''hold loiter position'''
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
mavproxy.expect('LOITER>')
|
|
mavproxy.send('status\n')
|
|
mavproxy.expect('>')
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
start_altitude = m.alt
|
|
tstart = time.time()
|
|
tholdstart = time.time()
|
|
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
print("Altitude %u" % m.alt)
|
|
if math.fabs(m.alt - start_altitude) > maxaltchange:
|
|
tholdstart = time.time()
|
|
if time.time() - tholdstart > holdtime:
|
|
print("Loiter OK for %u seconds" % holdtime)
|
|
return True
|
|
print("Loiter FAILED")
|
|
return False
|
|
|
|
|
|
def wait_heading(mav, heading, accuracy=5, timeout=30):
|
|
'''wait for a given heading'''
|
|
tstart = time.time()
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
print("Heading %u" % m.heading)
|
|
if math.fabs(m.heading - heading) <= accuracy:
|
|
return True
|
|
print("Failed to attain heading %u" % heading)
|
|
return False
|
|
|
|
|
|
def wait_distance(mav, distance, accuracy=5, timeout=30):
|
|
'''wait for flight of a given distance'''
|
|
tstart = time.time()
|
|
start = current_location(mav)
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='GPS_RAW', blocking=True)
|
|
pos = current_location(mav)
|
|
delta = get_distance(start, pos)
|
|
print("Distance %.2f meters" % delta)
|
|
if math.fabs(delta - distance) <= accuracy:
|
|
return True
|
|
print("Failed to attain distance %u" % distance)
|
|
return False
|
|
|
|
|
|
def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
|
|
'''wait for arrival at a location'''
|
|
tstart = time.time()
|
|
if target_altitude is None:
|
|
target_altitude = loc.alt
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='GPS_RAW', blocking=True)
|
|
pos = current_location(mav)
|
|
delta = get_distance(loc, pos)
|
|
print("Distance %.2f meters" % delta)
|
|
if delta <= accuracy:
|
|
if height_accuracy != -1 and math.fabs(pos.alt - target_altitude) > height_accuracy:
|
|
continue
|
|
print("Reached location (%.2f meters)" % delta)
|
|
return True
|
|
print("Failed to attain location")
|
|
return False
|
|
|
|
|
|
def fly_square(mavproxy, mav, side=50, timeout=120):
|
|
'''fly a square, flying N then E'''
|
|
mavproxy.send('switch 6\n')
|
|
mavproxy.expect('STABILIZE>')
|
|
tstart = time.time()
|
|
mavproxy.send('rc 3 1430\n')
|
|
mavproxy.send('rc 4 1610\n')
|
|
if not wait_heading(mav, 0):
|
|
return False
|
|
mavproxy.send('rc 4 1500\n')
|
|
|
|
print("Going north %u meters" % side)
|
|
mavproxy.send('rc 2 1390\n')
|
|
ok = wait_distance(mav, side)
|
|
mavproxy.send('rc 2 1500\n')
|
|
|
|
print("Going east %u meters" % side)
|
|
mavproxy.send('rc 1 1610\n')
|
|
ok = wait_distance(mav, side)
|
|
mavproxy.send('rc 1 1500\n')
|
|
|
|
print("Going south %u meters" % side)
|
|
mavproxy.send('rc 2 1610\n')
|
|
ok = wait_distance(mav, side)
|
|
mavproxy.send('rc 2 1500\n')
|
|
|
|
print("Going west %u meters" % side)
|
|
mavproxy.send('rc 1 1390\n')
|
|
ok = wait_distance(mav, side)
|
|
mavproxy.send('rc 1 1500\n')
|
|
return ok
|
|
|
|
|
|
|
|
|
|
def land(mavproxy, mav, timeout=60):
|
|
'''land the quad'''
|
|
print("STARTING LANDING")
|
|
mavproxy.send('switch 6\n')
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('status\n')
|
|
mavproxy.expect('>')
|
|
|
|
# start by dropping throttle till we have lost 5m
|
|
mavproxy.send('rc 3 1380\n')
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
wait_altitude(mav, 0, m.alt-5)
|
|
|
|
# now let it settle gently
|
|
mavproxy.send('rc 3 1400\n')
|
|
tstart = time.time()
|
|
if wait_altitude(mav, -5, 0):
|
|
print("LANDING OK")
|
|
return True
|
|
else:
|
|
print("LANDING FAILED")
|
|
return False
|
|
|
|
|
|
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
|
|
'''fly a mission from a file'''
|
|
global homeloc
|
|
mavproxy.send('wp load %s\n' % filename)
|
|
mavproxy.expect('flight plan received')
|
|
mavproxy.send('wp list\n')
|
|
mavproxy.expect('Requesting [0-9]+ waypoints')
|
|
mavproxy.send('switch 1\n') # auto mode
|
|
mavproxy.expect('AUTO>')
|
|
wait_distance(mav, 30, timeout=120)
|
|
wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy)
|
|
|
|
|
|
def setup_rc(mavproxy):
|
|
'''setup RC override control'''
|
|
for chan in range(1,9):
|
|
mavproxy.send('rc %u 1500\n' % chan)
|
|
# zero throttle
|
|
mavproxy.send('rc 3 1000\n')
|
|
|
|
|
|
def fly_ArduCopter():
|
|
'''fly ArduCopter in SIL'''
|
|
global expect_list, homeloc
|
|
|
|
sil = util.start_SIL('ArduCopter', wipe=True)
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
|
|
mavproxy.expect('Please Run Setup')
|
|
|
|
# we need to restart it after eeprom erase
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sil)
|
|
sil = util.start_SIL('ArduCopter')
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
# setup test parameters
|
|
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
|
|
mavproxy.expect('Loaded [0-9]+ parameters')
|
|
|
|
# reboot with new parameters
|
|
print("CLOSING")
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sil)
|
|
print("CLOSED THEM")
|
|
sil = util.start_SIL('ArduCopter')
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --out=192.168.2.15:14550 --quadcopter --streamrate=1')
|
|
mavproxy.expect('Logging to (\S+)')
|
|
logfile = mavproxy.match.group(1)
|
|
print("LOGFILE %s" % logfile)
|
|
|
|
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.mavlog")
|
|
print("buildlog=%s" % buildlog)
|
|
if os.path.exists(buildlog):
|
|
os.unlink(buildlog)
|
|
os.link(logfile, buildlog)
|
|
|
|
mavproxy.expect("Ready to FLY")
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
util.expect_setup_callback(mavproxy, expect_callback)
|
|
|
|
# start hil_quad.py
|
|
hquad = pexpect.spawn(util.reltopdir('../HILTest/hil_quad.py') + ' --fgout=192.168.2.15:9123 --fgrate=200 --home=%s' % HOME_LOCATION,
|
|
logfile=sys.stdout, timeout=10)
|
|
util.pexpect_autoclose(hquad)
|
|
hquad.expect('Starting at')
|
|
|
|
expect_list.extend([hquad, sil, mavproxy])
|
|
|
|
# get a mavlink connection going
|
|
try:
|
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
|
except Exception, msg:
|
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
|
raise
|
|
mav.message_hooks.append(message_hook)
|
|
|
|
failed = False
|
|
try:
|
|
mav.wait_heartbeat()
|
|
mav.recv_match(type='GPS_RAW', blocking=True)
|
|
setup_rc(mavproxy)
|
|
homeloc = current_location(mav)
|
|
arm_motors(mavproxy)
|
|
takeoff(mavproxy, mav)
|
|
fly_square(mavproxy, mav)
|
|
loiter(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, "mission1.txt"), height_accuracy = 0.5, target_altitude=10)
|
|
land(mavproxy, mav)
|
|
disarm_motors(mavproxy)
|
|
except pexpect.TIMEOUT, e:
|
|
failed = True
|
|
|
|
util.pexpect_close(mavproxy)
|
|
util.pexpect_close(sil)
|
|
util.pexpect_close(hquad)
|
|
|
|
if os.path.exists('ArduCopter-valgrind.log'):
|
|
os.chmod('ArduCopter-valgrind.log', 0644)
|
|
shutil.copy("ArduCopter-valgrind.log", util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
|
|
|
|
if failed:
|
|
print("FAILED: %s" % e)
|
|
return False
|
|
return True
|