mirror of https://github.com/ArduPilot/ardupilot
191 lines
6.7 KiB
Python
191 lines
6.7 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Fly ArduPlane QuadPlane in SITL
|
|
from __future__ import print_function
|
|
import os
|
|
import pexpect
|
|
from pymavlink import mavutil
|
|
|
|
from common import AutoTest
|
|
from pysim import util
|
|
|
|
# get location of scripts
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
HOME = mavutil.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
|
|
|
|
|
|
class AutoTestQuadPlane(AutoTest):
|
|
def __init__(self,
|
|
binary,
|
|
valgrind=False,
|
|
gdb=False,
|
|
speedup=10,
|
|
frame=None,
|
|
params=None,
|
|
gdbserver=False,
|
|
**kwargs):
|
|
super(AutoTestQuadPlane, self).__init__(**kwargs)
|
|
self.binary = binary
|
|
self.valgrind = valgrind
|
|
self.gdb = gdb
|
|
self.frame = frame
|
|
self.params = params
|
|
self.gdbserver = gdbserver
|
|
|
|
self.home = "%f,%f,%u,%u" % (HOME.lat,
|
|
HOME.lng,
|
|
HOME.alt,
|
|
HOME.heading)
|
|
self.homeloc = None
|
|
self.speedup = speedup
|
|
self.speedup_default = 10
|
|
|
|
self.log_name = "QuadPlane"
|
|
self.logfile = None
|
|
self.buildlog = None
|
|
self.copy_tlog = False
|
|
|
|
self.sitl = None
|
|
self.hasInit = False
|
|
|
|
def init(self):
|
|
if self.frame is None:
|
|
self.frame = 'quadplane'
|
|
|
|
defaults_file = os.path.join(testdir, 'default_params/quadplane.parm')
|
|
self.sitl = util.start_SITL(self.binary,
|
|
wipe=True,
|
|
model=self.frame,
|
|
home=self.home,
|
|
speedup=self.speedup,
|
|
defaults_file=defaults_file,
|
|
valgrind=self.valgrind,
|
|
gdb=self.gdb,
|
|
gdbserver=self.gdbserver)
|
|
self.mavproxy = util.start_MAVProxy_SITL(
|
|
'QuadPlane', options=self.mavproxy_options())
|
|
self.mavproxy.expect('Telemetry log: (\S+)\r\n')
|
|
logfile = self.mavproxy.match.group(1)
|
|
self.progress("LOGFILE %s" % logfile)
|
|
|
|
buildlog = self.buildlogs_path("QuadPlane-test.tlog")
|
|
self.progress("buildlog=%s" % buildlog)
|
|
if os.path.exists(buildlog):
|
|
os.unlink(buildlog)
|
|
try:
|
|
os.link(logfile, buildlog)
|
|
except Exception:
|
|
pass
|
|
|
|
self.mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
util.expect_setup_callback(self.mavproxy, self.expect_callback)
|
|
|
|
self.expect_list_clear()
|
|
self.expect_list_extend([self.sitl, self.mavproxy])
|
|
|
|
self.progress("Started simulator")
|
|
|
|
# get a mavlink connection going
|
|
connection_string = '127.0.0.1:19550'
|
|
try:
|
|
self.mav = mavutil.mavlink_connection(connection_string,
|
|
robust_parsing=True)
|
|
except Exception as msg:
|
|
self.progress("Failed to start mavlink connection on %s: %s" %
|
|
(connection_string, msg))
|
|
raise
|
|
self.mav.message_hooks.append(self.message_hook)
|
|
self.mav.idle_hooks.append(self.idle_hook)
|
|
self.hasInit = True
|
|
self.progress("Ready to start testing!")
|
|
|
|
# def test_arm_motors_radio(self):
|
|
# super(AutotestQuadPlane, self).test_arm_motors_radio()
|
|
#
|
|
# def test_disarm_motors_radio(self):
|
|
# super(AutotestQuadPlane, self).test_disarm_motors_radio()
|
|
#
|
|
# def test_autodisarm_motors(self):
|
|
# super(AutotestQuadPlane, self).test_autodisarm_motors()
|
|
#
|
|
# def test_rtl(self, home, distance_min=10, timeout=250):
|
|
# super(AutotestQuadPlane, self).test_rtl(home,
|
|
# distance_min=10, timeout=250)
|
|
#
|
|
# def test_throttle_failsafe(self, home, distance_min=10,
|
|
# side=60, timeout=180):
|
|
# super(AutotestQuadPlane, self).test_throttle_failsafe(home,
|
|
# distance_min=10, side=60, timeout=180)
|
|
#
|
|
# def test_mission(self, filename):
|
|
# super(AutotestQuadPlane, self).test_mission(filename)
|
|
|
|
def fly_mission(self, filename, fence, height_accuracy=-1):
|
|
"""Fly a mission from a file."""
|
|
self.progress("Flying mission %s" % filename)
|
|
self.mavproxy.send('wp load %s\n' % filename)
|
|
self.mavproxy.expect('Flight plan received')
|
|
self.mavproxy.send('fence load %s\n' % fence)
|
|
self.mavproxy.send('wp list\n')
|
|
self.mavproxy.expect('Requesting [0-9]+ waypoints')
|
|
self.mavproxy.send('mode AUTO\n')
|
|
self.wait_mode('AUTO')
|
|
if not self.wait_waypoint(1, 19, max_dist=60, timeout=1200):
|
|
return False
|
|
self.mavproxy.expect('DISARMED')
|
|
# wait for blood sample here
|
|
self.mavproxy.send('wp set 20\n')
|
|
self.arm_vehicle()
|
|
if not self.wait_waypoint(20, 34, max_dist=60, timeout=1200):
|
|
return False
|
|
self.mavproxy.expect('DISARMED')
|
|
self.progress("Mission OK")
|
|
return True
|
|
|
|
def autotest(self):
|
|
"""Autotest QuadPlane in SITL."""
|
|
self.frame = 'quadplane'
|
|
if not self.hasInit:
|
|
self.init()
|
|
|
|
failed = False
|
|
e = 'None'
|
|
try:
|
|
self.progress("Waiting for a heartbeat with mavlink protocol %s"
|
|
% self.mav.WIRE_PROTOCOL_VERSION)
|
|
self.mav.wait_heartbeat()
|
|
self.progress("Waiting for GPS fix")
|
|
self.mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
|
|
self.mav.wait_gps_fix()
|
|
while self.mav.location().alt < 10:
|
|
self.mav.wait_gps_fix()
|
|
self.homeloc = self.mav.location()
|
|
self.progress("Home location: %s" % self.homeloc)
|
|
|
|
# wait for EKF and GPS checks to pass
|
|
self.progress("Waiting reading for arm")
|
|
self.wait_seconds(30)
|
|
|
|
self.arm_vehicle()
|
|
|
|
m = os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt")
|
|
f = os.path.join(testdir,
|
|
"ArduPlane-Missions/Dalby-OBC2016-fence.txt")
|
|
if not self.fly_mission(m, f):
|
|
self.progress("Failed mission")
|
|
failed = True
|
|
except pexpect.TIMEOUT as e:
|
|
self.progress("Failed with timeout")
|
|
failed = True
|
|
|
|
self.close()
|
|
|
|
if failed:
|
|
self.progress("FAILED: %s" % e)
|
|
return False
|
|
return True
|