autotest: added initial implementation of flying of ArduPlane

takeoff, circuit, roll, RTL, mission

Tools/autotest/ArduPlane.parm

@@ -0,0 +1,38 @@
+LOG_BITMASK	4095
+SWITCH_ENABLE   0
+RC2_REV		-1
+RC1_MAX         2000
+RC1_MIN         1000
+RC1_TRIM        1500
+RC2_MAX         2000
+RC2_MIN         1000
+RC2_TRIM        1500
+RC3_MAX         2000
+RC3_MIN         1000
+RC3_TRIM        1500
+RC4_MAX         2000
+RC4_MIN         1000
+RC4_TRIM        1500
+RC5_MAX         2000
+RC5_MIN         1000
+RC5_TRIM        1500
+RC6_MAX         2000
+RC6_MIN         1000
+RC6_TRIM        1500
+RC7_MAX         2000
+RC7_MIN         1000
+RC7_TRIM        1500
+RC8_MAX         2000
+RC8_MIN         1000
+RC8_TRIM        1500
+FLTMODE1        10
+FLTMODE2        11
+FLTMODE3        12
+FLTMODE4        5
+FLTMODE5        2
+FLTMODE6        0
+FLTMODE_CH      8
+PTCH2SRV_P      2
+RLL2SRV_I       0.1
+RLL2SRV_P       1.2
+INVERTEDFLT_CH  7

Tools/autotest/ap1.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	1	0	16	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	582.000000	1
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	120.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	120.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361721	149.161835	120.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364170	149.164627	120.000000	1
+5	0	3	20	0.000000	0.000000	0.000000	0.000000	-35.363289	149.165253	50.000000	1

Tools/autotest/arducopter.py

@@ -279,6 +279,7 @@ def fly_ArduCopter(viewerip=None):
 
 
     failed = False
+    e = 'None'
     try:
         mav.wait_heartbeat()
         mav.recv_match(type='GPS_RAW', blocking=True)

Tools/autotest/arduplane.py

@@ -0,0 +1,269 @@
+# fly ArduPlane in SIL
+
+import util, pexpect, sys, time, math, shutil, os
+from common import *
+
+# 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'
+
+homeloc = None
+
+def takeoff(mavproxy, mav):
+    '''takeoff get to 30m altitude'''
+    mavproxy.send('switch 4\n')
+    wait_mode(mav, 'FBWA')
+
+    # 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')
+    mav.recv_match(condition='VFR_HUD.groundspeed>3', blocking=True)
+
+    # a bit faster
+    mavproxy.send('rc 3 1600\n')
+    mav.recv_match(condition='VFR_HUD.groundspeed>10', blocking=True)
+
+    # hit the gas harder now, and give it some elevator
+    mavproxy.send('rc 4 1500\n')
+    mavproxy.send('rc 2 1200\n')
+    mavproxy.send('rc 3 1800\n')
+
+    # gain a bit of altitude
+    wait_altitude(mav, homeloc.alt+30, homeloc.alt+40, timeout=30)
+
+    # level off
+    mavproxy.send('rc 2 1500\n')
+
+    print("TAKEOFF COMPLETE")
+    return True
+
+def fly_left_circuit(mavproxy, mav):
+    '''fly a left circuit, 200m on a side'''
+    mavproxy.send('switch 4\n')
+    wait_mode(mav, 'FBWA')
+
+    print("Flying left circuit")
+    # do 4 turns
+    for i in range(0,4):
+        # hard left
+        print("Starting turn %u" % i)
+        mavproxy.send('rc 1 1000\n')
+        wait_heading(mav, 270 - (90*i))
+        mavproxy.send('rc 1 1500\n')
+        print("Starting leg %u" % i)
+        wait_distance(mav, 100)
+    print("Circuit complete")
+    return True
+
+def fly_RTL(mavproxy, mav):
+    '''fly to home'''
+    mavproxy.send('switch 2\n')
+    wait_mode(mav, 'RTL')
+    wait_location(mav, homeloc, accuracy=30,
+                  target_altitude=100, height_accuracy=10)
+    print("RTL Complete")
+    return True
+
+def fly_LOITER(mavproxy, mav):
+    '''loiter where we are'''
+    mavproxy.send('switch 3\n')
+    wait_mode(mav, 'LOITER')
+    while True:
+        wait_heading(mav, 0)
+        wait_heading(mav, 180)
+    return True
+
+
+def change_altitude(mavproxy, mav, altitude, accuracy=10):
+    '''get to a given altitude'''
+    mavproxy.send('switch 4\n')
+    wait_mode(mav, 'FBWA')
+    alt_error = mav.messages['VFR_HUD'].alt - altitude
+    if alt_error > 0:
+        mavproxy.send('rc 2 2000\n')
+    else:
+        mavproxy.send('rc 2 1000\n')
+    wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2)
+    mavproxy.send('rc 2 1500\n')
+    print("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
+    return True
+
+
+def axial_left_roll(mavproxy, mav, count=1):
+    '''fly a left axial roll'''
+    # full throttle!
+    mavproxy.send('rc 3 2000\n')
+    change_altitude(mavproxy, mav, homeloc.alt+200)
+
+    # fly the roll in manual
+    mavproxy.send('switch 5\n')
+    wait_mode(mav, 'STABILIZE')
+
+    while count > 0:
+        print("Starting roll")
+        mavproxy.send('rc 1 1000\n')
+        wait_roll(mav, -150, accuracy=20)
+        wait_roll(mav, 150, accuracy=20)
+        wait_roll(mav, 0, accuracy=20)
+        count -= 1
+
+    # back to FBWA
+    mavproxy.send('rc 1 1500\n')
+    mavproxy.send('switch 4\n')
+    wait_mode(mav, 'FBWA')
+    mavproxy.send('rc 3 1700\n')
+    if not wait_distance(mav, 50):
+        return False
+    return True
+
+
+
+def setup_rc(mavproxy):
+    '''setup RC override control'''
+    for chan in [1,2,4,5,6,7]:
+        mavproxy.send('rc %u 1500\n' % chan)
+    mavproxy.send('rc 3 1000\n')
+    mavproxy.send('rc 8 1800\n')
+
+
+def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
+    '''fly a mission from a file'''
+    global homeloc
+    mavproxy.send('wp load %s\n' % filename)
+    mavproxy.expect('flight plan received')
+    mavproxy.send('wp list\n')
+    mavproxy.expect('Requesting [0-9]+ waypoints')
+    mavproxy.send('switch 1\n') # auto mode
+    wait_mode(mav, 'AUTO')
+    if not wait_distance(mav, 30, timeout=120):
+        return False
+    if not wait_location(mav, homeloc, accuracy=50, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
+        return False
+    print("Mission OK")
+    return True
+
+
+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
+
+    sil = util.start_SIL('ArduPlane', wipe=True)
+    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
+    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')
+
+    # setup test parameters
+    mavproxy.send("param load %s/ArduPlane.parm\n" % testdir)
+    mavproxy.expect('Loaded [0-9]+ parameters')
+
+    util.expect_setup_callback(mavproxy, expect_callback)
+
+    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="/home/tridge/project/UAV/fgdata/Scenery" \
+    --disable-anti-alias-hud \
+    --wind=0@0 \
+'''
+    # start fgear
+    if os.getenv('DISPLAY'):
+        cmd = 'fgfs %s' % fgear_options
+    else:
+        cmd = "xvfb-run -s '-screen 0 800x600x24' fgfs --enable-wireframe %s" % fgear_options
+    fgear = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10)
+    util.pexpect_autoclose(fgear)
+
+    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
+    mav.message_hooks.append(message_hook)
+
+    failed = False
+    e = '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',
+                       blocking=True)
+        homeloc = current_location(mav)
+        print("Home location: %s" % homeloc)
+        if not takeoff(mavproxy, mav):
+            failed = True
+        if not fly_left_circuit(mavproxy, mav):
+            failed = True
+        if not axial_left_roll(mavproxy, mav, 1):
+            failed = True
+        if not fly_RTL(mavproxy, mav):
+            failed = True
+        if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
+                           target_altitude=homeloc.alt+100):
+            failed = True
+    except pexpect.TIMEOUT, e:
+        failed = True
+
+    util.pexpect_close(mavproxy)
+    util.pexpect_close(sil)
+    util.pexpect_close(fgear)
+
+    if os.path.exists('ArduPlane-valgrind.log'):
+        os.chmod('ArduPlane-valgrind.log', 0644)
+        shutil.copy("ArduPlane-valgrind.log", util.reltopdir("../buildlogs/ArduPlane-valgrind.log"))
+
+    if failed:
+        print("FAILED: %s" % e)
+        return False
+    return True

Tools/autotest/autotest.py

@@ -2,7 +2,8 @@
 # APM automatic test suite
 # Andrew Tridgell, October 2011
 
-import pexpect, os, util, sys, shutil, arducopter
+import pexpect, os, util, sys, shutil
+import arducopter, arduplane
 import optparse, fnmatch, time, glob, traceback
 
 os.putenv('TMPDIR', util.reltopdir('tmp'))
@@ -91,6 +92,7 @@ steps = [
     'build.ArduPlane',
     'defaults.ArduPlane',
     'logs.ArduPlane',
+    'fly.ArduPlane',
     'build1280.ArduCopter',
     'build2560.ArduCopter',
     'build.ArduCopter',
@@ -147,6 +149,9 @@ def run_step(step):
     if step == 'fly.ArduCopter':
         return arducopter.fly_ArduCopter()
 
+    if step == 'fly.ArduPlane':
+        return arduplane.fly_ArduPlane(viewerip=opts.viewerip)
+
     if step == 'convertgpx':
         return convert_gpx()
 

Tools/autotest/util.py

@@ -73,9 +73,15 @@ def pexpect_autoclose(p):
 def pexpect_close(p):
     '''close a pexpect child'''
     global close_list
-    p.close()
+    try:
+        p.close()
+    except Exception:
+        pass
     time.sleep(1)
-    p.close(force=True)
+    try:
+        p.close(force=True)
+    except Exception:
+        pass
     if p in close_list:
         close_list.remove(p)