mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-08 17:08:28 -04:00
autotest: added CopterAVC test
this will test the copter code for the AVC2013 mission
This commit is contained in:
Andrew Tridgell
parent
6036e7f538
commit
a900226ff7
8
Tools/autotest/AVC2013.txt
Normal file
8
Tools/autotest/AVC2013.txt
Normal file
@ -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
|
||||
68
Tools/autotest/CopterAVC.parm
Normal file
68
Tools/autotest/CopterAVC.parm
Normal file
@ -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 !
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user