From a900226ff792fd5965edf8b27134c8a04cac85ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 May 2013 15:33:32 +1000 Subject: [PATCH] autotest: added CopterAVC test this will test the copter code for the AVC2013 mission --- Tools/autotest/AVC2013.txt | 8 ++ Tools/autotest/CopterAVC.parm | 68 +++++++++++++++++ Tools/autotest/arducopter.py | 133 +++++++++++++++++++++++++++++++++- Tools/autotest/autotest.py | 16 +++- 4 files changed, 222 insertions(+), 3 deletions(-) create mode 100644 Tools/autotest/AVC2013.txt create mode 100644 Tools/autotest/CopterAVC.parm diff --git a/Tools/autotest/AVC2013.txt b/Tools/autotest/AVC2013.txt new file mode 100644 index 0000000000..75d626125a --- /dev/null +++ b/Tools/autotest/AVC2013.txt @@ -0,0 +1,8 @@ +QGC WPL 110 +0 1 0 16 0 0 0 0 40.072842 -105.230575 0.000000 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10.000000 1 +2 0 3 16 10.000000 0.000000 0.000000 0.000000 40.075691 -105.232287 20.000000 1 +3 0 3 203 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +4 0 3 16 1.000000 0.000000 0.000000 0.000000 40.072670 -105.231257 20.000000 1 +5 0 3 16 1.000000 0.000000 0.000000 0.000000 40.072670 -105.231257 3.000000 1 +6 0 3 21 0.000000 0.000000 0.000000 0.000000 40.072846 -105.230575 0.000000 1 diff --git a/Tools/autotest/CopterAVC.parm b/Tools/autotest/CopterAVC.parm new file mode 100644 index 0000000000..804fd32619 --- /dev/null +++ b/Tools/autotest/CopterAVC.parm @@ -0,0 +1,68 @@ +WPNAV_SPEED 1000 +WPNAV_SPEED_UP 500 +WPNAV_SPEED_DN 300 +WP_YAW_BEHAVIOR 0 +FRAME 0 +MAG_ENABLE 1 +LOG_BITMASK 4095 +FS_THR_ENABLE 1 +COMPASS_LEARN 0 +COMPASS_OFS_X 5 +COMPASS_OFS_Y 13 +COMPASS_OFS_Z -18 +RC1_MAX 2000.000000 +RC1_MIN 1000.000000 +RC1_TRIM 1500.000000 +RC2_MAX 2000.000000 +RC2_MIN 1000.000000 +RC2_TRIM 1500.000000 +RC3_MAX 2000.000000 +RC3_MIN 1000.000000 +RC3_TRIM 1500.000000 +RC4_MAX 2000.000000 +RC4_MIN 1000.000000 +RC4_TRIM 1500.000000 +RC5_MAX 2000.000000 +RC5_MIN 1000.000000 +RC5_TRIM 1500.000000 +RC6_MAX 2000.000000 +RC6_MIN 1000.000000 +RC6_TRIM 1500.000000 +RC7_MAX 2000.000000 +RC7_MIN 1000.000000 +RC7_TRIM 1500.000000 +RC8_MAX 2000.000000 +RC8_MIN 1000.000000 +RC8_TRIM 1500.000000 +FLTMODE1 7 +FLTMODE2 9 +FLTMODE3 6 +FLTMODE4 3 +FLTMODE5 5 +FLTMODE6 0 +SUPER_SIMPLE 0 +RTL_ALT_FINAL 0 +SIM_GPS_DELAY 2 +SIM_ACC_RND 0 +SIM_GYR_RND 0 +SIM_WIND_SPD 0 +SIM_WIND_TURB 0 +SIM_BARO_RND 0 +SIM_MAG_RND 0 +# flightmodes +# switch 1 Circle +# switch 2 LAND +# switch 3 RTL +# switch 4 Auto +# switch 5 Loiter +# switch 6 Stab +# STABILIZE 0 ! +# ACRO 1 +# ALT_HOLD 2 +# AUTO 3 ! +# GUIDED 4 +# LOITER 5 ! +# RTL 6 ! +# CIRCLE 7 ! +# POSITION 8 +# LAND 9 ! diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index b30a559a49..11496d89f6 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10,6 +10,7 @@ testdir=os.path.dirname(os.path.realpath(__file__)) FRAME='+' TARGET='sitl' HOME=mavutil.location(-35.362938,149.165085,584,270) +AVCHOME=mavutil.location(40.072842,-105.230575,1586,0) homeloc = None num_wp = 0 @@ -34,6 +35,7 @@ def arm_motors(mavproxy, mav): mavproxy.send('rc 4 2000\n') mavproxy.expect('APM: ARMING MOTORS') mavproxy.send('rc 4 1500\n') + mav.motors_armed_wait() print("MOTORS ARMED OK") return True @@ -45,6 +47,7 @@ def disarm_motors(mavproxy, mav): mavproxy.send('rc 4 1000\n') mavproxy.expect('APM: DISARMING MOTORS') mavproxy.send('rc 4 1500\n') + mav.motors_disarmed_wait() print("MOTORS DISARMED OK") return True @@ -547,7 +550,7 @@ def fly_ArduCopter(viewerip=None, map=False): if viewerip: options += ' --out=%s:14550' % viewerip if map: - options += ' --map --console' + options += ' --map' mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) mavproxy.expect('Logging to (\S+)') logfile = mavproxy.match.group(1) @@ -775,3 +778,131 @@ def fly_ArduCopter(viewerip=None, map=False): print("FAILED: %s" % e) return False return True + + +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]) + + # 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) + mav.idle_hooks.append(idle_hook) + + + failed = False + e = 'None' + try: + mav.wait_heartbeat() + setup_rc(mavproxy) + homeloc = mav.location() + + print("# Calibrate level") + if not calibrate_level(mavproxy, mav): + print("calibrate_level failed") + failed = True + + # Arm + print("# Arm motors") + if not arm_motors(mavproxy, mav): + print("arm_motors failed") + failed = True + + # Fly mission #1 + print("# Upload AVC mission") + if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "AVC2013.txt")): + print("upload_mission_from_file failed") + failed = True + + # this grabs our mission count + print("# store mission1 locally") + if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "AVC2013.txt")): + print("load_mission_from_file failed") + failed = True + + print("# raising throttle") + mavproxy.send('rc 3 1300\n') + + print("# Fly mission 1") + if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10): + print("fly_mission failed") + failed = True + else: + print("Flew mission 1 OK") + + print("# lowering throttle") + mavproxy.send('rc 3 1000\n') + + #mission includes LAND at end so should be ok to disamr + print("# disarm motors") + if not disarm_motors(mavproxy, mav): + print("disarm_motors failed") + failed = True + + except pexpect.TIMEOUT, e: + failed = True + + mav.close() + util.pexpect_close(mavproxy) + util.pexpect_close(sil) + util.pexpect_close(sim) + + if failed: + print("FAILED: %s" % e) + return False + return True diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index ce09d13889..0e99d1e015 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -35,11 +35,15 @@ def get_default_params(atype): print("Saved defaults for %s to %s" % (atype, dest)) return True -def dump_logs(atype): +def dump_logs(atype, logname=None): '''dump DataFlash logs''' print("Dumping logs for %s" % atype) + + if logname is None: + logname = atype + sil = util.start_SIL(atype) - logfile = util.reltopdir('../buildlogs/%s.flashlog' % atype) + logfile = util.reltopdir('../buildlogs/%s.flashlog' % logname) log = open(logfile, mode='w') mavproxy = util.start_MAVProxy_SIL(atype, setup=True, logfile=log) mavproxy.send('\n\n\n') @@ -190,6 +194,8 @@ steps = [ 'defaults.ArduCopter', 'fly.ArduCopter', 'logs.ArduCopter', + 'fly.CopterAVC', + 'logs.CopterAVC', 'convertgpx', ] @@ -259,12 +265,18 @@ def run_step(step): if step == 'logs.ArduCopter': return dump_logs('ArduCopter') + if step == 'logs.CopterAVC': + return dump_logs('ArduCopter', 'CopterAVC') + if step == 'logs.APMrover2': return dump_logs('APMrover2') if step == 'fly.ArduCopter': return arducopter.fly_ArduCopter(viewerip=opts.viewerip, map=opts.map) + if step == 'fly.CopterAVC': + return arducopter.fly_CopterAVC(viewerip=opts.viewerip, map=opts.map) + if step == 'fly.ArduPlane': return arduplane.fly_ArduPlane(viewerip=opts.viewerip, map=opts.map)