2011-10-30 23:50:34 -03:00
|
|
|
# fly ArduCopter in SIL
|
|
|
|
|
2011-10-31 04:29:52 -03:00
|
|
|
import util, pexpect, sys, time, math, shutil, os
|
2011-11-12 05:01:58 -04:00
|
|
|
from common import *
|
2011-12-02 16:55:03 -04:00
|
|
|
import mavutil, mavwp, random
|
2011-10-31 04:29:52 -03:00
|
|
|
|
|
|
|
# get location of scripts
|
|
|
|
testdir=os.path.dirname(os.path.realpath(__file__))
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2012-01-04 07:03:37 -04:00
|
|
|
FRAME='octa'
|
|
|
|
TARGET='sitl-octa'
|
2011-11-25 23:45:18 -04:00
|
|
|
HOME=location(-35.362938,149.165085,584,270)
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-11-01 20:26:24 -03:00
|
|
|
homeloc = None
|
2011-11-13 04:24:25 -04:00
|
|
|
num_wp = 0
|
2011-11-01 20:26:24 -03:00
|
|
|
|
2011-12-12 07:07:53 -04:00
|
|
|
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
|
|
|
|
|
2011-11-13 06:14:58 -04:00
|
|
|
def arm_motors(mavproxy, mav):
|
2011-10-30 23:50:34 -03:00
|
|
|
'''arm motors'''
|
2011-11-09 05:27:36 -04:00
|
|
|
print("Arming motors")
|
2011-10-30 23:50:34 -03:00
|
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
2011-11-13 06:20:10 -04:00
|
|
|
wait_mode(mav, 'STABILIZE')
|
2011-10-30 23:50:34 -03:00
|
|
|
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")
|
2011-11-09 22:12:43 -04:00
|
|
|
return True
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-11-13 06:14:58 -04:00
|
|
|
def disarm_motors(mavproxy, mav):
|
2011-10-30 23:50:34 -03:00
|
|
|
'''disarm motors'''
|
2011-11-09 05:27:36 -04:00
|
|
|
print("Disarming motors")
|
|
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
2011-10-30 23:50:34 -03:00
|
|
|
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")
|
2011-11-09 22:12:43 -04:00
|
|
|
return True
|
2011-11-09 01:43:18 -04:00
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
def takeoff(mavproxy, mav, alt_min = 30):
|
2011-10-30 23:50:34 -03:00
|
|
|
'''takeoff get to 30m altitude'''
|
|
|
|
mavproxy.send('switch 6\n') # stabilize mode
|
2011-11-13 06:20:10 -04:00
|
|
|
wait_mode(mav, 'STABILIZE')
|
2011-10-30 23:50:34 -03:00
|
|
|
mavproxy.send('rc 3 1500\n')
|
2011-12-16 00:39:05 -04:00
|
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
if (m.alt < alt_min):
|
|
|
|
wait_altitude(mav, alt_min, (alt_min + 5))
|
2011-12-23 00:33:49 -04:00
|
|
|
mavproxy.send('rc 3 1430\n')
|
2011-10-30 23:50:34 -03:00
|
|
|
print("TAKEOFF COMPLETE")
|
2011-11-09 22:12:43 -04:00
|
|
|
return True
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-12-29 02:46:43 -04:00
|
|
|
def loiter(mavproxy, mav, holdtime=60, maxaltchange=20):
|
2011-10-30 23:50:34 -03:00
|
|
|
'''hold loiter position'''
|
2011-11-12 07:12:38 -04:00
|
|
|
mavproxy.send('switch 5\n') # loiter mode
|
2011-11-13 06:20:10 -04:00
|
|
|
wait_mode(mav, 'LOITER')
|
2011-10-30 23:50:34 -03:00
|
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
|
|
|
start_altitude = m.alt
|
2011-12-16 00:39:05 -04:00
|
|
|
start = current_location(mav)
|
2011-10-30 23:50:34 -03:00
|
|
|
tstart = time.time()
|
|
|
|
tholdstart = time.time()
|
|
|
|
print("Holding loiter at %u meters for %u seconds" % (start_altitude, holdtime))
|
2011-12-29 02:46:43 -04:00
|
|
|
while time.time() < tstart + holdtime:
|
2011-10-30 23:50:34 -03:00
|
|
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
2011-12-16 00:39:05 -04:00
|
|
|
pos = current_location(mav)
|
|
|
|
delta = get_distance(start, pos)
|
|
|
|
print("Loiter Dist: %.2fm, alt:%u" % (delta, m.alt))
|
2011-10-30 23:50:34 -03:00
|
|
|
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
|
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
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
|
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-10-31 04:29:52 -03:00
|
|
|
def fly_square(mavproxy, mav, side=50, timeout=120):
|
|
|
|
'''fly a square, flying N then E'''
|
|
|
|
mavproxy.send('switch 6\n')
|
2011-11-13 06:20:10 -04:00
|
|
|
wait_mode(mav, 'STABILIZE')
|
2011-10-31 04:29:52 -03:00
|
|
|
tstart = time.time()
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = False
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("Save WP 1 and 2")
|
2011-11-13 04:24:25 -04:00
|
|
|
save_wp(mavproxy, mav)
|
|
|
|
|
|
|
|
print("turn")
|
2011-10-31 04:29:52 -03:00
|
|
|
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')
|
2011-11-09 22:12:43 -04:00
|
|
|
if not wait_distance(mav, side):
|
|
|
|
failed = True
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.send('rc 2 1500\n')
|
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("Save WP 3")
|
2011-11-13 04:24:25 -04:00
|
|
|
save_wp(mavproxy, mav)
|
|
|
|
|
2011-10-31 04:29:52 -03:00
|
|
|
print("Going east %u meters" % side)
|
|
|
|
mavproxy.send('rc 1 1610\n')
|
2011-11-09 22:12:43 -04:00
|
|
|
if not wait_distance(mav, side):
|
|
|
|
failed = True
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.send('rc 1 1500\n')
|
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("Save WP 4")
|
2011-11-13 04:24:25 -04:00
|
|
|
save_wp(mavproxy, mav)
|
|
|
|
|
2011-10-31 04:29:52 -03:00
|
|
|
print("Going south %u meters" % side)
|
|
|
|
mavproxy.send('rc 2 1610\n')
|
2011-11-09 22:12:43 -04:00
|
|
|
if not wait_distance(mav, side):
|
|
|
|
failed = True
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.send('rc 2 1500\n')
|
2011-11-13 04:24:25 -04:00
|
|
|
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("Save WP 5")
|
2011-11-13 04:24:25 -04:00
|
|
|
save_wp(mavproxy, mav)
|
2011-10-31 04:29:52 -03:00
|
|
|
|
|
|
|
print("Going west %u meters" % side)
|
|
|
|
mavproxy.send('rc 1 1390\n')
|
2011-11-09 22:12:43 -04:00
|
|
|
if not wait_distance(mav, side):
|
|
|
|
failed = True
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.send('rc 1 1500\n')
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("Save WP 6")
|
2011-11-13 04:24:25 -04:00
|
|
|
save_wp(mavproxy, mav)
|
|
|
|
|
2011-11-09 22:12:43 -04:00
|
|
|
return not failed
|
2011-10-31 04:29:52 -03:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
def fly_simple(mavproxy, mav, side=60, timeout=120):
|
|
|
|
'''fly a square, flying N then E'''
|
2011-10-30 23:50:34 -03:00
|
|
|
mavproxy.send('switch 6\n')
|
2011-11-13 06:20:10 -04:00
|
|
|
wait_mode(mav, 'STABILIZE')
|
2011-12-21 19:24:29 -04:00
|
|
|
mavproxy.send('rc 3 1430\n')
|
2011-12-16 00:39:05 -04:00
|
|
|
tstart = time.time()
|
|
|
|
failed = False
|
2011-10-31 04:29:52 -03:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
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')
|
2011-10-31 04:29:52 -03:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Going east for 30 seconds")
|
|
|
|
mavproxy.send('rc 1 1650\n')
|
2011-10-30 23:50:34 -03:00
|
|
|
tstart = time.time()
|
2011-12-16 00:39:05 -04:00
|
|
|
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')
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Going back %u meters" % side)
|
|
|
|
mavproxy.send('rc 2 1650\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')
|
|
|
|
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, 5)
|
|
|
|
print("LANDING: ok= %s" % ret)
|
2011-11-13 08:50:04 -04:00
|
|
|
return ret
|
|
|
|
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2011-11-13 04:24:25 -04:00
|
|
|
def circle(mavproxy, mav, maxaltchange=10, holdtime=90, timeout=35):
|
|
|
|
'''fly circle'''
|
|
|
|
print("FLY CIRCLE")
|
|
|
|
mavproxy.send('switch 1\n') # CIRCLE mode
|
2011-11-13 08:50:04 -04:00
|
|
|
wait_mode(mav, 'CIRCLE')
|
2011-11-13 04:24:25 -04:00
|
|
|
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)
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2011-11-13 04:24:25 -04:00
|
|
|
print("CIRCLE OK for %u seconds" % holdtime)
|
|
|
|
return True
|
|
|
|
|
|
|
|
|
|
|
|
def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
|
2011-11-01 08:31:01 -03:00
|
|
|
'''fly a mission from a file'''
|
2011-11-01 20:26:24 -03:00
|
|
|
global homeloc
|
2011-11-13 04:24:25 -04:00
|
|
|
global num_wp
|
2011-11-28 14:25:16 -04:00
|
|
|
print("test: Fly a mission from 1 to %u" % num_wp)
|
|
|
|
mavproxy.send('wp set 1\n')
|
2011-11-13 04:24:25 -04:00
|
|
|
mavproxy.send('switch 4\n') # auto mode
|
2011-11-13 08:50:04 -04:00
|
|
|
wait_mode(mav, 'AUTO')
|
2011-11-13 08:59:20 -04:00
|
|
|
#wait_altitude(mav, 30, 40)
|
2012-01-04 13:52:25 -04:00
|
|
|
ret = wait_waypoint(mav, 0, num_wp, timeout=500)
|
2011-11-14 02:56:33 -04:00
|
|
|
print("test: MISSION COMPLETE: passed=%s" % ret)
|
2011-11-13 08:50:04 -04:00
|
|
|
# wait here until ready
|
|
|
|
mavproxy.send('switch 5\n')
|
|
|
|
wait_mode(mav, 'LOITER')
|
|
|
|
|
|
|
|
return ret
|
2011-11-13 04:24:25 -04:00
|
|
|
|
|
|
|
#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
|
|
|
|
|
2011-11-13 08:50:04 -04:00
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
def load_mission_from_file(mavproxy, mav, filename):
|
2011-11-13 04:24:25 -04:00
|
|
|
'''load a mission from a file'''
|
|
|
|
global num_wp
|
2011-11-14 02:56:33 -04:00
|
|
|
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
|
2011-11-01 08:31:01 -03:00
|
|
|
mavproxy.send('wp load %s\n' % filename)
|
|
|
|
mavproxy.expect('flight plan received')
|
|
|
|
mavproxy.send('wp list\n')
|
|
|
|
mavproxy.expect('Requesting [0-9]+ waypoints')
|
2011-11-14 02:56:33 -04:00
|
|
|
return True
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
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)
|
2011-11-13 08:50:04 -04:00
|
|
|
return True
|
2011-10-30 23:50:34 -03:00
|
|
|
|
|
|
|
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')
|
|
|
|
|
|
|
|
|
2011-11-10 17:03:27 -04:00
|
|
|
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
|
|
|
|
'''
|
2011-12-02 00:21:15 -04:00
|
|
|
global homeloc
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2012-01-04 07:03:37 -04:00
|
|
|
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)
|
2012-01-04 06:49:19 -04:00
|
|
|
sim_cmd += ' --wind=6,45,.3'
|
2011-11-25 23:45:18 -04:00
|
|
|
if viewerip:
|
2012-01-04 06:49:19 -04:00
|
|
|
sim_cmd += ' --fgout=%s:5503' % viewerip
|
2011-11-25 23:45:18 -04:00
|
|
|
|
2011-11-07 07:55:21 -04:00
|
|
|
sil = util.start_SIL('ArduCopter', wipe=True)
|
2011-11-25 23:45:18 -04:00
|
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
|
|
|
|
|
|
|
# setup test parameters
|
2011-12-02 16:55:03 -04:00
|
|
|
mavproxy.send('param set SYSID_THISMAV %u\n' % random.randint(100, 200))
|
2011-10-31 04:29:52 -03:00
|
|
|
mavproxy.send("param load %s/ArduCopter.parm\n" % testdir)
|
|
|
|
mavproxy.expect('Loaded [0-9]+ parameters')
|
|
|
|
|
|
|
|
# reboot with new parameters
|
2011-11-09 00:45:18 -04:00
|
|
|
util.pexpect_close(mavproxy)
|
|
|
|
util.pexpect_close(sil)
|
2011-11-28 14:25:16 -04:00
|
|
|
|
2011-11-25 23:45:18 -04:00
|
|
|
sil = util.start_SIL('ArduCopter', height=HOME.alt)
|
2012-01-04 06:49:19 -04:00
|
|
|
sim = pexpect.spawn(sim_cmd, logfile=sys.stdout, timeout=10)
|
|
|
|
sim.delaybeforesend = 0
|
|
|
|
util.pexpect_autoclose(sim)
|
2011-12-12 08:07:46 -04:00
|
|
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
|
2011-11-10 17:03:27 -04:00
|
|
|
if viewerip:
|
|
|
|
options += ' --out=%s:14550' % viewerip
|
|
|
|
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
|
2011-10-30 23:50:34 -03:00
|
|
|
mavproxy.expect('Logging to (\S+)')
|
|
|
|
logfile = mavproxy.match.group(1)
|
|
|
|
print("LOGFILE %s" % logfile)
|
2011-11-09 05:27:36 -04:00
|
|
|
|
|
|
|
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.mavlog")
|
|
|
|
print("buildlog=%s" % buildlog)
|
|
|
|
if os.path.exists(buildlog):
|
|
|
|
os.unlink(buildlog)
|
|
|
|
os.link(logfile, buildlog)
|
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
mavproxy.expect('Received [0-9]+ parameters')
|
2011-11-25 23:45:18 -04:00
|
|
|
mavproxy.expect("Ready to FLY")
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-11-01 08:31:01 -03:00
|
|
|
util.expect_setup_callback(mavproxy, expect_callback)
|
|
|
|
|
2011-12-02 00:21:15 -04:00
|
|
|
expect_list_clear()
|
2012-01-04 06:49:19 -04:00
|
|
|
expect_list_extend([sim, sil, mavproxy])
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
# get a mavlink connection going
|
2011-11-09 05:27:36 -04:00
|
|
|
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
|
2011-11-01 08:31:01 -03:00
|
|
|
mav.message_hooks.append(message_hook)
|
2011-11-27 22:52:54 -04:00
|
|
|
mav.idle_hooks.append(idle_hook)
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
failed = False
|
2011-11-12 07:13:17 -04:00
|
|
|
e = 'None'
|
2011-10-30 23:50:34 -03:00
|
|
|
try:
|
|
|
|
mav.wait_heartbeat()
|
2011-11-01 20:26:24 -03:00
|
|
|
mav.recv_match(type='GPS_RAW', blocking=True)
|
2011-10-30 23:50:34 -03:00
|
|
|
setup_rc(mavproxy)
|
2011-11-01 20:26:24 -03:00
|
|
|
homeloc = current_location(mav)
|
2011-12-12 07:07:53 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Calibrate level")
|
2011-12-12 07:07:53 -04:00
|
|
|
if not calibrate_level(mavproxy, mav):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("calibrate_level failed")
|
2011-12-12 07:07:53 -04:00
|
|
|
failed = True
|
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Arm motors")
|
2011-11-13 06:14:58 -04:00
|
|
|
if not arm_motors(mavproxy, mav):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("arm_motors failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Takeoff")
|
|
|
|
if not takeoff(mavproxy, mav, 10):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("takeoff failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-29 02:46:43 -04:00
|
|
|
# Loiter for 30 seconds
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Loiter for 30 seconds")
|
2011-12-29 02:46:43 -04:00
|
|
|
if not loiter(mavproxy, mav, 30):
|
2011-12-16 00:39:05 -04:00
|
|
|
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
|
|
|
|
|
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
print("# Fly A square")
|
2011-11-09 22:12:43 -04:00
|
|
|
if not fly_square(mavproxy, mav):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("fly_square failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
print("# Land")
|
|
|
|
if not land(mavproxy, mav):
|
|
|
|
print("land failed")
|
|
|
|
failed = True
|
|
|
|
|
|
|
|
print("Save landing WP")
|
|
|
|
save_wp(mavproxy, mav)
|
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
# 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")):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("save_mission_to_file failed")
|
2011-11-14 02:56:33 -04:00
|
|
|
failed = True
|
|
|
|
|
|
|
|
# save the stored mission
|
|
|
|
print("# Fly CH 7 saved mission")
|
2011-11-13 04:24:25 -04:00
|
|
|
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("fly_mission failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-12-16 00:39:05 -04:00
|
|
|
#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
|
|
|
|
|
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
print("# Upload mission1")
|
2011-11-19 18:03:26 -04:00
|
|
|
if not upload_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("upload_mission_from_file failed")
|
2011-11-14 02:56:33 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
# this grabs our mission count
|
|
|
|
print("# store mission1 locally")
|
2011-11-19 18:03:26 -04:00
|
|
|
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "mission2.txt")):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("load_mission_from_file failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-11-19 18:03:26 -04:00
|
|
|
print("# Fly mission 2")
|
2011-11-13 04:24:25 -04:00
|
|
|
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("fly_mission failed")
|
2011-11-13 04:24:25 -04:00
|
|
|
failed = True
|
2011-11-13 08:50:04 -04:00
|
|
|
else:
|
2011-11-19 18:03:26 -04:00
|
|
|
print("Flew mission2 OK")
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
print("# Land")
|
2011-11-09 22:12:43 -04:00
|
|
|
if not land(mavproxy, mav):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("land failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-13 04:24:25 -04:00
|
|
|
|
2011-11-14 02:56:33 -04:00
|
|
|
print("# disarm motors")
|
2011-11-13 06:14:58 -04:00
|
|
|
if not disarm_motors(mavproxy, mav):
|
2011-12-12 08:08:20 -04:00
|
|
|
print("disarm_motors failed")
|
2011-11-09 22:12:43 -04:00
|
|
|
failed = True
|
2011-11-01 08:31:01 -03:00
|
|
|
except pexpect.TIMEOUT, e:
|
2011-10-30 23:50:34 -03:00
|
|
|
failed = True
|
|
|
|
|
2011-12-02 00:21:15 -04:00
|
|
|
mav.close()
|
2011-11-09 00:45:18 -04:00
|
|
|
util.pexpect_close(mavproxy)
|
|
|
|
util.pexpect_close(sil)
|
2012-01-04 06:49:19 -04:00
|
|
|
util.pexpect_close(sim)
|
2011-10-30 23:50:34 -03:00
|
|
|
|
2011-11-03 19:59:33 -03:00
|
|
|
if os.path.exists('ArduCopter-valgrind.log'):
|
2011-11-03 20:05:40 -03:00
|
|
|
os.chmod('ArduCopter-valgrind.log', 0644)
|
2011-11-03 19:59:33 -03:00
|
|
|
shutil.copy("ArduCopter-valgrind.log", util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
|
2011-11-01 08:31:01 -03:00
|
|
|
|
2011-10-30 23:50:34 -03:00
|
|
|
if failed:
|
|
|
|
print("FAILED: %s" % e)
|
2011-11-09 01:15:53 -04:00
|
|
|
return False
|
|
|
|
return True
|
2011-12-16 00:39:05 -04:00
|
|
|
|
2012-01-04 13:52:25 -04:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#! 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
|