Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/Tools/autotest/arducopter.py

576 lines
17 KiB
Python
Raw Blame History

# fly ArduCopter in SIL
import util, pexpect, sys, time, math, shutil, os
from common import *
import mavutil, mavwp, random
# get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__))
FRAME='octa'
TARGET='sitl-octa'
HOME=location(-35.362938,149.165085,584,270)
homeloc = None
num_wp = 0
def calibrate_level(mavproxy, mav):
'''init the accelerometers'''
print("Initialising accelerometers")
MAV_ACTION_CALIBRATE_ACC = 19
mav.mav.action_send(mav.target_system, mav.target_component, MAV_ACTION_CALIBRATE_ACC)
mavproxy.expect('APM: action received')
return True
def arm_motors(mavproxy, mav):
'''arm motors'''
print("Arming motors")
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, '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")
return True
def disarm_motors(mavproxy, mav):
'''disarm motors'''
print("Disarming motors")
mavproxy.send('switch 6\n') # stabilize mode
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")
return True
def takeoff(mavproxy, mav, alt_min = 30):
'''takeoff get to 30m altitude'''
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1510\n')
m = mav.recv_match(type='VFR_HUD', blocking=True)
if (m.alt < alt_min):
wait_altitude(mav, alt_min, (alt_min + 5))
mavproxy.send('rc 3 1430\n')
print("TAKEOFF COMPLETE")
return True
def loiter(mavproxy, mav, holdtime=60, maxaltchange=20):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
start = current_location(mav)
tstart = time.time()
tholdstart = time.time()
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
while time.time() < tstart + holdtime:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = current_location(mav)
delta = get_distance(start, pos)
print("Loiter Dist: %.2fm, alt:%u" % (delta, 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 change_alt(mavproxy, mav, alt_min):
'''change altitude'''
m = mav.recv_match(type='VFR_HUD', blocking=True)
if(m.alt < alt_min):
print("Rise to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 1800\n')
wait_altitude(mav, alt_min, (alt_min + 5))
else:
print("Lower to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 1100\n')
wait_altitude(mav, (alt_min -5), alt_min)
mavproxy.send('rc 3 1430\n')
return True
def fly_square(mavproxy, mav, side=50, timeout=120):
'''fly a square, flying N then E'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
tstart = time.time()
failed = False
print("Save WP 1 and 2")
save_wp(mavproxy, mav)
print("turn")
mavproxy.send('rc 3 1430\n')
mavproxy.send('rc 4 1610\n')
if not wait_heading(mav, 0):
return False
mavproxy.send('rc 4 1500\n')
print("Going north %u meters" % side)
mavproxy.send('rc 2 1390\n')
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 2 1500\n')
print("Save WP 3")
save_wp(mavproxy, mav)
print("Going east %u meters" % side)
mavproxy.send('rc 1 1610\n')
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 1 1500\n')
print("Save WP 4")
save_wp(mavproxy, mav)
print("Going south %u meters" % side)
mavproxy.send('rc 2 1610\n')
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 2 1500\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
print("Save WP 5")
save_wp(mavproxy, mav)
print("Going west %u meters" % side)
mavproxy.send('rc 1 1390\n')
if not wait_distance(mav, side):
failed = True
mavproxy.send('rc 1 1500\n')
print("Save WP 6")
save_wp(mavproxy, mav)
return not failed
def fly_RTL(mavproxy, mav, side=60):
'''Fly, return, land'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1430\n')
failed = False
print("# Going forward %u meters" % side)
mavproxy.send('rc 2 1350\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
print("# Enter RTL")
mavproxy.send('switch 3\n')
tstart = time.time()
while time.time() < tstart + 200:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = current_location(mav)
#delta = get_distance(start, pos)
print("Alt: %u" % m.alt)
if(m.alt <= 1):
return True
return True
def fly_failsafe(mavproxy, mav, side=60):
'''Fly, Failsafe, return, land'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1430\n')
failed = False
print("# Going forward %u meters" % side)
mavproxy.send('rc 2 1350\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
print("# Enter Failsafe")
mavproxy.send('rc 3 900\n')
tstart = time.time()
while time.time() < tstart + 120:
m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = current_location(mav)
#delta = get_distance(start, pos)
print("Alt: %u" % m.alt)
if(m.alt <= 1):
mavproxy.send('rc 3 1100\n')
return True
return True
def fly_simple(mavproxy, mav, side=60, timeout=120):
'''fly Simple, flying N then E'''
mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1440\n')
tstart = time.time()
failed = False
print("# Going forward %u meters" % side)
mavproxy.send('rc 2 1390\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
print("# Going east for 30 seconds")
mavproxy.send('rc 1 1610\n')
tstart = time.time()
while time.time() < (tstart + 30):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (time.time() - tstart)
#print("%u" % delta)
mavproxy.send('rc 1 1500\n')
print("# Going back %u meters" % side)
mavproxy.send('rc 2 1610\n')
if not wait_distance(mav, side, 5, 60):
failed = True
mavproxy.send('rc 2 1500\n')
#restore to default
mavproxy.send('param set SIMPLE 0\n')
mavproxy.send('rc 3 1430\n')
return not failed
def land(mavproxy, mav, timeout=60):
'''land the quad'''
print("STARTING LANDING")
mavproxy.send('switch 2\n')
wait_mode(mav, 'LAND')
print("Entered Landing Mode")
ret = wait_altitude(mav, -5, 1)
print("LANDING: ok= %s" % ret)
return ret
def circle(mavproxy, mav, maxaltchange=10, holdtime=90, timeout=35):
'''fly circle'''
print("FLY CIRCLE")
mavproxy.send('switch 1\n') # CIRCLE mode
wait_mode(mav, 'CIRCLE')
m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
tstart = time.time()
tholdstart = time.time()
print("Circle at %u meters for %u seconds" % (start_altitude, holdtime))
while time.time() < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("heading %u" % m.heading)
print("CIRCLE OK for %u seconds" % holdtime)
return True
def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file'''
global homeloc
global num_wp
print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO')
#wait_altitude(mav, 30, 40)
ret = wait_waypoint(mav, 0, num_wp, timeout=500)
print("test: MISSION COMPLETE: passed=%s" % ret)
# wait here until ready
mavproxy.send('switch 5\n')
wait_mode(mav, 'LOITER')
return ret
#if not wait_distance(mav, 30, timeout=120):
# return False
#if not wait_location(mav, homeloc, timeout=600, target_altitude=target_altitude, height_accuracy=height_accuracy):
# return False
def load_mission_from_file(mavproxy, mav, filename):
'''load a mission from a file'''
global num_wp
wploader = mavwp.MAVWPLoader()
wploader.load(filename)
num_wp = wploader.count()
print("loaded mission with %u waypoints" % num_wp)
return True
def upload_mission_from_file(mavproxy, mav, filename):
'''Upload a mission from a file to APM'''
global num_wp
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
return True
def save_mission_to_file(mavproxy, mav, filename):
global num_wp
mavproxy.send('wp save %s\n' % filename)
mavproxy.expect('Saved ([0-9]+) waypoints')
num_wp = int(mavproxy.match.group(1))
print("num_wp: %d" % num_wp)
return True
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(viewerip=None):
'''fly ArduCopter 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 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, HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sim_cmd += ' --wind=6,45,.3'
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/ArduCopter.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
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/ArduCopter-test.mavlog")
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()
mav.recv_match(type='GPS_RAW', blocking=True)
setup_rc(mavproxy)
homeloc = current_location(mav)
print("# Calibrate level")
if not calibrate_level(mavproxy, mav):
print("calibrate_level failed")
failed = True
print("# Arm motors")
if not arm_motors(mavproxy, mav):
print("arm_motors failed")
failed = True
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
print("# Test RTL")
if not fly_RTL(mavproxy, mav):
print("RTL failed")
failed = True
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
print("# Test Failsafe")
if not fly_failsafe(mavproxy, mav):
print("FS failed")
failed = True
print("# Takeoff")
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
# Loiter for 30 seconds
print("# Loiter for 45 seconds")
if not loiter(mavproxy, mav, 45):
print("loiter failed")
failed = True
print("# Change alt to 60m")
if not change_alt(mavproxy, mav, 60):
print("change_alt failed")
failed = True
print("# Change alt to 20m")
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
print("# Change alt to 20m")
if not change_alt(mavproxy, mav, 20):
print("change_alt failed")
failed = True
print("# Fly A square")
if not fly_square(mavproxy, mav):
print("fly_square failed")
failed = True
print("# Land")
if not land(mavproxy, mav):
print("land failed")
failed = True
print("Save landing WP")
save_wp(mavproxy, mav)
# save the stored mission
print("# Save out the C7 mission")
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
print("save_mission_to_file failed")
failed = True
# save the stored mission
print("# Fly CH 7 saved mission")
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
print("fly_mission failed")
failed = True
#set SIMPLE mode
mavproxy.send('param set SIMPLE 63\n')
if not takeoff(mavproxy, mav, 10):
print("takeoff failed")
failed = True
print("# Fly in SIMPLE mode")
if not fly_simple(mavproxy, mav):
print("fly_simple failed")
failed = True
print("# Upload mission1")
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.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, "mission2.txt")):
print("load_mission_from_file failed")
failed = True
print("# Fly mission 2")
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
print("fly_mission failed")
failed = True
else:
print("Flew mission2 OK")
print("# Land")
if not land(mavproxy, mav):
print("land failed")
failed = True
#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 os.path.exists('ArduCopter-valgrind.log'):
os.chmod('ArduCopter-valgrind.log', 0644)
shutil.copy("ArduCopter-valgrind.log", util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
if failed:
print("FAILED: %s" % e)
return False
return True
#! mavproxy.send('rc 2 1390\n')
#! #adjust till the rate is 0;
#!
#! mavproxy.send('rc 4 1610\n')
#! if not wait_heading(mav, 0):
#! return False
#! mavproxy.send('rc 4 1500\n')
#!
#! mavproxy.send('rc 2 1455\n')
#! #adjust till the rate is 0;
#! pitch_test = 1455
#! roll_test = 1500
#! old_lat = 0
#! old_lon = 0
#!
#! while(1):
#! pos = current_location(mav)
#! tmp = (pos.lat *10e7) - (old_lat *10e7)
#! print("tmp %d " % tmp)
#! if(tmp > 0 ):
#! print("higher tmp %d " % (tmp))
#! pitch_test += 1
#! if(tmp < 0 ):
#! print("lower tmp %d " % (tmp))
#! pitch_test -= 1
#! mavproxy.send('rc 2 %u\n' % math.floor(pitch_test))
#! old_lat = pos.lat
#! #old_lon = pos.lon
Reference in New Issue View Git Blame Copy Permalink