mirror of https://github.com/ArduPilot/ardupilot
autotest: adapt test suite to new simulation framework
This commit is contained in:
Andrew Tridgell
parent
e6608b604b
commit
cf7073128d
|
|
@ -1,5 +1,14 @@
|
|||
LOG_BITMASK 4095
|
||||
SWITCH_ENABLE 0
|
||||
MAG_ENABLE 0
|
||||
TRIM_ARSPD_CM 2200
|
||||
ARSPD_ENABLE 1
|
||||
ARSP2PTCH_I 0.1
|
||||
ARSPD_FBW_MAX 30
|
||||
ARSPD_FBW_MIN 10
|
||||
KFF_RDDRMIX 0
|
||||
KFF_PTCHCOMP 0.2
|
||||
THR_MAX 100
|
||||
RC2_REV -1
|
||||
RC1_MAX 2000
|
||||
RC1_MIN 1000
|
||||
|
|
@ -33,6 +42,5 @@ FLTMODE5 2
|
|||
FLTMODE6 0
|
||||
FLTMODE_CH 8
|
||||
PTCH2SRV_P 2
|
||||
RLL2SRV_I 0.1
|
||||
RLL2SRV_I 0
|
||||
RLL2SRV_P 1.2
|
||||
INVERTEDFLT_CH 7
|
||||
|
|
|
|||
|
|
@ -2,12 +2,11 @@
|
|||
|
||||
import util, pexpect, sys, time, math, shutil, os
|
||||
from common import *
|
||||
import mavutil, mavwp
|
||||
|
||||
# get location of scripts
|
||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
sys.path.insert(0, util.reltopdir('../pymavlink'))
|
||||
import mavutil, mavwp
|
||||
|
||||
HOME=location(-35.362938,149.165085,584,270)
|
||||
|
||||
|
|
@ -229,12 +228,12 @@ def fly_ArduCopter(viewerip=None):
|
|||
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
|
||||
global homeloc
|
||||
|
||||
hquad_cmd = util.reltopdir('../HILTest/hil_quad.py') + ' --fgrate=200 --home=%f,%f,%u,%u' % (
|
||||
simquad_cmd = util.reltopdir('Tools/autotest/pysim/sim_quad.py') + ' --rate=400 --home=%f,%f,%u,%u' % (
|
||||
HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
if viewerip:
|
||||
hquad_cmd += ' --fgout=192.168.2.15:9123'
|
||||
simquad_cmd += ' --fgout=192.168.2.15:9123'
|
||||
|
||||
sil = util.start_SIL('ArduCopter', wipe=True)
|
||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
|
||||
|
|
@ -256,8 +255,9 @@ def fly_ArduCopter(viewerip=None):
|
|||
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)
|
||||
simquad = pexpect.spawn(simquad_cmd, logfile=sys.stdout, timeout=10)
|
||||
simquad.delaybeforesend = 0
|
||||
util.pexpect_autoclose(simquad)
|
||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1'
|
||||
if viewerip:
|
||||
options += ' --out=%s:14550' % viewerip
|
||||
|
|
@ -277,7 +277,8 @@ def fly_ArduCopter(viewerip=None):
|
|||
|
||||
util.expect_setup_callback(mavproxy, expect_callback)
|
||||
|
||||
expect_list.extend([hquad, sil, mavproxy])
|
||||
expect_list_clear()
|
||||
expect_list_extend([simquad, sil, mavproxy])
|
||||
|
||||
# get a mavlink connection going
|
||||
try:
|
||||
|
|
@ -351,9 +352,10 @@ def fly_ArduCopter(viewerip=None):
|
|||
except pexpect.TIMEOUT, e:
|
||||
failed = True
|
||||
|
||||
mav.close()
|
||||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sil)
|
||||
util.pexpect_close(hquad)
|
||||
util.pexpect_close(simquad)
|
||||
|
||||
if os.path.exists('ArduCopter-valgrind.log'):
|
||||
os.chmod('ArduCopter-valgrind.log', 0644)
|
||||
|
|
|
|||
|
|
@ -2,12 +2,11 @@
|
|||
|
||||
import util, pexpect, sys, time, math, shutil, os
|
||||
from common import *
|
||||
import mavutil
|
||||
|
||||
# get location of scripts
|
||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
sys.path.insert(0, util.reltopdir('../pymavlink'))
|
||||
import mavutil
|
||||
|
||||
HOME_LOCATION='-35.362938,149.165085,584,270'
|
||||
|
||||
|
|
@ -21,8 +20,8 @@ def takeoff(mavproxy, mav):
|
|||
# some rudder to counteract the prop torque
|
||||
mavproxy.send('rc 4 1600\n')
|
||||
|
||||
# get it moving a bit first to avoid bad fgear ground physics
|
||||
mavproxy.send('rc 3 1150\n')
|
||||
# get it moving a bit first to avoid bad JSBSim ground physics
|
||||
mavproxy.send('rc 3 1040\n')
|
||||
mav.recv_match(condition='VFR_HUD.groundspeed>3', blocking=True)
|
||||
|
||||
# a bit faster
|
||||
|
|
@ -35,7 +34,7 @@ def takeoff(mavproxy, mav):
|
|||
mavproxy.send('rc 3 1800\n')
|
||||
|
||||
# gain a bit of altitude
|
||||
wait_altitude(mav, homeloc.alt+30, homeloc.alt+40, timeout=30)
|
||||
wait_altitude(mav, homeloc.alt+30, homeloc.alt+60, timeout=30)
|
||||
|
||||
# level off
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
|
|
@ -47,6 +46,7 @@ def fly_left_circuit(mavproxy, mav):
|
|||
'''fly a left circuit, 200m on a side'''
|
||||
mavproxy.send('switch 4\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
wait_level_flight(mavproxy, mav)
|
||||
|
||||
print("Flying left circuit")
|
||||
|
|
@ -64,15 +64,17 @@ def fly_left_circuit(mavproxy, mav):
|
|||
|
||||
def fly_RTL(mavproxy, mav):
|
||||
'''fly to home'''
|
||||
print("Flying home in RTL")
|
||||
mavproxy.send('switch 2\n')
|
||||
wait_mode(mav, 'RTL')
|
||||
wait_location(mav, homeloc, accuracy=30,
|
||||
wait_location(mav, homeloc, accuracy=80,
|
||||
target_altitude=100, height_accuracy=10)
|
||||
print("RTL Complete")
|
||||
return True
|
||||
|
||||
def fly_LOITER(mavproxy, mav):
|
||||
'''loiter where we are'''
|
||||
print("Testing LOITER")
|
||||
mavproxy.send('switch 3\n')
|
||||
wait_mode(mav, 'LOITER')
|
||||
while True:
|
||||
|
|
@ -193,11 +195,11 @@ def fly_ArduPlane(viewerip=None):
|
|||
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
|
||||
global homeloc
|
||||
|
||||
options = '--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
|
||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
|
||||
if viewerip:
|
||||
options += ' --out=%s:14550' % viewerip
|
||||
options += " --out=%s:14550" % viewerip
|
||||
|
||||
sil = util.start_SIL('ArduPlane', wipe=True)
|
||||
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
|
||||
|
|
@ -211,6 +213,17 @@ def fly_ArduPlane(viewerip=None):
|
|||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sil)
|
||||
|
||||
cmd = util.reltopdir("Tools/autotest/jsbsim/runsim.py")
|
||||
cmd += " --home=%s --script=%s/rascal_test.xml" % (
|
||||
HOME_LOCATION, util.reltopdir("Tools/autotest/jsbsim"))
|
||||
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+)')
|
||||
|
|
@ -227,57 +240,10 @@ def fly_ArduPlane(viewerip=None):
|
|||
|
||||
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")
|
||||
expect_list_clear()
|
||||
expect_list_extend([runsim, sil, mavproxy])
|
||||
|
||||
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])
|
||||
print("Started simulator")
|
||||
|
||||
# get a mavlink connection going
|
||||
try:
|
||||
|
|
@ -293,7 +259,8 @@ def fly_ArduPlane(viewerip=None):
|
|||
try:
|
||||
mav.wait_heartbeat()
|
||||
setup_rc(mavproxy)
|
||||
mav.recv_match(type='GPS_RAW', condition='GPS_RAW.lat != 0 and GPS_RAW.alt != 0 and VFR_HUD.alt != 0',
|
||||
mavproxy.expect('APM: init home')
|
||||
mav.recv_match(type='GPS_RAW', condition='MAV.flightmode!="INITIALISING" and GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0 and VFR_HUD.alt > 10',
|
||||
blocking=True)
|
||||
homeloc = current_location(mav)
|
||||
print("Home location: %s" % homeloc)
|
||||
|
|
@ -319,9 +286,10 @@ def fly_ArduPlane(viewerip=None):
|
|||
except pexpect.TIMEOUT, e:
|
||||
failed = True
|
||||
|
||||
mav.close()
|
||||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sil)
|
||||
util.pexpect_close(fgear)
|
||||
util.pexpect_close(runsim)
|
||||
|
||||
if os.path.exists('ArduPlane-valgrind.log'):
|
||||
os.chmod('ArduPlane-valgrind.log', 0644)
|
||||
|
|
|
|||
|
|
@ -2,10 +2,13 @@
|
|||
# APM automatic test suite
|
||||
# Andrew Tridgell, October 2011
|
||||
|
||||
import pexpect, os, util, sys, shutil
|
||||
import arducopter, arduplane
|
||||
import pexpect, os, sys, shutil, atexit
|
||||
import optparse, fnmatch, time, glob, traceback
|
||||
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim'))
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pymavlink'))
|
||||
import util, arducopter, arduplane
|
||||
|
||||
os.environ['PYTHONUNBUFFERED'] = '1'
|
||||
|
||||
os.putenv('TMPDIR', util.reltopdir('tmp'))
|
||||
|
|
@ -165,9 +168,6 @@ def run_step(step):
|
|||
return arducopter.fly_ArduCopter(viewerip=opts.viewerip)
|
||||
|
||||
if step == 'fly.ArduPlane':
|
||||
if not opts.experimental:
|
||||
print("DISABLED: use --experimental to enable fly.ArduPlane")
|
||||
return True
|
||||
return arduplane.fly_ArduPlane(viewerip=opts.viewerip)
|
||||
|
||||
if step == 'convertgpx':
|
||||
|
|
@ -235,6 +235,7 @@ def run_tests(steps):
|
|||
passed = True
|
||||
failed = []
|
||||
for step in steps:
|
||||
util.pexpect_close_all()
|
||||
if skip_step(step):
|
||||
continue
|
||||
|
||||
|
|
@ -253,13 +254,14 @@ def run_tests(steps):
|
|||
print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg))
|
||||
traceback.print_exc(file=sys.stdout)
|
||||
results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1)
|
||||
util.pexpect_close_all()
|
||||
continue
|
||||
results.add(step, '<span class="passed-text">PASSED</span>', time.time() - t1)
|
||||
print(">>>> PASSED STEP: %s at %s" % (step, time.asctime()))
|
||||
if not passed:
|
||||
print("FAILED %u tests: %s" % (len(failed), failed))
|
||||
|
||||
util.pexpect_close_all()
|
||||
|
||||
results.addglob("Google Earth track", '*.kmz')
|
||||
results.addfile('Full Logs', 'autotest-output.txt')
|
||||
results.addglob('DataFlash Log', '*.flashlog')
|
||||
|
|
@ -284,6 +286,8 @@ if lck is None:
|
|||
print("autotest is locked - exiting")
|
||||
sys.exit(0)
|
||||
|
||||
atexit.register(util.pexpect_close_all)
|
||||
|
||||
try:
|
||||
if not run_tests(steps):
|
||||
sys.exit(1)
|
||||
|
|
|
|||
|
|
@ -4,14 +4,22 @@ import util, pexpect, time, math
|
|||
# messages. This keeps the output to stdout flowing
|
||||
expect_list = []
|
||||
|
||||
def expect_list_clear():
|
||||
'''clear the expect list'''
|
||||
global expect_list
|
||||
for p in expect_list[:]:
|
||||
expect_list.remove(p)
|
||||
|
||||
def expect_list_extend(list):
|
||||
'''extend the expect list'''
|
||||
global expect_list
|
||||
expect_list.extend(list)
|
||||
|
||||
def idle_hook(mav):
|
||||
'''called when waiting for a mavlink message'''
|
||||
global expect_list
|
||||
for p in expect_list:
|
||||
try:
|
||||
p.read_nonblocking(100, timeout=0)
|
||||
except pexpect.TIMEOUT:
|
||||
pass
|
||||
util.pexpect_drain(p)
|
||||
|
||||
def message_hook(mav, msg):
|
||||
'''called as each mavlink msg is received'''
|
||||
|
|
@ -25,12 +33,7 @@ def expect_callback(e):
|
|||
for p in expect_list:
|
||||
if p == e:
|
||||
continue
|
||||
try:
|
||||
while p.read_nonblocking(100, timeout=0):
|
||||
pass
|
||||
except pexpect.TIMEOUT:
|
||||
pass
|
||||
|
||||
util.pexpect_drain(p)
|
||||
|
||||
class location(object):
|
||||
'''represent a GPS coordinate'''
|
||||
|
|
|
|||
|
|
@ -1,2 +1,2 @@
|
|||
<?xml version="1.0"?>
|
||||
<output name="localhost" type="FLIGHTGEAR" port="5123" protocol="udp" rate="1000">
|
||||
<output name="localhost" type="FLIGHTGEAR" port="5123" protocol="udp" rate="1000"/>
|
||||
|
|
|
|||
|
|
@ -123,6 +123,8 @@ for m in [ 'home', 'script' ]:
|
|||
parser.print_help()
|
||||
sys.exit(1)
|
||||
|
||||
os.chdir(util.reltopdir('Tools/autotest'))
|
||||
|
||||
# kill off child when we exit
|
||||
atexit.register(util.pexpect_close_all)
|
||||
|
||||
|
|
@ -144,7 +146,6 @@ 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")
|
||||
print(dir(jsb))
|
||||
|
||||
# setup output to jsbsim
|
||||
print("JSBSim console on %s" % str(jsb_out_address))
|
||||
|
|
|
|||
|
|
@ -64,6 +64,10 @@ class mavfile(object):
|
|||
'''default recv method'''
|
||||
raise RuntimeError('no recv() method supplied')
|
||||
|
||||
def close(self, n=None):
|
||||
'''default close method'''
|
||||
raise RuntimeError('no close() method supplied')
|
||||
|
||||
def write(self, buf):
|
||||
'''default write method'''
|
||||
raise RuntimeError('no write() method supplied')
|
||||
|
|
@ -219,6 +223,9 @@ class mavserial(mavfile):
|
|||
fd = None
|
||||
mavfile.__init__(self, fd, device, source_system=source_system)
|
||||
|
||||
def close(self):
|
||||
self.port.close()
|
||||
|
||||
def recv(self,n=None):
|
||||
if n is None:
|
||||
n = self.mav.bytes_needed()
|
||||
|
|
@ -270,6 +277,9 @@ class mavudp(mavfile):
|
|||
self.last_address = None
|
||||
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
|
||||
|
||||
def close(self):
|
||||
self.port.close()
|
||||
|
||||
def recv(self,n=None):
|
||||
try:
|
||||
data, self.last_address = self.port.recvfrom(300)
|
||||
|
|
@ -315,6 +325,9 @@ class mavtcp(mavfile):
|
|||
self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
|
||||
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
|
||||
|
||||
def close(self):
|
||||
self.port.close()
|
||||
|
||||
def recv(self,n=None):
|
||||
if n is None:
|
||||
n = self.mav.bytes_needed()
|
||||
|
|
@ -355,6 +368,9 @@ class mavlogfile(mavfile):
|
|||
self.f = open(filename, mode)
|
||||
mavfile.__init__(self, None, filename, source_system=source_system)
|
||||
|
||||
def close(self):
|
||||
self.f.close()
|
||||
|
||||
def recv(self,n=None):
|
||||
if n is None:
|
||||
n = self.mav.bytes_needed()
|
||||
|
|
@ -413,6 +429,9 @@ class mavchildexec(mavfile):
|
|||
|
||||
mavfile.__init__(self, self.fd, filename, source_system=source_system)
|
||||
|
||||
def close(self):
|
||||
self.child.close()
|
||||
|
||||
def recv(self,n=None):
|
||||
try:
|
||||
x = self.child.stdout.read(1)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,135 @@
|
|||
'''
|
||||
module for loading/saving waypoints
|
||||
'''
|
||||
|
||||
import mavlink
|
||||
|
||||
class MAVWPError(Exception):
|
||||
'''MAVLink WP error class'''
|
||||
def __init__(self, msg):
|
||||
Exception.__init__(self, msg)
|
||||
self.message = msg
|
||||
|
||||
class MAVWPLoader(object):
|
||||
'''MAVLink waypoint loader'''
|
||||
def __init__(self, target_system=0, target_component=0):
|
||||
self.wpoints = []
|
||||
self.target_system = target_system
|
||||
self.target_component = target_component
|
||||
|
||||
|
||||
def count(self):
|
||||
'''return number of waypoints'''
|
||||
return len(self.wpoints)
|
||||
|
||||
def wp(self, i):
|
||||
'''return a waypoint'''
|
||||
return self.wpoints[i]
|
||||
|
||||
def add(self, w):
|
||||
'''add a waypoint'''
|
||||
w.seq = self.count()
|
||||
self.wpoints.append(w)
|
||||
|
||||
def remove(self, w):
|
||||
'''remove a waypoint'''
|
||||
self.wpoints.remove(w)
|
||||
|
||||
def clear(self):
|
||||
'''clear waypoint list'''
|
||||
self.wpoints = []
|
||||
|
||||
def _read_waypoint_v100(self, line):
|
||||
'''read a version 100 waypoint'''
|
||||
cmdmap = {
|
||||
2 : mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||
3 : mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
|
||||
4 : mavlink.MAV_CMD_NAV_LAND,
|
||||
24: mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||
26: mavlink.MAV_CMD_NAV_LAND,
|
||||
25: mavlink.MAV_CMD_NAV_WAYPOINT ,
|
||||
27: mavlink.MAV_CMD_NAV_LOITER_UNLIM
|
||||
}
|
||||
a = line.split()
|
||||
if len(a) != 13:
|
||||
raise MAVWPError("invalid waypoint line with %u values" % len(a))
|
||||
w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
|
||||
int(a[0]), # seq
|
||||
int(a[1]), # frame
|
||||
int(a[2]), # action
|
||||
int(a[7]), # current
|
||||
int(a[12]), # autocontinue
|
||||
float(a[5]), # param1,
|
||||
float(a[6]), # param2,
|
||||
float(a[3]), # param3
|
||||
float(a[4]), # param4
|
||||
float(a[9]), # x, latitude
|
||||
float(a[8]), # y, longitude
|
||||
float(a[10]) # z
|
||||
)
|
||||
if not w.command in cmdmap:
|
||||
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
|
||||
|
||||
w.command = cmdmap[w.command]
|
||||
return w
|
||||
|
||||
def _read_waypoint_v110(self, line):
|
||||
'''read a version 110 waypoint'''
|
||||
a = line.split()
|
||||
if len(a) != 12:
|
||||
raise MAVWPError("invalid waypoint line with %u values" % len(a))
|
||||
w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
|
||||
int(a[0]), # seq
|
||||
int(a[2]), # frame
|
||||
int(a[3]), # command
|
||||
int(a[1]), # current
|
||||
int(a[11]), # autocontinue
|
||||
float(a[4]), # param1,
|
||||
float(a[5]), # param2,
|
||||
float(a[6]), # param3
|
||||
float(a[7]), # param4
|
||||
float(a[8]), # x (latitude)
|
||||
float(a[9]), # y (longitude)
|
||||
float(a[10]) # z (altitude)
|
||||
)
|
||||
return w
|
||||
|
||||
|
||||
def load(self, filename):
|
||||
'''load waypoints from a file.
|
||||
returns number of waypoints loaded'''
|
||||
f = open(filename, mode='r')
|
||||
version_line = f.readline().strip()
|
||||
if version_line == "QGC WPL 100":
|
||||
readfn = self._read_waypoint_v100
|
||||
elif version_line == "QGC WPL 110":
|
||||
readfn = self._read_waypoint_v110
|
||||
else:
|
||||
f.close()
|
||||
raise MAVWPError("Unsupported waypoint format '%s'" % version_line)
|
||||
|
||||
self.clear()
|
||||
|
||||
for line in f:
|
||||
if line.startswith('#'):
|
||||
continue
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
w = readfn(line)
|
||||
if w is not None:
|
||||
self.add(w)
|
||||
f.close()
|
||||
return len(self.wpoints)
|
||||
|
||||
|
||||
def save(self, filename):
|
||||
'''save waypoints to a file'''
|
||||
f = open(filename, mode='w')
|
||||
f.write("QGC WPL 110\n")
|
||||
for w in self.wpoints:
|
||||
f.write("%u\t%u\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%u\n" % (
|
||||
w.seq, w.current, w.frame, w.command,
|
||||
w.param1, w.param2, w.param3, w.param4,
|
||||
w.x, w.y, w.z, w.autocontinue))
|
||||
f.close()
|
||||
Loading…
Reference in New Issue