ardupilot/Tools/autotest/quadplane.py

136 lines
4.2 KiB
Python

# fly ArduPlane QuadPlane in SITL
import os
import pexpect
import shutil
from pymavlink import mavutil
from common import *
from pysim import util
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME_LOCATION = '-27.274439,151.290064,343,8.7'
MISSION = 'ArduPlane-Missions/Dalby-OBC2016.txt'
FENCE = 'ArduPlane-Missions/Dalby-OBC2016-fence.txt'
WIND = "0,180,0.2" # speed,direction,variance
homeloc = None
def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
"""Fly a mission from a file."""
print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received')
mavproxy.send('fence load %s\n' % fence)
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
mavproxy.send('mode AUTO\n')
wait_mode(mav, 'AUTO')
if not wait_waypoint(mav, 1, 19, max_dist=60, timeout=1200):
return False
mavproxy.expect('DISARMED')
# wait for blood sample here
mavproxy.send('wp set 20\n')
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')
if not wait_waypoint(mav, 20, 34, max_dist=60, timeout=1200):
return False
mavproxy.expect('DISARMED')
print("Mission OK")
return True
def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
"""Fly QuadPlane in SITL.
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time.
"""
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if use_map:
options += ' --map'
sitl = util.start_SITL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/QuadPlane-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
util.expect_setup_callback(mavproxy, expect_callback)
mavproxy.expect('Received [0-9]+ parameters')
expect_list_clear()
expect_list_extend([sitl, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
e = 'None'
try:
print("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
mav.wait_heartbeat()
print("Waiting for GPS fix")
mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
mav.wait_gps_fix()
while mav.location().alt < 10:
mav.wait_gps_fix()
homeloc = mav.location()
print("Home location: %s" % homeloc)
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')
if not fly_mission(mavproxy, mav,
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"),
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")):
print("Failed mission")
failed = True
except pexpect.TIMEOUT as e:
print("Failed with timeout")
failed = True
mav.close()
util.pexpect_close(mavproxy)
util.pexpect_close(sitl)
valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log"))
if failed:
print("FAILED: %s" % e)
return False
return True