autotest: adapt test suite to new simulation framework

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

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)
+    simquad = pexpect.spawn(simquad_cmd, logfile=sys.stdout, timeout=10)
-    util.pexpect_autoclose(hquad)
+    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)

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
+    # get it moving a bit first to avoid bad JSBSim ground physics
-    mavproxy.send('rc 3 1150\n')
+    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")
+    expect_list_clear()
-    if not fg_scenery:
+    expect_list_extend([runsim, sil, mavproxy])
-        raise RuntimeError("You must set the FG_SCENERY environment variable")
 
-    fgear_options = '''
+    print("Started simulator")
-    --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:
@@ -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)

Tools/autotest/autotest.py

@@ -2,10 +2,13 @@
 # APM automatic test suite
 # Andrew Tridgell, October 2011
 
-import pexpect, os, util, sys, shutil
+import pexpect, os, sys, shutil, atexit
-import arducopter, arduplane
 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)

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:
+        util.pexpect_drain(p)
-            p.read_nonblocking(100, timeout=0)
-        except pexpect.TIMEOUT:
-            pass
 
 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:
+        util.pexpect_drain(p)
-            while p.read_nonblocking(100, timeout=0):
-                pass
-        except pexpect.TIMEOUT:
-            pass
-
 
 class location(object):
     '''represent a GPS coordinate'''

Tools/autotest/jsbsim/fgout.xml

@@ -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"/>

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))

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)

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()