2017-02-21 13:32:26 -04:00
|
|
|
#!/usr/bin/env python
|
|
|
|
|
|
|
|
# Dive ArduSub in SITL
|
2017-02-24 19:22:38 -04:00
|
|
|
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
|
|
|
|
|
|
|
|
|
2017-03-09 18:30:35 -04:00
|
|
|
def dive_manual(mavproxy, mav):
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc(mavproxy, mav, 3, 1600)
|
|
|
|
set_rc(mavproxy, mav, 5, 1600)
|
|
|
|
set_rc(mavproxy, mav, 6, 1550)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
2017-07-20 02:45:15 -03:00
|
|
|
if not wait_distance(mav, 50, accuracy=7, timeout=200):
|
2017-02-24 19:22:38 -04:00
|
|
|
return False
|
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc(mavproxy, mav, 4, 1550)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
if not wait_heading(mav, 0):
|
|
|
|
return False
|
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc(mavproxy, mav, 4, 1500)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
if not wait_distance(mav, 50, accuracy=7, timeout=100):
|
|
|
|
return False
|
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc(mavproxy, mav, 4, 1550)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
if not wait_heading(mav, 0):
|
|
|
|
return False
|
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc(mavproxy, mav, 4, 1500)
|
|
|
|
set_rc(mavproxy, mav, 5, 1500)
|
|
|
|
set_rc(mavproxy, mav, 6, 1100)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
if not wait_distance(mav, 75, accuracy=7, timeout=100):
|
|
|
|
return False
|
2017-03-09 18:30:35 -04:00
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc_default(mavproxy)
|
|
|
|
|
|
|
|
disarm_vehicle(mavproxy, mav)
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Manual dive OK")
|
2017-02-24 19:22:38 -04:00
|
|
|
return True
|
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
|
2017-03-09 18:30:35 -04:00
|
|
|
def dive_mission(mavproxy, mav, filename):
|
|
|
|
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Executing mission %s" % filename)
|
2017-03-09 18:30:35 -04:00
|
|
|
mavproxy.send('wp load %s\n' % filename)
|
|
|
|
mavproxy.expect('Flight plan received')
|
|
|
|
mavproxy.send('wp list\n')
|
|
|
|
mavproxy.expect('Saved [0-9]+ waypoints')
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc_default(mavproxy)
|
2017-03-09 18:30:35 -04:00
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
if not arm_vehicle(mavproxy, mav):
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed to ARM")
|
2017-03-09 18:30:35 -04:00
|
|
|
return False
|
|
|
|
|
|
|
|
mavproxy.send('mode auto\n')
|
|
|
|
wait_mode(mav, 'AUTO')
|
|
|
|
|
2017-04-07 02:02:13 -03:00
|
|
|
if not wait_waypoint(mav, 1, 5, max_dist=5):
|
2017-03-09 18:30:35 -04:00
|
|
|
return False
|
2017-08-16 10:55:21 -03:00
|
|
|
|
|
|
|
disarm_vehicle(mavproxy, mav)
|
2017-03-09 18:30:35 -04:00
|
|
|
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Mission OK")
|
2017-03-09 18:30:35 -04:00
|
|
|
return True
|
2017-02-24 19:22:38 -04:00
|
|
|
|
2017-08-16 10:55:21 -03:00
|
|
|
|
2017-08-26 01:59:36 -03:00
|
|
|
def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10):
|
2017-02-24 19:22:38 -04:00
|
|
|
"""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
|
|
|
|
"""
|
|
|
|
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)
|
2017-08-26 01:59:36 -03:00
|
|
|
sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=speedup)
|
2017-08-16 10:55:21 -03:00
|
|
|
mavproxy = util.start_MAVProxy_SITL('ArduSub')
|
2017-02-24 19:22:38 -04:00
|
|
|
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)
|
|
|
|
|
2017-08-26 01:59:36 -03:00
|
|
|
sitl = util.start_SITL(binary, model='vectored', home=home, speedup=speedup, valgrind=valgrind, gdb=gdb, gdbserver=gdbserver)
|
2017-02-24 19:22:38 -04:00
|
|
|
mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
|
2017-07-20 00:57:24 -03:00
|
|
|
mavproxy.expect('Telemetry log: (\S+)\r\n')
|
2017-02-24 19:22:38 -04:00
|
|
|
logfile = mavproxy.match.group(1)
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("LOGFILE %s" % logfile)
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
buildlog = util.reltopdir("../buildlogs/ArduSub-test.tlog")
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("buildlog=%s" % buildlog)
|
2017-02-24 19:22:38 -04:00
|
|
|
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])
|
|
|
|
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Started simulator")
|
2017-02-24 19:22:38 -04:00
|
|
|
|
|
|
|
# get a mavlink connection going
|
|
|
|
try:
|
|
|
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
|
|
|
except Exception as msg:
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
2017-02-24 19:22:38 -04:00
|
|
|
raise
|
|
|
|
mav.message_hooks.append(message_hook)
|
|
|
|
mav.idle_hooks.append(idle_hook)
|
|
|
|
|
|
|
|
failed = False
|
|
|
|
e = 'None'
|
|
|
|
try:
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
|
2017-02-24 19:22:38 -04:00
|
|
|
mav.wait_heartbeat()
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Waiting for GPS fix")
|
2017-02-24 19:22:38 -04:00
|
|
|
mav.wait_gps_fix()
|
2017-03-09 18:30:35 -04:00
|
|
|
|
|
|
|
# wait for EKF and GPS checks to pass
|
|
|
|
mavproxy.expect('IMU0 is using GPS')
|
|
|
|
|
2017-02-24 19:22:38 -04:00
|
|
|
homeloc = mav.location()
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Home location: %s" % homeloc)
|
2017-08-16 10:55:21 -03:00
|
|
|
set_rc_default(mavproxy)
|
|
|
|
if not arm_vehicle(mavproxy, mav):
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed to ARM")
|
2017-02-24 19:22:38 -04:00
|
|
|
failed = True
|
2017-03-09 18:30:35 -04:00
|
|
|
if not dive_manual(mavproxy, mav):
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed manual dive")
|
2017-03-09 18:30:35 -04:00
|
|
|
failed = True
|
|
|
|
if not dive_mission(mavproxy, mav, os.path.join(testdir, "sub_mission.txt")):
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed auto mission")
|
2017-02-24 19:22:38 -04:00
|
|
|
failed = True
|
|
|
|
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/ArduSub-log.bin")):
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed log download")
|
2017-02-24 19:22:38 -04:00
|
|
|
failed = True
|
|
|
|
except pexpect.TIMEOUT as e:
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("Failed with timeout")
|
2017-02-24 19:22:38 -04:00
|
|
|
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:
|
2017-11-14 00:30:31 -04:00
|
|
|
progress("FAILED: %s" % e)
|
2017-02-24 19:22:38 -04:00
|
|
|
return False
|
|
|
|
return True
|