mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
40cd0cd17f
If any of the tests throws an exception, it will be caught by the generic test function that collects all errors and displays them before passing the result to the vehicle testcode. Hence the name of the test and the exception that got raised get printed in a single location. This nicely reduces code duplication.
184 lines
5.6 KiB
Python
184 lines
5.6 KiB
Python
#!/usr/bin/env python
|
|
|
|
# Dive ArduSub in SITL
|
|
from __future__ import print_function
|
|
import os
|
|
|
|
import pexpect
|
|
from pymavlink import mavutil
|
|
|
|
from pysim import util
|
|
|
|
from common import AutoTest
|
|
|
|
# get location of scripts
|
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
|
HOME = mavutil.location(33.810313, -118.393867, 0, 185)
|
|
|
|
|
|
class AutoTestSub(AutoTest):
|
|
def __init__(self,
|
|
binary,
|
|
valgrind=False,
|
|
gdb=False,
|
|
speedup=10,
|
|
frame=None,
|
|
params=None,
|
|
gdbserver=False,
|
|
**kwargs):
|
|
super(AutoTestSub, 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.sitl = None
|
|
self.hasInit = False
|
|
|
|
self.log_name = "ArduSub"
|
|
|
|
def init(self):
|
|
if self.frame is None:
|
|
self.frame = 'vectored'
|
|
|
|
self.apply_parameters_using_sitl()
|
|
|
|
self.sitl = util.start_SITL(self.binary,
|
|
model=self.frame,
|
|
home=self.home,
|
|
speedup=self.speedup,
|
|
valgrind=self.valgrind,
|
|
gdb=self.gdb,
|
|
gdbserver=self.gdbserver)
|
|
self.mavproxy = util.start_MAVProxy_SITL(
|
|
'ArduSub', 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("ArduSub-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 dive_manual(self):
|
|
self.set_rc(3, 1600)
|
|
self.set_rc(5, 1600)
|
|
self.set_rc(6, 1550)
|
|
|
|
self.wait_distance(50, accuracy=7, timeout=200)
|
|
self.set_rc(4, 1550)
|
|
|
|
self.wait_heading(0)
|
|
self.set_rc(4, 1500)
|
|
|
|
self.wait_distance(50, accuracy=7, timeout=100)
|
|
self.set_rc(4, 1550)
|
|
|
|
self.wait_heading(0)
|
|
self.set_rc(4, 1500)
|
|
self.set_rc(5, 1500)
|
|
self.set_rc(6, 1100)
|
|
|
|
self.wait_distance(75, accuracy=7, timeout=100)
|
|
self.set_rc_default()
|
|
|
|
self.disarm_vehicle()
|
|
self.progress("Manual dive OK")
|
|
|
|
def dive_mission(self, filename):
|
|
self.progress("Executing mission %s" % filename)
|
|
self.mavproxy.send('wp load %s\n' % filename)
|
|
self.mavproxy.expect('Flight plan received')
|
|
self.mavproxy.send('wp list\n')
|
|
self.mavproxy.expect('Saved [0-9]+ waypoints')
|
|
self.set_rc_default()
|
|
|
|
self.arm_vehicle()
|
|
|
|
self.mavproxy.send('mode auto\n')
|
|
self.wait_mode('AUTO')
|
|
|
|
self.wait_waypoint(1, 5, max_dist=5)
|
|
|
|
self.disarm_vehicle()
|
|
|
|
self.progress("Mission OK")
|
|
|
|
def autotest(self):
|
|
"""Autotest ArduSub in SITL."""
|
|
if not self.hasInit:
|
|
self.init()
|
|
|
|
self.fail_list = []
|
|
try:
|
|
self.progress("Waiting for a heartbeat with mavlink protocol %s"
|
|
% self.mav.WIRE_PROTOCOL_VERSION)
|
|
self.mav.wait_heartbeat()
|
|
self.mavproxy.send('param set FS_GCS_ENABLE 0\n')
|
|
self.progress("Waiting for GPS fix")
|
|
self.mav.wait_gps_fix()
|
|
|
|
# wait for EKF and GPS checks to pass
|
|
self.mavproxy.expect('IMU0 is using GPS')
|
|
|
|
self.homeloc = self.mav.location()
|
|
self.progress("Home location: %s" % self.homeloc)
|
|
self.set_rc_default()
|
|
|
|
self.run_test("Arm vehicle", self.arm_vehicle)
|
|
|
|
self.run_test("Dive manual", self.dive_manual)
|
|
|
|
self.run_test("Dive mission", lambda: self.dive_mission(os.path.join(testdir, "sub_mission.txt")))
|
|
|
|
self.run_test("Log download", lambda: self.log_download(self.buildlogs_path("ArduSub-log.bin")))
|
|
|
|
except pexpect.TIMEOUT as e:
|
|
self.progress("Failed with timeout")
|
|
self.fail_list.append("Failed with timeout")
|
|
|
|
self.close()
|
|
|
|
if len(self.fail_list):
|
|
self.progress("FAILED: %s" % self.fail_list)
|
|
return False
|
|
return True
|