def drive_APMrover2(viewerip=None):
'''drive APMrover2 in SIL
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
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/Rover.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Starting at lat')
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def drive_APMrover2(viewerip=None):
'''drive APMrover2 in SIL
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
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/Rover.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Starting at lat')
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduCopter_scripted(testname):
'''fly ArduCopter in SIL
'''
global homeloc
sim_cmd = util.reltopdir(
'Tools/autotest/pysim/sim_multicopter.py'
) + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL(
'ArduCopter',
options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, arducopter.expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduCopter_scripted(testname):
'''fly ArduCopter in SIL
'''
global homeloc
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, arducopter.expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def run_mission(mission_path, frame, home, viewerip=None):
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py')
sim_cmd += ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
frame, home.lat, home.lng, home.alt, home.heading)
sim_cmd += ' --wind=6,45,.3'
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL(
'ArduCopter',
options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/autotest/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
mavproxy.send('module load mmap\n')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=home.alt)
print 'Executing command %s' % (sim_cmd,)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = ('--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter '
'--streamrate=5')
if viewerip:
options += ' --out=%s:14550' % viewerip
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print 'Saving log %s' % (logfile,)
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.send('module load mmap\n')
util.expect_setup_callback(mavproxy, common.expect_callback)
common.expect_list_clear()
common.expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
error("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def init_JSBSim(self):
cmd = "JSBSim --realtime --suspend --simulation-rate=400 --script=%s --logdirectivefile=data/fgout.xml" % self.script
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
# time.sleep(10)
print 'Waiting for JSBSim'
jsb.expect("JSBSim startup beginning")
i = jsb.expect([
"Creating input TCP socket on port (\d+)",
"Could not bind to socket for input"
])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb.expect("Creating UDP socket on port (\d+)")
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
jsb_out_address = ('127.0.0.1', 5124)
print("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
# jsb_out.bind(jsb_out_address)
# jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = open('/home/mvacanti/logf.txt', 'wt')
jsb_console.expect('Connected to JSBSim server')
# setup input from jsbsim
jsb_in_address = ('127.0.0.1', 5123)
print("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def init_jsbsim(self):
cmd = "JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=data/flightgear.xml --script=%s" % self.script
if self.options:
cmd += ' %s' % self.options
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect([
"Successfully bound to socket for input on port (\d+)",
"Could not bind to socket for input"
])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = self.interpret_address("127.0.0.1:%u" %
int(jsb.match.group(1)))
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = self.interpret_address("127.0.0.1:%u" %
int(jsb.match.group(1)))
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = None
# setup input from jsbsim
print("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def init_jsbsim(self):
cmd = (
"JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=data/flightgear.xml --script=%s"
% self.script
)
if self.options:
cmd += " %s" % self.options
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect(["Successfully bound to socket for input on port (\d+)", "Could not bind to socket for input"])
if i == 1:
print ("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = self.interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = self.interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print ("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
jsb_console = fdpexpect.fdspawn(jsb_out.fileno(), logfile=sys.stdout)
jsb_console.delaybeforesend = 0
jsb_console.logfile = None
# setup input from jsbsim
print ("JSBSim FG FDM input on %s" % str(jsb_in_address))
jsb_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
jsb_in.bind(jsb_in_address)
jsb_in.setblocking(0)
# set class data
self.jsb = jsb
self.jsb_in = jsb_in
self.jsb_out = jsb_out
self.jsb_console = jsb_console
self.fdm = fgFDM.fgFDM()
def fly_ArduCopter(viewerip=None, map=False):
"""fly ArduCopter in SIL
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
if TARGET != "sitl":
util.build_SIL("ArduCopter", target=TARGET)
sim_cmd = util.reltopdir(
"Tools/autotest/pysim/sim_multicopter.py"
) + " --frame=%s --rate=400 --home=%f,%f,%u,%u" % (FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += " --wind=6,45,.3"
if viewerip:
sim_cmd += " --fgout=%s:5503" % viewerip
sil = util.start_SIL("ArduCopter", wipe=True)
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options="--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter")
mavproxy.expect("Received [0-9]+ parameters")
# setup test parameters
mavproxy.send("param set SYSID_THISMAV %u\n" % random.randint(100, 200))
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
mavproxy.expect("Loaded [0-9]+ parameters")
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL("ArduCopter", height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = "--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5"
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
options += " --map"
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options=options)
mavproxy.expect("Logging to (\S+)")
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# the received parameters can come before or after the ready to fly message
mavproxy.expect(["Received [0-9]+ parameters", "Ready to FLY"])
mavproxy.expect(["Received [0-9]+ parameters", "Ready to FLY"])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection("127.0.0.1:19550", robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def setup_class(cls):
#Set this to False to enable logging.
DEBUG = False
CONSOLE = False
cls.TIMEOUT = 5
cls.DELAY = 2
cls.resource_path = os.path.join(
os.path.join(os.path.dirname(os.path.abspath(__file__)),
'TestGroupLib/'), 'Resources/')
#start MAVLink, etc
try:
os.remove('eeprom.bin')
except OSError:
pass
try:
os.remove('jsb_pipe')
os.remove('mavproxy_pipe')
except Exception:
pass
util.run_cmd('mkfifo mavproxy_pipe')
util.run_cmd('mkfifo jsb_pipe')
if DEBUG:
util.run_cmd('xterm -e tail -f jsb_pipe &')
util.run_cmd('xterm -e tail -f mavproxy_pipe &')
util.run_cmd('echo "INITIALIZED MAVPROXY PIPE" > mavproxy_pipe &')
util.run_cmd('echo "INITIALIZED JSBSIM PIPE" > jsb_pipe &')
cls.homeloc = None
HOME_LOCATION = '35.7168007,-120.7644466,275,295' #McMillan Lat/Lon/Alt/Heading
WIND = "0,180,0.0" # speed,direction,variance
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=100 '
#Start SITL
cls.sil = util.start_SIL('ArduPlane', wipe=True)
cls.mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
cls.mavproxy.expect('Received [0-9]+ parameters')
#Load SITL params
cls.mavproxy.send("param load %ssitl.parm\n" % cls.resource_path)
cls.mavproxy.expect('Loaded [0-9]+ parameters')
#Fetch list of params
cls.mavproxy.send("param fetch\n")
#TODO: Optimize this. Is it necessary to start and close it?
util.pexpect_close(cls.mavproxy)
util.pexpect_close(cls.sil)
#Start JSBSim simulation
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
cmd += " --home=%s --wind=%s" % (HOME_LOCATION, WIND)
#cls.runsim = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
cls.runsim = pexpect.spawn(cmd,
logfile=file('jsb_pipe', 'w+', 0),
timeout=10)
cls.runsim.delaybeforesend = 0
util.pexpect_autoclose(cls.runsim)
cls.runsim.expect('Simulator ready to fly.')
cls.sil = util.start_SIL('ArduPlane', valgrind=True)
options += ' --map'
if CONSOLE:
options += ' --console'
cls.mavproxy = util.start_MAVProxy_SIL('ArduPlane',
logfile=file(
'mavproxy_pipe', 'w+', 0),
options=options)
#TODO: Make these easier to find
cls.mavproxy.expect('Logging to (\S+)')
logfile = cls.mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
#TODO: Make these easier to find
buildlog = util.reltopdir("../buildlogs/ArduPlane-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
cls.mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(cls.mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([cls.runsim, cls.sil, cls.mavproxy])
print("Started simulator.")
#Get a mavlink connection started.
try:
cls.mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" %
msg)
raise
def fly_ArduCopter(viewerip=None, map=False):
'''fly ArduCopter in SIL
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
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/copter_params.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
copyTLog = False
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
print( "WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location" )
copyTLog = True
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def setup_class(cls):
#Set this to False to enable logging.
DEBUG = False
CONSOLE = False
cls.TIMEOUT=5
cls.DELAY=2
cls.resource_path = os.path.join(os.path.join(os.path.dirname(os.path.abspath(__file__)), 'TestGroupLib/'), 'Resources/')
#start MAVLink, etc
try:
os.remove('eeprom.bin')
except OSError:
pass
try:
os.remove('jsb_pipe')
os.remove('mavproxy_pipe')
except Exception:
pass
util.run_cmd('mkfifo mavproxy_pipe')
util.run_cmd('mkfifo jsb_pipe')
if DEBUG:
util.run_cmd('xterm -e tail -f jsb_pipe &')
util.run_cmd('xterm -e tail -f mavproxy_pipe &')
util.run_cmd('echo "INITIALIZED MAVPROXY PIPE" > mavproxy_pipe &')
util.run_cmd('echo "INITIALIZED JSBSIM PIPE" > jsb_pipe &')
cls.homeloc = None
HOME_LOCATION='35.7168007,-120.7644466,275,295' #McMillan Lat/Lon/Alt/Heading
WIND="0,180,0.0" # speed,direction,variance
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=100 '
#Start SITL
cls.sil = util.start_SIL('ArduPlane', wipe=True)
cls.mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
cls.mavproxy.expect('Received [0-9]+ parameters')
#Load SITL params
cls.mavproxy.send("param load %ssitl.parm\n" % cls.resource_path)
cls.mavproxy.expect('Loaded [0-9]+ parameters')
#Fetch list of params
cls.mavproxy.send("param fetch\n")
#TODO: Optimize this. Is it necessary to start and close it?
util.pexpect_close(cls.mavproxy)
util.pexpect_close(cls.sil)
#Start JSBSim simulation
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
cmd += " --home=%s --wind=%s" % (HOME_LOCATION, WIND)
#cls.runsim = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
cls.runsim = pexpect.spawn(cmd, logfile=file('jsb_pipe', 'w+', 0), timeout=10)
cls.runsim.delaybeforesend = 0
util.pexpect_autoclose(cls.runsim)
cls.runsim.expect('Simulator ready to fly.')
cls.sil = util.start_SIL('ArduPlane', valgrind=True)
options += ' --map'
if CONSOLE:
options += ' --console'
cls.mavproxy = util.start_MAVProxy_SIL('ArduPlane',
logfile=file('mavproxy_pipe', 'w+', 0), options=options)
#TODO: Make these easier to find
cls.mavproxy.expect('Logging to (\S+)')
logfile = cls.mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
#TODO: Make these easier to find
buildlog = util.reltopdir("../buildlogs/ArduPlane-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
cls.mavproxy.expect('Received [0-9]+ parameters')
util.expect_setup_callback(cls.mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([cls.runsim, cls.sil, cls.mavproxy])
print("Started simulator.")
#Get a mavlink connection started.
try:
cls.mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduPlane(viewerip=None):
"""fly ArduPlane in SIL
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 expect_list, homeloc
options = "--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5"
if viewerip:
options += " --out=%s:14550" % viewerip
sil = util.start_SIL("ArduPlane", wipe=True)
mavproxy = util.start_MAVProxy_SIL("ArduPlane", options=options)
mavproxy.expect("Received [0-9]+ parameters")
# setup test parameters
mavproxy.send("param load %s/ArduPlane.parm\n" % testdir)
mavproxy.expect("Loaded [0-9]+ parameters")
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL("ArduPlane")
mavproxy = util.start_MAVProxy_SIL("ArduPlane", options=options)
mavproxy.expect("Logging to (\S+)")
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduPlane-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect("Received [0-9]+ parameters")
util.expect_setup_callback(mavproxy, expect_callback)
fg_scenery = os.getenv("FG_SCENERY")
if not fg_scenery:
raise RuntimeError("You must set the FG_SCENERY environment variable")
fgear_options = (
"""
--generic=socket,in,25,,5502,udp,MAVLink \
--generic=socket,out,50,,5501,udp,MAVLink \
--aircraft=Rascal110-JSBSim \
--control=mouse \
--disable-intro-music \
--airport=YKRY \
--lat=-35.362851 \
--lon=149.165223 \
--heading=350 \
--altitude=0 \
--geometry=650x550 \
--jpg-httpd=5502 \
--disable-anti-alias-hud \
--disable-hud-3d \
--disable-enhanced-lighting \
--disable-distance-attenuation \
--disable-horizon-effect \
--shading-flat \
--disable-textures \
--timeofday=noon \
--fdm=jsb \
--disable-sound \
--disable-fullscreen \
--disable-random-objects \
--disable-ai-models \
--shading-flat \
--fog-disable \
--disable-specular-highlight \
--disable-skyblend \
--fg-scenery=%s \
--disable-anti-alias-hud \
--wind=0@0 \
"""
% fg_scenery
)
# start fgear
if os.getenv("DISPLAY"):
cmd = "fgfs %s" % fgear_options
fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
else:
cmd = "xvfb-run --server-num=42 -s '-screen 0 800x600x24' fgfs --enable-wireframe %s" % fgear_options
util.kill_xvfb(42)
fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
fgear.xvfb_server_num = 42
util.pexpect_autoclose(fgear)
fgear.expect("creating 3D noise", timeout=30)
expect_list.extend([fgear, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection("127.0.0.1:19550", robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_CopterAVC(viewerip=None, map=False):
"""fly ArduCopter in SIL for AVC2013 mission
"""
global homeloc
if TARGET != "sitl":
util.build_SIL("ArduCopter", target=TARGET)
sim_cmd = util.reltopdir("Tools/autotest/pysim/sim_multicopter.py") + " --frame=%s --home=%f,%f,%u,%u" % (
FRAME,
AVCHOME.lat,
AVCHOME.lng,
AVCHOME.alt,
AVCHOME.heading,
)
if viewerip:
sim_cmd += " --fgout=%s:5505" % viewerip
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
sil = util.start_SIL("ArduCopter", wipe=True)
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options="--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter")
mavproxy.expect("Received [0-9]+ parameters")
# setup test parameters
mavproxy.send("param load %s/copter_AVC2013_params.parm\n" % testdir)
# mavproxy.send("param load %s/copter_params.parm\n" % testdir)
mavproxy.expect("Loaded [0-9]+ parameters")
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL("ArduCopter", height=HOME.alt)
options = "--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5"
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
options += " --map"
mavproxy = util.start_MAVProxy_SIL("ArduCopter", options=options)
mavproxy.expect("Logging to (\S+)")
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# the received parameters can come before or after the ready to fly message
mavproxy.expect(["Received [0-9]+ parameters", "Ready to FLY"])
mavproxy.expect(["Received [0-9]+ parameters", "Ready to FLY"])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
if map:
mavproxy.send("map icon 40.072467969730496 -105.2314389590174\n")
mavproxy.send("map icon 40.072600990533829 -105.23146100342274\n")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection("127.0.0.1:19550", robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def drive_APMrover2(viewerip=None, map=False, console=False):
'''drive APMrover2 in SIL
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
'''
sim_mode='HOLD' #config mode 4 system tests (AUTO | HOLD | GUIDED)
loop_mode='SIL' #HIL or SIL mode (HIL | SIL)
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --out=192.168.1.61:14501 --out=127.0.0.1:6601 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
options += ' --map'
if console:
options += ' --console'
if loop_mode == 'SIL':
sil = util.start_SIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
if loop_mode == 'HIL':
sil = util.start_HIL('APMrover2', wipe=True)
mavproxy = util.start_MAVProxy_HIL('APMrover2', options=options)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
# mavproxy.send("param load %s/Rover.parm\n" % testdir)
mavproxy.send("param load %s/Rover-def.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_rover.py') + ' --rate=50 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
runsim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Starting at lat')
if loop_mode == 'SIL':
sil = util.start_SIL('APMrover2')
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
if loop_mode == 'HIL':
sil = util.start_HIL('APMrover2')
mavproxy = util.start_MAVProxy_HIL('APMrover2', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/APMrover2-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([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduCopter(viewerip=None):
'''fly ArduCopter in SIL
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 expect_list, homeloc
hquad_cmd = util.reltopdir('../HILTest/hil_quad.py') + ' --fgrate=200 --home=%f,%f,%u,%u' % (
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
if viewerip:
hquad_cmd += ' --fgout=192.168.2.15:9123'
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
mavproxy.expect('Please Run Setup')
# we need to restart it after eeprom erase
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter')
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
hquad = pexpect.spawn(hquad_cmd, logfile=sys.stdout, timeout=10)
util.pexpect_autoclose(hquad)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1'
if viewerip:
options += ' --out=%s:14550' % viewerip
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.mavlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
mavproxy.expect('Received [0-9]+ parameters')
mavproxy.expect("Ready to FLY")
util.expect_setup_callback(mavproxy, expect_callback)
expect_list.extend([hquad, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_CopterAVC(viewerip=None, map=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
global homeloc
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/copter_AVC2013_params.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
pass
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
if map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduCopter(viewerip=None, map=False):
'''fly ArduCopter in SIL
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
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/copter_params.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
copyTLog = False
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
print( "WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location" )
copyTLog = True
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_ArduPlane(viewerip=None, map=False):
'''fly ArduPlane in SIL
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 map:
options += ' --map'
sil = util.start_SIL('ArduPlane', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/ArduPlane.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
mavproxy.send("param fetch\n")
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
cmd += " --home=%s --wind=%s" % (HOME_LOCATION, WIND)
if viewerip:
cmd += " --fgout=%s:5503" % viewerip
runsim = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Simulator ready to fly')
sil = util.start_SIL('ArduPlane')
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduPlane-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([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550',
robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
def fly_CopterAVC(viewerip=None, map=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
global homeloc
if TARGET != 'sitl':
util.build_SIL('ArduCopter', target=TARGET)
sim_cmd = util.reltopdir('Tools/autotest/pysim/sim_multicopter.py') + ' --frame=%s --rate=400 --home=%f,%f,%u,%u' % (
FRAME, AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
if viewerip:
sim_cmd += ' --fgout=%s:5503' % viewerip
sil = util.start_SIL('ArduCopter', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
mavproxy.send("param load %s/CopterAVC.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
# reboot with new parameters
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL('ArduCopter', height=HOME.alt)
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
sim.delaybeforesend = 0
util.pexpect_autoclose(sim)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/CopterAVC-test.tlog")
print("buildlog=%s" % buildlog)
if os.path.exists(buildlog):
os.unlink(buildlog)
os.link(logfile, buildlog)
# the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear()
expect_list_extend([sim, sil, mavproxy])
if map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
from optparse import OptionParser
parser = OptionParser("runFG.py [options]")
parser.add_option("--simin", help="SITL input (IP:port)")
parser.add_option("--simout", help="SITL output (IP:port)")
parser.add_option("--options", type='string', help='flightgear startup options')
(opts, args) = parser.parse_args()
os.chdir(util.reltopdir('Tools/autotest'))
atexit.register(util.pexpect_close_all)
setup_initial([0,0,0,0]);
cmd = "fgfs --model-hz=1000 --enable-freeze"
fg = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
fg.delaybeforesend = 0
util.pexpect_autoclose(fg)
i = fg.expect(["Successfully bound to socket for input on port (\d+)",
"Could not bind to socket for input"])
if i == 1:
print("Failed to start FlightGear - is another copy running?")
sys.exit(1)
jsb_out_address = interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
# socket addresses
sim_out_address = interpret_address(opts.simout)
sim_in_address = interpret_address(opts.simin)
# setup input from SITL sim
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sim_in.bind(sim_in_address)
sim_in.setblocking(0)
def fly_ArduPlane(viewerip=None, map=False):
'''fly ArduPlane in SIL
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 map:
options += ' --map'
sil = util.start_SIL('ArduPlane', wipe=True)
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters
mavproxy.send("param load %s/ArduPlane.parm\n" % testdir)
mavproxy.expect('Loaded [0-9]+ parameters')
mavproxy.send("param fetch\n")
# restart with new parms
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
# util.reltopdir("Tools/autotest/sim_arduplane_hil.sh")
# cmd += " --home=%s --wind=%s --script='%s/jsbsim/SW13_test.xml' "% (HOME_LOCATION, WIND, testdir)
#Original
cmd += " --home=%s --wind=%s"% (HOME_LOCATION, WIND)
if viewerip:
cmd += " --fgout=%s:5503" % viewerip
runsim = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
runsim.delaybeforesend = 0
util.pexpect_autoclose(runsim)
runsim.expect('Simulator ready to fly')
sil = util.start_SIL('ArduPlane')
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
mavproxy.expect('Logging to (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduPlane-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([runsim, sil, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise
(opts, args) = parser.parse_args()
os.chdir(util.reltopdir('Tools/autotest'))
# kill off child when we exit
atexit.register(util.pexpect_close_all)
# start child
cmd = "roslaunch last_letter launcher.launch ArduPlane:=true"
if opts.options:
cmd += ' %s' % opts.options
ros = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
ros.delaybeforesend = 0
util.pexpect_autoclose(ros)
ros_out_address = interpret_address("127.0.0.1:5505")
ros_in_address = interpret_address("127.0.0.1:5504")
# setup output to ROS
print("ROS listens for FG-FDM packets at %s" % str(ros_out_address))
ros_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_out.connect(ros_out_address)
# setup input from ROS
print("ROS sends FG-FDM packetes for the SITL at %s" % str(ros_in_address))
ros_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_in.bind(ros_in_address)
ros_in.setblocking(0)
os.chdir(util.reltopdir('Tools/autotest'))
# kill off child when we exit
atexit.register(util.pexpect_close_all)
setup_template(opts.home)
# start child
cmd = "JSBSim --realtime --suspend --nice --simulation-rate=%u --logdirectivefile=jsb_sim/fgout.xml --script=%s" % (opts.rate, opts.script)
if opts.options:
cmd += ' %s' % opts.options
jsb = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
jsb.delaybeforesend = 0
util.pexpect_autoclose(jsb)
i = jsb.expect(["Successfully bound to socket for input on port (\d+)",
"Could not bind to socket for input"])
if i == 1:
print("Failed to start JSBSim - is another copy running?")
sys.exit(1)
jsb_out_address = interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Creating UDP socket on port (\d+)")
jsb_in_address = interpret_address("127.0.0.1:%u" % int(jsb.match.group(1)))
jsb.expect("Successfully connected to socket for output")
jsb.expect("JSBSim Execution beginning")
# setup output to jsbsim
print("JSBSim console on %s" % str(jsb_out_address))
jsb_out = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
jsb_out.connect(jsb_out_address)
(opts, args) = parser.parse_args()
os.chdir(util.reltopdir('Tools/autotest'))
# kill off child when we exit
atexit.register(util.pexpect_close_all)
# start child
cmd = "roslaunch last_letter launcher.launch ArduPlane:=true"
if opts.options:
cmd += ' %s' % opts.options
ros = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
ros.delaybeforesend = 0
util.pexpect_autoclose(ros)
ros_out_address = interpret_address("127.0.0.1:5505")
ros_in_address = interpret_address("127.0.0.1:5504")
# setup output to ROS
print("ROS listens for FG-FDM packets at %s" % str(ros_out_address))
ros_out = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_out.connect(ros_out_address)
# setup input from ROS
print("ROS sends FG-FDM packetes for the SITL at %s" % str(ros_in_address))
ros_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ros_in.bind(ros_in_address)
ros_in.setblocking(0)
