mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
0b484afbbe
This does SIL testing of ArduPlane and ArduCopter. For now it just does a basic LOITER test of ArduCopter. It produces logs and a kml of the flight on apm.tridgell.net
144 lines
4.3 KiB
Python
144 lines
4.3 KiB
Python
# fly ArduCopter in SIL
|
|
|
|
import util, pexpect, sys, time, math, shutil
|
|
|
|
sys.path.insert(0, 'pymavlink')
|
|
import mavutil
|
|
|
|
HOME_LOCATION='-35.362938,149.165085,650,270'
|
|
|
|
def arm_motors(mavproxy):
|
|
'''arm motors'''
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('rc 3 1000\n')
|
|
mavproxy.send('rc 4 2000\n')
|
|
mavproxy.expect('APM: ARMING MOTORS')
|
|
mavproxy.send('rc 4 1500\n')
|
|
print("MOTORS ARMED OK")
|
|
|
|
def disarm_motors(mavproxy):
|
|
'''disarm motors'''
|
|
mavproxy.send('rc 3 1000\n')
|
|
mavproxy.send('rc 4 1000\n')
|
|
mavproxy.expect('APM: DISARMING MOTORS')
|
|
mavproxy.send('rc 4 1500\n')
|
|
print("MOTORS DISARMED OK")
|
|
|
|
|
|
def takeoff(mavproxy, mav):
|
|
'''takeoff get to 30m altitude'''
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('rc 3 1500\n')
|
|
mavproxy.send('status\n')
|
|
m = mav.recv_match(type='VFR_HUD', condition='VFR_HUD.alt>=30', blocking=True)
|
|
print("Altitude %u" % m.alt)
|
|
print("TAKEOFF COMPLETE")
|
|
|
|
|
|
def loiter(mavproxy, mav, maxaltchange=10, holdtime=10, timeout=60):
|
|
'''hold loiter position'''
|
|
mavproxy.send('switch 5\n') # loiter mode
|
|
mavproxy.expect('LOITER>')
|
|
mavproxy.send('status\n')
|
|
mavproxy.expect('>')
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
start_altitude = m.alt
|
|
tstart = time.time()
|
|
tholdstart = time.time()
|
|
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
print("Altitude %u" % m.alt)
|
|
if math.fabs(m.alt - start_altitude) > maxaltchange:
|
|
tholdstart = time.time()
|
|
if time.time() - tholdstart > holdtime:
|
|
print("Loiter OK for %u seconds" % holdtime)
|
|
return True
|
|
print("Loiter FAILED")
|
|
return False
|
|
|
|
|
|
def land(mavproxy, mav, timeout=60):
|
|
'''land the quad'''
|
|
mavproxy.send('switch 6\n')
|
|
mavproxy.expect('STABILIZE>')
|
|
mavproxy.send('status\n')
|
|
mavproxy.expect('>')
|
|
mavproxy.send('rc 3 1300\n')
|
|
tstart = time.time()
|
|
while time.time() < tstart + timeout:
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
print("Altitude %u" % m.alt)
|
|
if m.alt <= 0:
|
|
print("LANDED OK")
|
|
return True
|
|
print("LANDING FAILED")
|
|
return False
|
|
|
|
|
|
|
|
def setup_rc(mavproxy):
|
|
'''setup RC override control'''
|
|
for chan in range(1,9):
|
|
mavproxy.send('rc %u 1500\n' % chan)
|
|
# zero throttle
|
|
mavproxy.send('rc 3 1000\n')
|
|
|
|
|
|
def fly_ArduCopter():
|
|
'''fly ArduCopter in SIL'''
|
|
util.rmfile('eeprom.bin')
|
|
sil = util.start_SIL('ArduCopter')
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter')
|
|
mavproxy.expect('Please Run Setup')
|
|
# we need to restart it after eeprom erase
|
|
mavproxy.close()
|
|
sil.close()
|
|
sil = util.start_SIL('ArduCopter')
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:14550 --quadcopter')
|
|
mavproxy.expect('Logging to (\S+)')
|
|
logfile = mavproxy.match.group(1)
|
|
print("LOGFILE %s" % logfile)
|
|
mavproxy.expect("Ready to FLY")
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
# setup test parameters
|
|
mavproxy.send("param load autotest/ArduCopter.parm\n")
|
|
mavproxy.expect('Loaded [0-9]+ parameters')
|
|
|
|
# start hil_quad.py
|
|
hquad = pexpect.spawn('HILTest/hil_quad.py --fgout=192.168.2.15:9123 --home=%s' % HOME_LOCATION,
|
|
logfile=sys.stdout, timeout=10)
|
|
hquad.expect('Starting at')
|
|
|
|
# get a mavlink connection going
|
|
mav = mavutil.mavlink_connection('127.0.0.1:14550', robust_parsing=True)
|
|
|
|
failed = False
|
|
try:
|
|
mav.wait_heartbeat()
|
|
mav.recv_match(type='GPS_RAW')
|
|
setup_rc(mavproxy)
|
|
arm_motors(mavproxy)
|
|
takeoff(mavproxy, mav)
|
|
loiter(mavproxy, mav)
|
|
land(mavproxy, mav)
|
|
disarm_motors(mavproxy)
|
|
except Exception, e:
|
|
failed = True
|
|
|
|
mavproxy.close()
|
|
sil.close()
|
|
hquad.close()
|
|
|
|
shutil.copy(logfile, "buildlogs/ArduCopter-test.mavlog")
|
|
util.run_cmd("pymavlink/examples/mavtogpx.py buildlogs/ArduCopter-test.mavlog")
|
|
util.run_cmd("bin/gpxtokml buildlogs/ArduCopter-test.mavlog.gpx")
|
|
|
|
if failed:
|
|
print("FAILED: %s" % e)
|
|
sys.exit(1)
|
|
|