mirror of https://github.com/ArduPilot/ardupilot
autotest: Add Sub
This commit is contained in:
Jacob Walser
Andrew Tridgell
parent
289aab06bc
commit
ec15f0fee5
|
|
@ -0,0 +1,180 @@
|
|||
# drive APMrover2 in SITL
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import shutil
|
||||
|
||||
import pexpect
|
||||
from pymavlink import mavutil
|
||||
|
||||
from common import *
|
||||
from pysim import util
|
||||
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
|
||||
HOME = mavutil.location(33.810313, -118.393867, 0, 185)
|
||||
homeloc = None
|
||||
|
||||
|
||||
def wait_ready_to_arm(mavproxy):
|
||||
# wait for EKF and GPS checks to pass
|
||||
mavproxy.expect('IMU0 is using GPS')
|
||||
|
||||
def arm_sub(mavproxy, mav):
|
||||
wait_ready_to_arm(mavproxy);
|
||||
for i in range(8):
|
||||
mavproxy.send('rc %d 1500\n' % (i+1))
|
||||
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
|
||||
print("SUB ARMED")
|
||||
return True
|
||||
|
||||
def dive_mission(mavproxy, mav, filename):
|
||||
"""Dive a mission from a file."""
|
||||
global homeloc
|
||||
|
||||
mavproxy.send('rc 3 1600\n')
|
||||
mavproxy.send('rc 6 1600\n')
|
||||
mavproxy.send('rc 7 1550\n')
|
||||
|
||||
if not wait_distance(mav, 50, accuracy=7, timeout=100):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1550\n')
|
||||
|
||||
if not wait_heading(mav, 0):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
|
||||
if not wait_distance(mav, 50, accuracy=7, timeout=100):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1550\n')
|
||||
|
||||
if not wait_heading(mav, 0):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
mavproxy.send('rc 6 1500\n')
|
||||
mavproxy.send('rc 7 1100\n')
|
||||
|
||||
if not wait_distance(mav, 75, accuracy=7, timeout=100):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 7 1500\n')
|
||||
|
||||
wait_mode(mav, 'MANUAL')
|
||||
print("Mission OK")
|
||||
return True
|
||||
|
||||
|
||||
def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||
"""Dive ArduSub in SITL.
|
||||
|
||||
you can pass viewerip as an IP address to optionally send fg and
|
||||
mavproxy packets too for local viewing of the mission 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'
|
||||
|
||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=10)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
# setup test parameters
|
||||
mavproxy.send("param load %s/default_params/sub.parm\n" % testdir)
|
||||
mavproxy.expect('Loaded [0-9]+ parameters')
|
||||
mavproxy.send('param set FS_GCS_ENABLE 0\n')
|
||||
mavproxy.send("param set LOG_REPLAY 1\n")
|
||||
mavproxy.send("param set LOG_DISARMED 1\n")
|
||||
time.sleep(3)
|
||||
|
||||
# reboot with new parameters
|
||||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sitl)
|
||||
|
||||
sitl = util.start_SITL(binary, model='vectored', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
|
||||
mavproxy.expect('Telemetry log: (\S+)')
|
||||
logfile = mavproxy.match.group(1)
|
||||
print("LOGFILE %s" % logfile)
|
||||
|
||||
buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog")
|
||||
print("buildlog=%s" % buildlog)
|
||||
if os.path.exists(buildlog):
|
||||
os.unlink(buildlog)
|
||||
try:
|
||||
os.link(logfile, buildlog)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
util.expect_setup_callback(mavproxy, expect_callback)
|
||||
|
||||
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.wait_gps_fix()
|
||||
|
||||
homeloc = mav.location()
|
||||
print("Home location: %s" % homeloc)
|
||||
if not arm_sub(mavproxy, mav):
|
||||
print("Failed to ARM")
|
||||
failed = True
|
||||
if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub1.txt")):
|
||||
print("Failed mission")
|
||||
failed = True
|
||||
|
||||
mavproxy.send('disarm\n');
|
||||
|
||||
# wait for disarm
|
||||
mav.motors_disarmed_wait()
|
||||
|
||||
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")):
|
||||
print("Failed log download")
|
||||
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 = util.valgrind_log_filepath(binary=binary, model='sub')
|
||||
if os.path.exists(valgrind_log):
|
||||
os.chmod(valgrind_log, 0o644)
|
||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log"))
|
||||
|
||||
if failed:
|
||||
print("FAILED: %s" % e)
|
||||
return False
|
||||
return True
|
||||
|
|
@ -19,6 +19,7 @@ import apmrover2
|
|||
import arducopter
|
||||
import arduplane
|
||||
import quadplane
|
||||
import ardusub
|
||||
from pysim import util
|
||||
|
||||
os.environ['PYTHONUNBUFFERED'] = '1'
|
||||
|
|
@ -196,6 +197,10 @@ steps = [
|
|||
'fly.CopterAVC',
|
||||
|
||||
'build.AntennaTracker',
|
||||
|
||||
'build.ArduSub',
|
||||
'defaults.ArduSub',
|
||||
'dive.ArduSub',
|
||||
|
||||
'convertgpx',
|
||||
]
|
||||
|
|
@ -233,6 +238,8 @@ def binary_path(step, debug=False):
|
|||
binary_name = "arducopter-heli"
|
||||
elif step.find("QuadPlane") != -1:
|
||||
binary_name = "arduplane"
|
||||
elif step.find("ArduSub") != -1:
|
||||
binary_name = "ardusub"
|
||||
else:
|
||||
# cope with builds that don't have a specific binary
|
||||
return None
|
||||
|
|
@ -275,6 +282,9 @@ def run_step(step):
|
|||
|
||||
if step == 'build.Helicopter':
|
||||
return util.build_SITL('bin/arducopter-heli', j=opts.j, debug=opts.debug)
|
||||
|
||||
if step == 'build.ArduSub':
|
||||
return util.build_SITL('bin/ardusub', j=opts.j, debug=opts.debug)
|
||||
|
||||
binary = binary_path(step, debug=opts.debug)
|
||||
|
||||
|
|
@ -286,6 +296,9 @@ def run_step(step):
|
|||
|
||||
if step == 'defaults.APMrover2':
|
||||
return get_default_params('APMrover2', binary)
|
||||
|
||||
if step == 'defaults.ArduSub':
|
||||
return get_default_params('ArduSub', binary)
|
||||
|
||||
if step == 'fly.ArduCopter':
|
||||
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||
|
|
@ -301,6 +314,9 @@ def run_step(step):
|
|||
|
||||
if step == 'drive.APMrover2':
|
||||
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||
|
||||
if step == 'dive.ArduSub':
|
||||
return ardusub.dive_ArduSub(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||
|
||||
if step == 'build.All':
|
||||
return build_all()
|
||||
|
|
@ -420,10 +436,18 @@ def write_fullresults():
|
|||
results.addfile('AntennaTracker code size', 'AntennaTracker.sizes.txt')
|
||||
results.addfile('AntennaTracker stack sizes', 'AntennaTracker.framesizes.txt')
|
||||
results.addglob("AntennaTracker ELF", 'AntennaTracker.elf')
|
||||
results.addfile('ArduSub build log', 'ArduSub.txt')
|
||||
results.addfile('ArduSub code size', 'ArduSub.sizes.txt')
|
||||
results.addfile('ArduSub stack sizes', 'ArduSub.framesizes.txt')
|
||||
results.addfile('ArduSub defaults', 'default_params/ArduSub-defaults.parm')
|
||||
results.addglob("ArduSub log", 'ArduSub-*.BIN')
|
||||
results.addglob("ArduSub core", 'ArduSub.core')
|
||||
results.addglob("ArduSub ELF", 'ArduSub.elf')
|
||||
results.addglob('APM:Libraries documentation', 'docs/libraries/index.html')
|
||||
results.addglob('APM:Plane documentation', 'docs/ArduPlane/index.html')
|
||||
results.addglob('APM:Copter documentation', 'docs/ArduCopter/index.html')
|
||||
results.addglob('APM:Rover documentation', 'docs/APMrover2/index.html')
|
||||
results.addglob('APM:Sub documentation', 'docs/ArduSub/index.html')
|
||||
results.addglobimage("Flight Track", '*.png')
|
||||
|
||||
write_webresults(results)
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@ set -x
|
|||
git submodule init
|
||||
git submodule update
|
||||
|
||||
for d in ArduPlane ArduCopter APMrover2; do
|
||||
for d in ArduPlane ArduCopter APMrover2 ArduSub; do
|
||||
pushd $d
|
||||
make px4-clean
|
||||
popd
|
||||
|
|
@ -32,4 +32,9 @@ pushd APMrover2
|
|||
make px4
|
||||
popd
|
||||
|
||||
echo "Testing ArduSub build"
|
||||
pushd ArduSub
|
||||
make px4
|
||||
popd
|
||||
|
||||
exit 0
|
||||
|
|
|
|||
Loading…
Reference in New Issue