diff --git a/Tools/autotest/ArduPlane.parm b/Tools/autotest/ArduPlane.parm index f8086d4aad..83c0c44adf 100644 --- a/Tools/autotest/ArduPlane.parm +++ b/Tools/autotest/ArduPlane.parm @@ -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 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3b15c8aa0e..0990c5ff30 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index cdb6569e51..badd267940 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -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) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index c11cb2612c..edeeabdee7 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -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, 'FAILED', time.time() - t1) - util.pexpect_close_all() continue results.add(step, 'PASSED', 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) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 3e7808489b..84731ae940 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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''' diff --git a/Tools/autotest/jsbsim/fgout.xml b/Tools/autotest/jsbsim/fgout.xml index 9f0e3c2d4b..ce92818c77 100644 --- a/Tools/autotest/jsbsim/fgout.xml +++ b/Tools/autotest/jsbsim/fgout.xml @@ -1,2 +1,2 @@ - + diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 2664ef250a..21d25da4b8 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/Tools/autotest/jsbsim/runsim.py @@ -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)) diff --git a/Tools/autotest/pysim/mavextra.py b/Tools/autotest/pymavlink/mavextra.py similarity index 100% rename from Tools/autotest/pysim/mavextra.py rename to Tools/autotest/pymavlink/mavextra.py diff --git a/Tools/autotest/pymavlink/mavutil.py b/Tools/autotest/pymavlink/mavutil.py index 420ac76553..d52fc8e867 100644 --- a/Tools/autotest/pymavlink/mavutil.py +++ b/Tools/autotest/pymavlink/mavutil.py @@ -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) diff --git a/Tools/autotest/pymavlink/mavwp.py b/Tools/autotest/pymavlink/mavwp.py new file mode 100644 index 0000000000..c7c1be71f1 --- /dev/null +++ b/Tools/autotest/pymavlink/mavwp.py @@ -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()