mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-27 10:13:57 -04:00
Tools: autotest: move common functionality to common.py
Tools: don't need to pass option on first mavproxy Tools: reformat common.py and add commun function Tools: use new common functions Tools: move functions from rover to commun and reorder Tools: add and use set_rc function with timeout Tools: fix style for pep8
This commit is contained in:
khancyr
Peter Barker
parent
da11241aa8
commit
fac89ed437
@ -4,60 +4,49 @@
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import shutil
|
||||
|
||||
import pexpect
|
||||
from pymavlink import mavutil
|
||||
|
||||
from common import *
|
||||
from pysim import util
|
||||
from pysim import vehicleinfo
|
||||
from pymavlink import mavutil, mavwp
|
||||
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
#################################################
|
||||
# CONFIG
|
||||
#################################################
|
||||
|
||||
# HOME=mavutil.location(-35.362938,149.165085,584,270)
|
||||
# HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
|
||||
HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
|
||||
homeloc = None
|
||||
num_wp = 0
|
||||
speedup_default = 10
|
||||
|
||||
|
||||
def arm_rover(mavproxy, mav):
|
||||
wait_ready_to_arm(mav);
|
||||
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
|
||||
progress("ROVER ARMED")
|
||||
return True
|
||||
|
||||
|
||||
def disarm_rover(mavproxy, mav):
|
||||
mavproxy.send('disarm\n')
|
||||
mavproxy.expect('DISARMED')
|
||||
|
||||
progress("ROVER DISARMED")
|
||||
return True
|
||||
|
||||
|
||||
##########################################################
|
||||
# TESTS DRIVE
|
||||
##########################################################
|
||||
def drive_left_circuit(mavproxy, mav):
|
||||
"""Drive a left circuit, 50m on a side."""
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'MANUAL')
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
|
||||
progress("Driving left circuit")
|
||||
# do 4 turns
|
||||
for i in range(0, 4):
|
||||
# hard left
|
||||
progress("Starting turn %u" % i)
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
set_rc(mavproxy, mav, 1, 1000)
|
||||
if not wait_heading(mav, 270 - (90*i), accuracy=10):
|
||||
return False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
progress("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 50, accuracy=7):
|
||||
return False
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
progress("Circuit complete")
|
||||
return True
|
||||
|
||||
@ -72,13 +61,9 @@ def drive_RTL(mavproxy, mav):
|
||||
return True
|
||||
|
||||
|
||||
def setup_rc(mavproxy):
|
||||
"""Setup RC override control."""
|
||||
for chan in [1, 2, 3, 4, 5, 6, 7]:
|
||||
mavproxy.send('rc %u 1500\n' % chan)
|
||||
mavproxy.send('rc 8 1800\n')
|
||||
|
||||
|
||||
#################################################
|
||||
# AUTOTEST ALL
|
||||
#################################################
|
||||
def drive_mission(mavproxy, mav, filename):
|
||||
"""Drive a mission from a file."""
|
||||
global homeloc
|
||||
@ -88,7 +73,7 @@ def drive_mission(mavproxy, mav, filename):
|
||||
mavproxy.send('wp list\n')
|
||||
mavproxy.expect('Requesting [0-9]+ waypoints')
|
||||
mavproxy.send('switch 4\n') # auto mode
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
wait_mode(mav, 'AUTO')
|
||||
if not wait_waypoint(mav, 1, 4, max_dist=5):
|
||||
return False
|
||||
@ -96,6 +81,7 @@ def drive_mission(mavproxy, mav, filename):
|
||||
progress("Mission OK")
|
||||
return True
|
||||
|
||||
|
||||
def do_get_banner(mavproxy, mav):
|
||||
mavproxy.send("long DO_SEND_BANNER 1\n")
|
||||
start = time.time()
|
||||
@ -111,28 +97,6 @@ def do_get_banner(mavproxy, mav):
|
||||
|
||||
return False
|
||||
|
||||
def do_get_autopilot_capabilities(mavproxy, mav):
|
||||
mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n")
|
||||
m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
progress("AUTOPILOT_VERSION not received")
|
||||
return False
|
||||
progress("AUTOPILOT_VERSION received")
|
||||
return True;
|
||||
|
||||
def do_set_mode_via_command_long(mavproxy, mav):
|
||||
base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
custom_mode = 4 # hold
|
||||
start = time.time()
|
||||
while time.time() - start < 5:
|
||||
mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode,custom_mode))
|
||||
m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
return False
|
||||
if m.custom_mode == custom_mode:
|
||||
return True
|
||||
time.sleep(0.1)
|
||||
return False
|
||||
|
||||
def drive_brake_get_stopping_distance(mavproxy, mav, speed):
|
||||
# measure our stopping distance:
|
||||
@ -146,7 +110,7 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
|
||||
set_parameter(mavproxy, 'ATC_ACCEL_MAX', 15)
|
||||
mavproxy.send("mode STEERING\n")
|
||||
wait_mode(mav, 'STEERING')
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
wait_groundspeed(mav, 15, 100)
|
||||
initial = mav.location()
|
||||
initial_time = time.time()
|
||||
@ -155,8 +119,8 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
|
||||
start = mav.location()
|
||||
if start != initial:
|
||||
break
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
wait_groundspeed(mav, 0, 0.2) # why do we not stop?!
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
wait_groundspeed(mav, 0, 0.2) # why do we not stop?!
|
||||
initial = mav.location()
|
||||
initial_time = time.time()
|
||||
while time.time() - initial_time < 2:
|
||||
@ -171,6 +135,7 @@ def drive_brake_get_stopping_distance(mavproxy, mav, speed):
|
||||
|
||||
return delta
|
||||
|
||||
|
||||
def drive_brake(mavproxy, mav):
|
||||
old_using_brake = get_parameter(mavproxy, 'ATC_BRAKE')
|
||||
old_cruise_speed = get_parameter(mavproxy, 'CRUISE_SPEED')
|
||||
@ -188,7 +153,7 @@ def drive_brake(mavproxy, mav):
|
||||
set_parameter(mavproxy, 'CRUISE_SPEED', old_cruise_speed)
|
||||
|
||||
delta = distance_without_brakes - distance_with_brakes
|
||||
if delta < distance_without_brakes*0.05: # 5% isn't asking for much
|
||||
if delta < distance_without_brakes * 0.05: # 5% isn't asking for much
|
||||
progress("Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)" % (distance_with_brakes, distance_without_brakes, delta))
|
||||
return False
|
||||
else:
|
||||
@ -196,8 +161,10 @@ def drive_brake(mavproxy, mav):
|
||||
|
||||
return True
|
||||
|
||||
|
||||
vinfo = vehicleinfo.VehicleInfo()
|
||||
|
||||
|
||||
def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
|
||||
"""Drive APMrover2 in SITL.
|
||||
|
||||
@ -217,7 +184,7 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa
|
||||
|
||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
|
||||
mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
|
||||
mavproxy = util.start_MAVProxy_SITL('APMrover2')
|
||||
|
||||
progress("WAITING FOR PARAMETERS")
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
@ -276,21 +243,32 @@ def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fa
|
||||
progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
|
||||
mav.wait_heartbeat()
|
||||
progress("Setting up RC parameters")
|
||||
setup_rc(mavproxy)
|
||||
set_rc_default(mavproxy)
|
||||
set_rc(mavproxy, mav, 8, 1800)
|
||||
progress("Waiting for GPS fix")
|
||||
mav.wait_gps_fix()
|
||||
homeloc = mav.location()
|
||||
progress("Home location: %s" % homeloc)
|
||||
if not arm_rover(mavproxy, mav):
|
||||
mavproxy.send('switch 6\n') # Manual mode
|
||||
wait_mode(mav, 'MANUAL')
|
||||
progress("Waiting reading for arm")
|
||||
wait_ready_to_arm(mav)
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
progress("Failed to ARM")
|
||||
failed = True
|
||||
progress("#")
|
||||
progress("########## Drive a square and save WPs with CH7 switch ##########")
|
||||
progress("#")
|
||||
# Drive a square in learning mode
|
||||
if not drive_mission(mavproxy, mav, os.path.join(testdir, "rover1.txt")):
|
||||
progress("Failed mission")
|
||||
failed = True
|
||||
|
||||
if not drive_brake(mavproxy, mav):
|
||||
progress("Failed brake")
|
||||
failed = True
|
||||
if not disarm_rover(mavproxy, mav):
|
||||
|
||||
if not disarm_vehicle(mavproxy, mav):
|
||||
progress("Failed to DISARM")
|
||||
failed = True
|
||||
|
||||
|
||||
@ -19,51 +19,32 @@ vinfo = vehicleinfo.VehicleInfo()
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
#################################################
|
||||
# CONFIG
|
||||
#################################################
|
||||
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
|
||||
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
|
||||
|
||||
homeloc = None
|
||||
num_wp = 0
|
||||
|
||||
|
||||
def hover(mavproxy, mav, hover_throttle=1500):
|
||||
mavproxy.send('rc 3 %u\n' % hover_throttle)
|
||||
return True
|
||||
|
||||
|
||||
def arm_motors(mavproxy, mav):
|
||||
"""Arm motors."""
|
||||
progress("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')
|
||||
mav.motors_armed_wait()
|
||||
progress("MOTORS ARMED OK")
|
||||
return True
|
||||
|
||||
|
||||
def disarm_motors(mavproxy, mav):
|
||||
"""Disarm motors."""
|
||||
progress("Disarming motors")
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
mavproxy.send('rc 4 1000\n')
|
||||
mavproxy.expect('APM: Disarming motors')
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
mav.motors_disarmed_wait()
|
||||
progress("MOTORS DISARMED OK")
|
||||
return True
|
||||
# Flight mode switch positions are set-up in arducopter.param to be
|
||||
# switch 1 = Circle
|
||||
# switch 2 = Land
|
||||
# switch 3 = RTL
|
||||
# switch 4 = Auto
|
||||
# switch 5 = Loiter
|
||||
# switch 6 = Stabilize
|
||||
|
||||
|
||||
#################################################
|
||||
# UTILITIES
|
||||
#################################################
|
||||
def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
|
||||
"""Takeoff get to 30m altitude."""
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 %u\n' % takeoff_throttle)
|
||||
set_rc(mavproxy, mav, 3, takeoff_throttle)
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
if (m.alt < alt_min):
|
||||
wait_altitude(mav, alt_min, (alt_min + 5))
|
||||
@ -72,6 +53,22 @@ def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
|
||||
return True
|
||||
|
||||
|
||||
def land(mavproxy, mav, timeout=60):
|
||||
"""Land the quad."""
|
||||
progress("STARTING LANDING")
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
progress("Entered Landing Mode")
|
||||
ret = wait_altitude(mav, -5, 1)
|
||||
progress("LANDING: ok= %s" % ret)
|
||||
return ret
|
||||
|
||||
|
||||
def hover(mavproxy, mav, hover_throttle=1500):
|
||||
set_rc(mavproxy, mav, 3, hover_throttle)
|
||||
return True
|
||||
|
||||
|
||||
# loiter - fly south west, then hold loiter within 5m position and altitude
|
||||
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
||||
"""Hold loiter position."""
|
||||
@ -80,16 +77,16 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
||||
|
||||
# first aim south east
|
||||
progress("turn south east")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 170):
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# fly south east 50m
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
if not wait_distance(mav, 50):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# wait for copter to slow moving
|
||||
if not wait_groundspeed(mav, 0, 2):
|
||||
@ -126,16 +123,19 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
if(m.alt < alt_min):
|
||||
progress("Rise to alt:%u from %u" % (alt_min, m.alt))
|
||||
mavproxy.send('rc 3 %u\n' % climb_throttle)
|
||||
set_rc(mavproxy, mav, 3, climb_throttle)
|
||||
wait_altitude(mav, alt_min, (alt_min + 5))
|
||||
else:
|
||||
progress("Lower to alt:%u from %u" % (alt_min, m.alt))
|
||||
mavproxy.send('rc 3 %u\n' % descend_throttle)
|
||||
set_rc(mavproxy, mav, 3, descend_throttle)
|
||||
wait_altitude(mav, (alt_min - 5), alt_min)
|
||||
hover(mavproxy, mav)
|
||||
return True
|
||||
|
||||
|
||||
#################################################
|
||||
# TESTS FLY
|
||||
#################################################
|
||||
# fly a square in stabilize mode
|
||||
def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
"""Fly a square, flying N then E ."""
|
||||
@ -143,10 +143,10 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
success = True
|
||||
|
||||
# ensure all sticks in the middle
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# switch to loiter mode temporarily to stop us from rising
|
||||
mavproxy.send('switch 5\n')
|
||||
@ -154,11 +154,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
|
||||
# first aim north
|
||||
progress("turn right towards north")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 10):
|
||||
progress("Failed to reach heading")
|
||||
success = False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
mav.recv_match(condition='RC_CHANNELS.chan4_raw==1500', blocking=True)
|
||||
|
||||
# save bottom left corner of box as waypoint
|
||||
@ -166,17 +166,17 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
save_wp(mavproxy, mav)
|
||||
|
||||
# switch back to stabilize mode
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
|
||||
# pitch forward to fly north
|
||||
progress("Going north %u meters" % side)
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
set_rc(mavproxy, mav, 2, 1300)
|
||||
if not wait_distance(mav, side):
|
||||
progress("Failed to reach distance of %u" % side)
|
||||
success = False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# save top left corner of square as waypoint
|
||||
progress("Save WP 3")
|
||||
@ -184,11 +184,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
|
||||
# roll right to fly east
|
||||
progress("Going east %u meters" % side)
|
||||
mavproxy.send('rc 1 1700\n')
|
||||
set_rc(mavproxy, mav, 1, 1700)
|
||||
if not wait_distance(mav, side):
|
||||
progress("Failed to reach distance of %u" % side)
|
||||
success = False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
|
||||
# save top right corner of square as waypoint
|
||||
progress("Save WP 4")
|
||||
@ -196,11 +196,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
|
||||
# pitch back to fly south
|
||||
progress("Going south %u meters" % side)
|
||||
mavproxy.send('rc 2 1700\n')
|
||||
set_rc(mavproxy, mav, 2, 1700)
|
||||
if not wait_distance(mav, side):
|
||||
progress("Failed to reach distance of %u" % side)
|
||||
success = False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# save bottom right corner of square as waypoint
|
||||
progress("Save WP 5")
|
||||
@ -208,11 +208,11 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
|
||||
# roll left to fly west
|
||||
progress("Going west %u meters" % side)
|
||||
mavproxy.send('rc 1 1300\n')
|
||||
set_rc(mavproxy, mav, 1, 1300)
|
||||
if not wait_distance(mav, side):
|
||||
progress("Failed to reach distance of %u" % side)
|
||||
success = False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
|
||||
# save bottom left corner of square (should be near home) as waypoint
|
||||
progress("Save WP 6")
|
||||
@ -222,7 +222,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||
progress("Descend to 10m in Loiter")
|
||||
mavproxy.send('switch 5\n') # loiter mode
|
||||
wait_mode(mav, 'LOITER')
|
||||
mavproxy.send('rc 3 1300\n')
|
||||
set_rc(mavproxy, mav, 3, 1300)
|
||||
time_left = timeout - (get_sim_time(mav) - tstart)
|
||||
progress("timeleft = %u" % time_left)
|
||||
if time_left < 20:
|
||||
@ -259,13 +259,13 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||
|
||||
# first aim east
|
||||
progress("turn east")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 135):
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# raise throttle slightly to avoid hitting the ground
|
||||
mavproxy.send('rc 3 1600\n')
|
||||
set_rc(mavproxy, mav, 3, 1600)
|
||||
|
||||
# switch to stabilize mode
|
||||
mavproxy.send('switch 6\n')
|
||||
@ -274,14 +274,14 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||
|
||||
# fly east 60 meters
|
||||
progress("# Going forward %u meters" % side)
|
||||
mavproxy.send('rc 2 1350\n')
|
||||
set_rc(mavproxy, mav, 2, 1350)
|
||||
if not wait_distance(mav, side, 5, 60):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# pull throttle low
|
||||
progress("# Enter Failsafe")
|
||||
mavproxy.send('rc 3 900\n')
|
||||
set_rc(mavproxy, mav, 3, 900)
|
||||
|
||||
tstart = get_sim_time(mav)
|
||||
while get_sim_time(mav) < tstart + timeout:
|
||||
@ -292,7 +292,7 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||
# check if we've reached home
|
||||
if m.alt <= 1 and home_distance < 10:
|
||||
# reduce throttle
|
||||
mavproxy.send('rc 3 1100\n')
|
||||
set_rc(mavproxy, mav, 3, 1100)
|
||||
# switch back to stabilize
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
@ -301,13 +301,14 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||
progress("Reached failsafe home OK")
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
if not arm_motors(mavproxy, mav):
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
progress("Failed to re-arm")
|
||||
return False
|
||||
return True
|
||||
progress("Failed to land on failsafe RTL - timed out after %u seconds" % timeout)
|
||||
# reduce throttle
|
||||
mavproxy.send('rc 3 1100\n')
|
||||
set_rc(mavproxy, mav, 3, 1100)
|
||||
# switch back to stabilize mode
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
@ -356,16 +357,16 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
|
||||
|
||||
# first south
|
||||
progress("turn south")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 180):
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# fly west 80m
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
if not wait_distance(mav, 80):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# wait for copter to slow moving
|
||||
if not wait_groundspeed(mav, 0, 2):
|
||||
@ -419,14 +420,14 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||
|
||||
# first east
|
||||
progress("turn east")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 160):
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# fly forward (east) at least 20m
|
||||
pitching_forward = True
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
if not wait_distance(mav, 20):
|
||||
return False
|
||||
|
||||
@ -440,12 +441,12 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||
# recenter pitch sticks once we reach home so we don't fly off again
|
||||
if pitching_forward and home_distance < 10:
|
||||
pitching_forward = False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
# disable fence
|
||||
mavproxy.send('param set FENCE_ENABLE 0\n')
|
||||
if m.alt <= 1 and home_distance < 10:
|
||||
# reduce throttle
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
# switch mode to stabilize
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
@ -454,12 +455,14 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||
progress("Reached home OK")
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
if not arm_motors(mavproxy, mav):
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
mavproxy.send('arm uncheck all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
progress("Failed to re-arm")
|
||||
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
return False
|
||||
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
mavproxy.send('arm check all\n') # remove if we ever clear battery failsafe flag on disarm
|
||||
progress("Reached home OK")
|
||||
return True
|
||||
|
||||
# disable fence, enable avoidance
|
||||
@ -467,7 +470,7 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||
mavproxy.send('param set AVOID_ENABLE 1\n')
|
||||
|
||||
# reduce throttle
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
# switch mode to stabilize
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
@ -477,17 +480,6 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||
return False
|
||||
|
||||
|
||||
def show_gps_and_sim_positions(mavproxy, on_off):
|
||||
if on_off is True:
|
||||
# turn on simulator display of gps and actual position
|
||||
mavproxy.send('map set showgpspos 1\n')
|
||||
mavproxy.send('map set showsimpos 1\n')
|
||||
else:
|
||||
# turn off simulator display of gps and actual position
|
||||
mavproxy.send('map set showgpspos 0\n')
|
||||
mavproxy.send('map set showsimpos 0\n')
|
||||
|
||||
|
||||
def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
|
||||
"""fly_gps_glitch_loiter_test.
|
||||
|
||||
@ -509,20 +501,20 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
|
||||
|
||||
# turn south east
|
||||
progress("turn south east")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 150):
|
||||
if (use_map):
|
||||
show_gps_and_sim_positions(mavproxy, False)
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# fly forward (south east) at least 60m
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
if not wait_distance(mav, 60):
|
||||
if (use_map):
|
||||
show_gps_and_sim_positions(mavproxy, False)
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# wait for copter to slow down
|
||||
if not wait_groundspeed(mav, 0, 1):
|
||||
@ -598,7 +590,11 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||
|
||||
# Fly mission #1
|
||||
progress("# Load copter_glitch_mission")
|
||||
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_glitch_mission.txt")):
|
||||
# load the waypoint count
|
||||
global homeloc
|
||||
global num_wp
|
||||
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_glitch_mission.txt"))
|
||||
if not num_wp:
|
||||
progress("load copter_glitch_mission failed")
|
||||
return False
|
||||
|
||||
@ -606,16 +602,13 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||
if (use_map):
|
||||
show_gps_and_sim_positions(mavproxy, True)
|
||||
|
||||
# load the waypoint count
|
||||
global homeloc
|
||||
global num_wp
|
||||
progress("test: Fly a mission from 1 to %u" % num_wp)
|
||||
mavproxy.send('wp set 1\n')
|
||||
|
||||
# switch into AUTO mode and raise throttle
|
||||
mavproxy.send('switch 4\n') # auto mode
|
||||
wait_mode(mav, 'AUTO')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
|
||||
# wait until 100m from home
|
||||
if not wait_distance(mav, 100, 5, 60):
|
||||
@ -692,41 +685,41 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
|
||||
# switch to stabilize mode
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
|
||||
# fly south 50m
|
||||
progress("# Flying south %u meters" % side)
|
||||
mavproxy.send('rc 1 1300\n')
|
||||
set_rc(mavproxy, mav, 1, 1300)
|
||||
if not wait_distance(mav, side, 5, 60):
|
||||
failed = True
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
|
||||
# fly west 8 seconds
|
||||
progress("# Flying west for 8 seconds")
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
set_rc(mavproxy, mav, 2, 1300)
|
||||
tstart = get_sim_time(mav)
|
||||
while get_sim_time(mav) < (tstart + 8):
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
delta = (get_sim_time(mav) - tstart)
|
||||
# progress("%u" % delta)
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# fly north 25 meters
|
||||
progress("# Flying north %u meters" % (side/2.0))
|
||||
mavproxy.send('rc 1 1700\n')
|
||||
set_rc(mavproxy, mav, 1, 1700)
|
||||
if not wait_distance(mav, side/2, 5, 60):
|
||||
failed = True
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
|
||||
# fly east 8 seconds
|
||||
progress("# Flying east for 8 seconds")
|
||||
mavproxy.send('rc 2 1700\n')
|
||||
set_rc(mavproxy, mav, 2, 1700)
|
||||
tstart = get_sim_time(mav)
|
||||
while get_sim_time(mav) < (tstart + 8):
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
delta = (get_sim_time(mav) - tstart)
|
||||
# progress("%u" % delta)
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# restore to default
|
||||
mavproxy.send('param set SIMPLE 0\n')
|
||||
@ -747,10 +740,10 @@ def fly_super_simple(mavproxy, mav, timeout=45):
|
||||
|
||||
# fly forward 20m
|
||||
progress("# Flying forward 20 meters")
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
set_rc(mavproxy, mav, 2, 1300)
|
||||
if not wait_distance(mav, 20, 5, 60):
|
||||
failed = True
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# set SUPER SIMPLE mode for all flight modes
|
||||
mavproxy.send('param set SUPER_SIMPLE 63\n')
|
||||
@ -758,22 +751,22 @@ def fly_super_simple(mavproxy, mav, timeout=45):
|
||||
# switch to stabilize mode
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
|
||||
# start copter yawing slowly
|
||||
mavproxy.send('rc 4 1550\n')
|
||||
set_rc(mavproxy, mav, 4, 1550)
|
||||
|
||||
# roll left for timeout seconds
|
||||
progress("# rolling left from pilot's point of view for %u seconds" % timeout)
|
||||
mavproxy.send('rc 1 1300\n')
|
||||
set_rc(mavproxy, mav, 1, 1300)
|
||||
tstart = get_sim_time(mav)
|
||||
while get_sim_time(mav) < (tstart + timeout):
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
delta = (get_sim_time(mav) - tstart)
|
||||
|
||||
# stop rolling and yawing
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# restore simple mode parameters to default
|
||||
mavproxy.send('param set SUPER_SIMPLE 0\n')
|
||||
@ -792,21 +785,21 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
||||
|
||||
# face west
|
||||
progress("turn west")
|
||||
mavproxy.send('rc 4 1580\n')
|
||||
set_rc(mavproxy, mav, 4, 1580)
|
||||
if not wait_heading(mav, 270):
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
# set CIRCLE radius
|
||||
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
|
||||
|
||||
# fly forward (east) at least 100m
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
if not wait_distance(mav, 100):
|
||||
return False
|
||||
|
||||
# return pitch stick back to middle
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# set CIRCLE mode
|
||||
mavproxy.send('switch 1\n') # circle mode
|
||||
@ -831,20 +824,21 @@ def fly_auto_test(mavproxy, mav):
|
||||
|
||||
# Fly mission #1
|
||||
progress("# Load copter_mission")
|
||||
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_mission.txt")):
|
||||
progress("load copter_mission failed")
|
||||
return False
|
||||
|
||||
# load the waypoint count
|
||||
global homeloc
|
||||
global num_wp
|
||||
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_mission.txt"))
|
||||
if not num_wp:
|
||||
progress("load copter_mission failed")
|
||||
return False
|
||||
|
||||
progress("test: Fly a mission from 1 to %u" % num_wp)
|
||||
mavproxy.send('wp set 1\n')
|
||||
|
||||
# switch into AUTO mode and raise throttle
|
||||
mavproxy.send('switch 4\n') # auto mode
|
||||
wait_mode(mav, 'AUTO')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
|
||||
# fly the mission
|
||||
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
||||
@ -854,7 +848,7 @@ def fly_auto_test(mavproxy, mav):
|
||||
land(mavproxy, mav)
|
||||
|
||||
# set throttle to minimum
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
|
||||
# wait for disarm
|
||||
mav.motors_disarmed_wait()
|
||||
@ -870,13 +864,14 @@ def fly_avc_test(mavproxy, mav):
|
||||
|
||||
# upload mission from file
|
||||
progress("# Load copter_AVC2013_mission")
|
||||
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")):
|
||||
progress("load copter_AVC2013_mission failed")
|
||||
return False
|
||||
|
||||
# load the waypoint count
|
||||
global homeloc
|
||||
global num_wp
|
||||
num_wp = load_mission_from_file(mavproxy, os.path.join(testdir, "copter_AVC2013_mission.txt"))
|
||||
if not num_wp:
|
||||
progress("load copter_AVC2013_mission failed")
|
||||
return False
|
||||
|
||||
progress("Fly AVC mission from 1 to %u" % num_wp)
|
||||
mavproxy.send('wp set 1\n')
|
||||
|
||||
@ -886,13 +881,13 @@ def fly_avc_test(mavproxy, mav):
|
||||
# switch into AUTO mode and raise throttle
|
||||
mavproxy.send('switch 4\n') # auto mode
|
||||
wait_mode(mav, 'AUTO')
|
||||
mavproxy.send('rc 3 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1500)
|
||||
|
||||
# fly the mission
|
||||
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
||||
|
||||
# set throttle to minimum
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
|
||||
# wait for disarm
|
||||
mav.motors_disarmed_wait()
|
||||
@ -903,17 +898,6 @@ def fly_avc_test(mavproxy, mav):
|
||||
return ret
|
||||
|
||||
|
||||
def land(mavproxy, mav, timeout=60):
|
||||
"""Land the quad."""
|
||||
progress("STARTING LANDING")
|
||||
mavproxy.send('switch 2\n') # land mode
|
||||
wait_mode(mav, 'LAND')
|
||||
progress("Entered Landing Mode")
|
||||
ret = wait_altitude(mav, -5, 1)
|
||||
progress("LANDING: ok= %s" % ret)
|
||||
return ret
|
||||
|
||||
|
||||
def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
|
||||
"""Fly a mission from a file."""
|
||||
global homeloc
|
||||
@ -933,38 +917,6 @@ def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
|
||||
return ret
|
||||
|
||||
|
||||
def load_mission_from_file(mavproxy, mav, filename):
|
||||
"""Load a mission from a file to flight controller."""
|
||||
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')
|
||||
|
||||
# update num_wp
|
||||
wploader = mavwp.MAVWPLoader()
|
||||
wploader.load(filename)
|
||||
num_wp = wploader.count()
|
||||
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))
|
||||
progress("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(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, frame=None, params=None, gdbserver=False, speedup=10):
|
||||
"""Fly ArduCopter in SITL.
|
||||
|
||||
@ -978,7 +930,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||
|
||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduCopter')
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
# setup test parameters
|
||||
@ -1041,15 +993,23 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||
failed_test_msg = "None"
|
||||
|
||||
try:
|
||||
progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
|
||||
mav.wait_heartbeat()
|
||||
setup_rc(mavproxy)
|
||||
progress("Setting up RC parameters")
|
||||
set_rc_default(mavproxy)
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
homeloc = mav.location()
|
||||
|
||||
progress("Home location: %s" % homeloc)
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
mav.wait_heartbeat()
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
progress("Waiting reading for arm")
|
||||
wait_ready_to_arm(mav)
|
||||
|
||||
# Arm
|
||||
progress("# Arm motors")
|
||||
if not arm_motors(mavproxy, mav):
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
failed_test_msg = "arm_motors failed"
|
||||
progress(failed_test_msg)
|
||||
failed = True
|
||||
@ -1071,7 +1031,9 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||
|
||||
# save the stored mission to file
|
||||
progress("# Save out the CH7 mission to file")
|
||||
if not save_mission_to_file(mavproxy, mav, os.path.join(testdir, "ch7_mission.txt")):
|
||||
global num_wp
|
||||
num_wp = save_mission_to_file(mavproxy, os.path.join(testdir, "ch7_mission.txt"))
|
||||
if not num_wp:
|
||||
failed_test_msg = "save_mission_to_file failed"
|
||||
progress(failed_test_msg)
|
||||
failed = True
|
||||
@ -1298,8 +1260,8 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||
progress(failed_test_msg)
|
||||
failed = True
|
||||
|
||||
except pexpect.TIMEOUT as failed_test_msg:
|
||||
failed_test_msg = "Timeout"
|
||||
except pexpect.TIMEOUT as e:
|
||||
progress("Failed with timeout")
|
||||
failed = True
|
||||
|
||||
mav.close()
|
||||
@ -1331,7 +1293,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
|
||||
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
|
||||
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=speedup)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduCopter')
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
# setup test parameters
|
||||
@ -1397,23 +1359,26 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
|
||||
try:
|
||||
mav.wait_heartbeat()
|
||||
setup_rc(mavproxy)
|
||||
set_rc_default(mavproxy)
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
homeloc = mav.location()
|
||||
|
||||
progress("Lowering rotor speed")
|
||||
mavproxy.send('rc 8 1000\n')
|
||||
set_rc(mavproxy, mav, 8, 1000)
|
||||
|
||||
mavproxy.send('switch 6\n') # stabilize mode
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
wait_ready_to_arm(mav)
|
||||
|
||||
# Arm
|
||||
progress("# Arm motors")
|
||||
if not arm_motors(mavproxy, mav):
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
failed_test_msg = "arm_motors failed"
|
||||
progress(failed_test_msg)
|
||||
failed = True
|
||||
|
||||
progress("Raising rotor speed")
|
||||
mavproxy.send('rc 8 2000\n')
|
||||
set_rc(mavproxy, mav, 8, 2000)
|
||||
|
||||
progress("# Fly AVC mission")
|
||||
if not fly_avc_test(mavproxy, mav):
|
||||
@ -1424,7 +1389,7 @@ def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
progress("Flew AVC mission OK")
|
||||
|
||||
progress("Lowering rotor speed")
|
||||
mavproxy.send('rc 8 1000\n')
|
||||
set_rc(mavproxy, mav, 8, 1000)
|
||||
|
||||
# mission includes disarm at end so should be ok to download logs now
|
||||
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
|
||||
|
||||
@ -21,42 +21,41 @@ WIND = "0,180,0.2" # speed,direction,variance
|
||||
|
||||
homeloc = None
|
||||
|
||||
|
||||
def takeoff(mavproxy, mav):
|
||||
"""Takeoff get to 30m altitude."""
|
||||
|
||||
wait_ready_to_arm(mav)
|
||||
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
arm_vehicle(mavproxy, mav)
|
||||
|
||||
mavproxy.send('switch 4\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
|
||||
# some rudder to counteract the prop torque
|
||||
mavproxy.send('rc 4 1700\n')
|
||||
set_rc(mavproxy, mav, 4, 1700)
|
||||
|
||||
# some up elevator to keep the tail down
|
||||
mavproxy.send('rc 2 1200\n')
|
||||
set_rc(mavproxy, mav, 2, 1200)
|
||||
|
||||
# get it moving a bit first
|
||||
mavproxy.send('rc 3 1300\n')
|
||||
set_rc(mavproxy, mav, 3, 1300)
|
||||
mav.recv_match(condition='VFR_HUD.groundspeed>6', blocking=True)
|
||||
|
||||
# a bit faster again, straighten rudder
|
||||
mavproxy.send('rc 3 1600\n')
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1600)
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
mav.recv_match(condition='VFR_HUD.groundspeed>12', blocking=True)
|
||||
|
||||
# hit the gas harder now, and give it some more elevator
|
||||
mavproxy.send('rc 2 1100\n')
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 2, 1100)
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
|
||||
# gain a bit of altitude
|
||||
if not wait_altitude(mav, homeloc.alt+150, homeloc.alt+180, timeout=30):
|
||||
return False
|
||||
|
||||
# level off
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
progress("TAKEOFF COMPLETE")
|
||||
return True
|
||||
@ -66,7 +65,7 @@ def fly_left_circuit(mavproxy, mav):
|
||||
"""Fly a left circuit, 200m on a side."""
|
||||
mavproxy.send('switch 4\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
if not wait_level_flight(mavproxy, mav):
|
||||
return False
|
||||
|
||||
@ -75,10 +74,10 @@ def fly_left_circuit(mavproxy, mav):
|
||||
for i in range(0, 4):
|
||||
# hard left
|
||||
progress("Starting turn %u" % i)
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
set_rc(mavproxy, mav, 1, 1000)
|
||||
if not wait_heading(mav, 270 - (90*i), accuracy=10):
|
||||
return False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
progress("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 100, accuracy=20):
|
||||
return False
|
||||
@ -169,9 +168,9 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
||||
"""Wait for level flight."""
|
||||
tstart = get_sim_time(mav)
|
||||
progress("Waiting for level flight")
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
while get_sim_time(mav) < tstart + timeout:
|
||||
m = mav.recv_match(type='ATTITUDE', blocking=True)
|
||||
roll = math.degrees(m.roll)
|
||||
@ -190,12 +189,12 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
||||
wait_mode(mav, 'FBWA')
|
||||
alt_error = mav.messages['VFR_HUD'].alt - altitude
|
||||
if alt_error > 0:
|
||||
mavproxy.send('rc 2 2000\n')
|
||||
set_rc(mavproxy, mav, 2, 2000)
|
||||
else:
|
||||
mavproxy.send('rc 2 1000\n')
|
||||
set_rc(mavproxy, mav, 2, 1000)
|
||||
if not wait_altitude(mav, altitude-accuracy/2, altitude+accuracy/2):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
progress("Reached target altitude at %u" % mav.messages['VFR_HUD'].alt)
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
@ -203,7 +202,7 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
||||
def axial_left_roll(mavproxy, mav, count=1):
|
||||
"""Fly a left axial roll."""
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
|
||||
@ -213,30 +212,30 @@ def axial_left_roll(mavproxy, mav, count=1):
|
||||
|
||||
while count > 0:
|
||||
progress("Starting roll")
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
set_rc(mavproxy, mav, 1, 1000)
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
return False
|
||||
if not wait_roll(mav, 0, accuracy=90):
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
mavproxy.send('switch 4\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
set_rc(mavproxy, mav, 3, 1700)
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def inside_loop(mavproxy, mav, count=1):
|
||||
"""Fly a inside loop."""
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
|
||||
@ -246,7 +245,7 @@ def inside_loop(mavproxy, mav, count=1):
|
||||
|
||||
while count > 0:
|
||||
progress("Starting loop")
|
||||
mavproxy.send('rc 2 1000\n')
|
||||
set_rc(mavproxy, mav, 2, 1000)
|
||||
if not wait_pitch(mav, -60, accuracy=20):
|
||||
return False
|
||||
if not wait_pitch(mav, 0, accuracy=20):
|
||||
@ -254,21 +253,21 @@ def inside_loop(mavproxy, mav, count=1):
|
||||
count -= 1
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
mavproxy.send('switch 4\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
set_rc(mavproxy, mav, 3, 1700)
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def test_stabilize(mavproxy, mav, count=1):
|
||||
"""Fly stabilize mode."""
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
set_rc(mavproxy, mav, 2, 1300)
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
mavproxy.send("mode STABILIZE\n")
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
@ -276,7 +275,7 @@ def test_stabilize(mavproxy, mav, count=1):
|
||||
count = 1
|
||||
while count > 0:
|
||||
progress("Starting roll")
|
||||
mavproxy.send('rc 1 2000\n')
|
||||
set_rc(mavproxy, mav, 1, 2000)
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
@ -285,25 +284,25 @@ def test_stabilize(mavproxy, mav, count=1):
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
if not wait_roll(mav, 0, accuracy=5):
|
||||
return False
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
set_rc(mavproxy, mav, 3, 1700)
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def test_acro(mavproxy, mav, count=1):
|
||||
"""Fly ACRO mode."""
|
||||
# full throttle!
|
||||
mavproxy.send('rc 3 2000\n')
|
||||
mavproxy.send('rc 2 1300\n')
|
||||
set_rc(mavproxy, mav, 3, 2000)
|
||||
set_rc(mavproxy, mav, 2, 1300)
|
||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||
return False
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
mavproxy.send("mode ACRO\n")
|
||||
wait_mode(mav, 'ACRO')
|
||||
@ -311,7 +310,7 @@ def test_acro(mavproxy, mav, count=1):
|
||||
count = 1
|
||||
while count > 0:
|
||||
progress("Starting roll")
|
||||
mavproxy.send('rc 1 1000\n')
|
||||
set_rc(mavproxy, mav, 1, 1000)
|
||||
if not wait_roll(mav, -150, accuracy=90):
|
||||
return False
|
||||
if not wait_roll(mav, 150, accuracy=90):
|
||||
@ -319,7 +318,7 @@ def test_acro(mavproxy, mav, count=1):
|
||||
if not wait_roll(mav, 0, accuracy=90):
|
||||
return False
|
||||
count -= 1
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
@ -333,19 +332,19 @@ def test_acro(mavproxy, mav, count=1):
|
||||
count = 2
|
||||
while count > 0:
|
||||
progress("Starting loop")
|
||||
mavproxy.send('rc 2 1000\n')
|
||||
set_rc(mavproxy, mav, 2, 1000)
|
||||
if not wait_pitch(mav, -60, accuracy=20):
|
||||
return False
|
||||
if not wait_pitch(mav, 0, accuracy=20):
|
||||
return False
|
||||
count -= 1
|
||||
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# back to FBWA
|
||||
mavproxy.send('mode FBWA\n')
|
||||
wait_mode(mav, 'FBWA')
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
set_rc(mavproxy, mav, 3, 1700)
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
@ -353,13 +352,13 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||
"""Fly FBWB or CRUISE mode."""
|
||||
mavproxy.send("mode %s\n" % mode)
|
||||
wait_mode(mav, mode)
|
||||
mavproxy.send('rc 3 1700\n')
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 3, 1700)
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
|
||||
# lock in the altitude by asking for an altitude change then releasing
|
||||
mavproxy.send('rc 2 1000\n')
|
||||
set_rc(mavproxy, mav, 2, 1000)
|
||||
wait_distance(mav, 50, accuracy=20)
|
||||
mavproxy.send('rc 2 1500\n')
|
||||
set_rc(mavproxy, mav, 2, 1500)
|
||||
wait_distance(mav, 50, accuracy=20)
|
||||
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
@ -371,11 +370,11 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||
for i in range(0, 4):
|
||||
# hard left
|
||||
progress("Starting turn %u" % i)
|
||||
mavproxy.send('rc 1 1800\n')
|
||||
set_rc(mavproxy, mav, 1, 1800)
|
||||
if not wait_heading(mav, 0 + (90*i), accuracy=20, timeout=60):
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
return False
|
||||
mavproxy.send('rc 1 1500\n')
|
||||
set_rc(mavproxy, mav, 1, 1500)
|
||||
progress("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 100, accuracy=20):
|
||||
return False
|
||||
@ -386,11 +385,11 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||
for i in range(0, 4):
|
||||
# hard left
|
||||
progress("Starting turn %u" % i)
|
||||
mavproxy.send('rc 4 1900\n')
|
||||
set_rc(mavproxy, mav, 4, 1900)
|
||||
if not wait_heading(mav, 360 - (90*i), accuracy=20, timeout=60):
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
return False
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
progress("Starting leg %u" % i)
|
||||
if not wait_distance(mav, 100, accuracy=20):
|
||||
return False
|
||||
@ -411,14 +410,6 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||
return wait_level_flight(mavproxy, mav)
|
||||
|
||||
|
||||
def setup_rc(mavproxy):
|
||||
"""Setup RC override control."""
|
||||
for chan in [1, 2, 4, 5, 6, 7]:
|
||||
mavproxy.send('rc %u 1500\n' % chan)
|
||||
mavproxy.send('rc 3 1000\n')
|
||||
mavproxy.send('rc 8 1800\n')
|
||||
|
||||
|
||||
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
|
||||
"""Fly a mission from a file."""
|
||||
global homeloc
|
||||
@ -494,7 +485,9 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
progress("Waiting for a heartbeat with mavlink protocol %s" % mav.WIRE_PROTOCOL_VERSION)
|
||||
mav.wait_heartbeat()
|
||||
progress("Setting up RC parameters")
|
||||
setup_rc(mavproxy)
|
||||
set_rc_default(mavproxy)
|
||||
set_rc(mavproxy, mav, 3, 1000)
|
||||
set_rc(mavproxy, mav, 8, 1800)
|
||||
progress("Waiting for GPS fix")
|
||||
mav.recv_match(condition='VFR_HUD.alt>10', blocking=True)
|
||||
mav.wait_gps_fix()
|
||||
@ -546,7 +539,7 @@ def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
progress("Failed CIRCLE")
|
||||
failed = True
|
||||
fail_list.append("LOITER")
|
||||
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy = 10,
|
||||
if not fly_mission(mavproxy, mav, os.path.join(testdir, "ap1.txt"), height_accuracy=10,
|
||||
target_altitude=homeloc.alt+100):
|
||||
progress("Failed mission")
|
||||
failed = True
|
||||
|
||||
@ -19,55 +19,43 @@ HOME = mavutil.location(33.810313, -118.393867, 0, 185)
|
||||
homeloc = None
|
||||
|
||||
|
||||
def arm_sub(mavproxy, mav):
|
||||
for i in range(8):
|
||||
mavproxy.send('rc %d 1500\n' % (i+1))
|
||||
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
|
||||
progress("SUB ARMED")
|
||||
return True
|
||||
|
||||
def dive_manual(mavproxy, mav):
|
||||
mavproxy.send('rc 3 1600\n')
|
||||
mavproxy.send('rc 5 1600\n')
|
||||
mavproxy.send('rc 6 1550\n')
|
||||
set_rc(mavproxy, mav, 3, 1600)
|
||||
set_rc(mavproxy, mav, 5, 1600)
|
||||
set_rc(mavproxy, mav, 6, 1550)
|
||||
|
||||
if not wait_distance(mav, 50, accuracy=7, timeout=200):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1550\n')
|
||||
set_rc(mavproxy, mav, 4, 1550)
|
||||
|
||||
if not wait_heading(mav, 0):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
|
||||
if not wait_distance(mav, 50, accuracy=7, timeout=100):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1550\n')
|
||||
set_rc(mavproxy, mav, 4, 1550)
|
||||
|
||||
if not wait_heading(mav, 0):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc 4 1500\n')
|
||||
mavproxy.send('rc 5 1500\n')
|
||||
mavproxy.send('rc 6 1100\n')
|
||||
set_rc(mavproxy, mav, 4, 1500)
|
||||
set_rc(mavproxy, mav, 5, 1500)
|
||||
set_rc(mavproxy, mav, 6, 1100)
|
||||
|
||||
if not wait_distance(mav, 75, accuracy=7, timeout=100):
|
||||
return False
|
||||
|
||||
mavproxy.send('rc all 1500\n')
|
||||
|
||||
mavproxy.send('disarm\n');
|
||||
|
||||
# wait for disarm
|
||||
mav.motors_disarmed_wait()
|
||||
set_rc_default(mavproxy)
|
||||
|
||||
disarm_vehicle(mavproxy, mav)
|
||||
progress("Manual dive OK")
|
||||
return True
|
||||
|
||||
|
||||
def dive_mission(mavproxy, mav, filename):
|
||||
|
||||
progress("Executing mission %s" % filename)
|
||||
@ -75,8 +63,9 @@ def dive_mission(mavproxy, mav, filename):
|
||||
mavproxy.expect('Flight plan received')
|
||||
mavproxy.send('wp list\n')
|
||||
mavproxy.expect('Saved [0-9]+ waypoints')
|
||||
set_rc_default(mavproxy)
|
||||
|
||||
if not arm_sub(mavproxy, mav):
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
progress("Failed to ARM")
|
||||
return False
|
||||
|
||||
@ -85,15 +74,13 @@ def dive_mission(mavproxy, mav, filename):
|
||||
|
||||
if not wait_waypoint(mav, 1, 5, max_dist=5):
|
||||
return False
|
||||
|
||||
mavproxy.send('disarm\n');
|
||||
|
||||
# wait for disarm
|
||||
mav.motors_disarmed_wait()
|
||||
|
||||
disarm_vehicle(mavproxy, mav)
|
||||
|
||||
progress("Mission OK")
|
||||
return True
|
||||
|
||||
|
||||
def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False, gdbserver=False, speedup=10):
|
||||
"""Dive ArduSub in SITL.
|
||||
|
||||
@ -108,7 +95,7 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False
|
||||
|
||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
sitl = util.start_SITL(binary, model='vectored', wipe=True, home=home, speedup=speedup)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduSub', options=options)
|
||||
mavproxy = util.start_MAVProxy_SITL('ArduSub')
|
||||
mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
# setup test parameters
|
||||
@ -169,7 +156,8 @@ def dive_ArduSub(binary, viewerip=None, use_map=False, valgrind=False, gdb=False
|
||||
|
||||
homeloc = mav.location()
|
||||
progress("Home location: %s" % homeloc)
|
||||
if not arm_sub(mavproxy, mav):
|
||||
set_rc_default(mavproxy)
|
||||
if not arm_vehicle(mavproxy, mav):
|
||||
progress("Failed to ARM")
|
||||
failed = True
|
||||
if not dive_manual(mavproxy, mav):
|
||||
|
||||
@ -2,7 +2,7 @@ from __future__ import print_function
|
||||
import math
|
||||
import time
|
||||
|
||||
from pymavlink import mavwp
|
||||
from pymavlink import mavwp, mavutil
|
||||
|
||||
from pysim import util
|
||||
|
||||
@ -10,30 +10,14 @@ from pysim import util
|
||||
# messages. This keeps the output to stdout flowing
|
||||
expect_list = []
|
||||
|
||||
|
||||
class AutoTestTimeoutException(Exception):
|
||||
pass
|
||||
|
||||
def wait_ready_to_arm(mav, timeout=None):
|
||||
# wait for EKF checks to pass
|
||||
return wait_ekf_happy(mav, timeout=timeout)
|
||||
|
||||
def wait_ekf_happy(mav, timeout=30):
|
||||
"""Wait for EKF to be happy"""
|
||||
|
||||
tstart = get_sim_time(mav)
|
||||
required_value = 831
|
||||
progress("Waiting for EKF value %u" % (required_value))
|
||||
while timeout is None or get_sim_time(mav) < tstart + timeout:
|
||||
m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True)
|
||||
current = m.flags
|
||||
if (tstart - get_sim_time(mav)) % 5 == 0:
|
||||
progress("Wait EKF.flags: required:%u current:%u" % (required_value, current))
|
||||
if current == required_value:
|
||||
progress("EKF Flags OK")
|
||||
return
|
||||
progress("Failed to get EKF.flags=%u" % required_value)
|
||||
raise AutoTestTimeoutException()
|
||||
|
||||
#################################################
|
||||
# GENERAL UTILITIES
|
||||
#################################################
|
||||
def expect_list_clear():
|
||||
"""clear the expect list."""
|
||||
global expect_list
|
||||
@ -68,13 +52,152 @@ def expect_callback(e):
|
||||
util.pexpect_drain(p)
|
||||
|
||||
|
||||
'''
|
||||
SIM UTILITIES
|
||||
'''
|
||||
#################################################
|
||||
# SIM UTILITIES
|
||||
#################################################
|
||||
def progress(text):
|
||||
"""Display autotest progress text."""
|
||||
print("AUTOTEST: " + text)
|
||||
|
||||
|
||||
def get_sim_time(mav):
|
||||
"""Get SITL time."""
|
||||
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
|
||||
return m.time_boot_ms * 1.0e-3
|
||||
|
||||
|
||||
def sim_location(mav):
|
||||
"""Return current simulator location."""
|
||||
m = mav.recv_match(type='SIMSTATE', blocking=True)
|
||||
return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
|
||||
|
||||
|
||||
def save_wp(mavproxy, mav):
|
||||
"""Trigger RC 7 to save waypoint."""
|
||||
mavproxy.send('rc 7 1000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
mavproxy.send('rc 7 2000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
mavproxy.send('rc 7 1000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
|
||||
|
||||
def log_download(mavproxy, mav, filename, timeout=360):
|
||||
"""Download latest log."""
|
||||
mavproxy.send("log list\n")
|
||||
mavproxy.expect("numLogs")
|
||||
mav.wait_heartbeat()
|
||||
mav.wait_heartbeat()
|
||||
mavproxy.send("set shownoise 0\n")
|
||||
mavproxy.send("log download latest %s\n" % filename)
|
||||
mavproxy.expect("Finished downloading", timeout=timeout)
|
||||
mav.wait_heartbeat()
|
||||
mav.wait_heartbeat()
|
||||
return True
|
||||
|
||||
|
||||
def show_gps_and_sim_positions(mavproxy, on_off):
|
||||
"""Allow to display gps and actual position on map."""
|
||||
if on_off is True:
|
||||
# turn on simulator display of gps and actual position
|
||||
mavproxy.send('map set showgpspos 1\n')
|
||||
mavproxy.send('map set showsimpos 1\n')
|
||||
else:
|
||||
# turn off simulator display of gps and actual position
|
||||
mavproxy.send('map set showgpspos 0\n')
|
||||
mavproxy.send('map set showsimpos 0\n')
|
||||
|
||||
|
||||
def mission_count(filename):
|
||||
"""Load a mission from a file and return number of waypoints."""
|
||||
wploader = mavwp.MAVWPLoader()
|
||||
wploader.load(filename)
|
||||
num_wp = wploader.count()
|
||||
return num_wp
|
||||
|
||||
|
||||
def load_mission_from_file(mavproxy, filename):
|
||||
"""Load a mission from a file to flight controller."""
|
||||
mavproxy.send('wp load %s\n' % filename)
|
||||
mavproxy.expect('Flight plan received')
|
||||
mavproxy.send('wp list\n')
|
||||
mavproxy.expect('Requesting [0-9]+ waypoints')
|
||||
|
||||
# update num_wp
|
||||
wploader = mavwp.MAVWPLoader()
|
||||
wploader.load(filename)
|
||||
num_wp = wploader.count()
|
||||
return num_wp
|
||||
|
||||
|
||||
def save_mission_to_file(mavproxy, filename):
|
||||
"""Save a mission to a file"""
|
||||
mavproxy.send('wp save %s\n' % filename)
|
||||
mavproxy.expect('Saved ([0-9]+) waypoints')
|
||||
num_wp = int(mavproxy.match.group(1))
|
||||
progress("num_wp: %d" % num_wp)
|
||||
return num_wp
|
||||
|
||||
|
||||
def set_rc_default(mavproxy):
|
||||
"""Setup all simulated RC control to 1500."""
|
||||
for chan in range(1, 16):
|
||||
mavproxy.send('rc %u 1500\n' % chan)
|
||||
|
||||
|
||||
def set_rc(mavproxy, mav, chan, pwm, timeout=2):
|
||||
"""Setup a simulated RC control to a PWM value"""
|
||||
tstart = get_sim_time(mav)
|
||||
while get_sim_time(mav) < tstart + timeout:
|
||||
mavproxy.send('rc %u %u\n' % (chan, pwm))
|
||||
m = mav.recv_match(type='RC_CHANNELS', blocking=True)
|
||||
chan_pwm = getattr(m, "chan" + str(chan) + "_raw")
|
||||
if chan_pwm == pwm:
|
||||
return True
|
||||
progress("Failed to send RC commands")
|
||||
return False
|
||||
|
||||
|
||||
def arm_vehicle(mavproxy, mav):
|
||||
"""Arm vehicle with mavlink arm message."""
|
||||
mavproxy.send('arm throttle\n')
|
||||
mav.motors_armed_wait()
|
||||
progress("ARMED")
|
||||
return True
|
||||
|
||||
|
||||
def disarm_vehicle(mavproxy, mav):
|
||||
"""Disarm vehicle with mavlink disarm message."""
|
||||
mavproxy.send('disarm\n')
|
||||
mav.motors_disarmed_wait()
|
||||
progress("DISARMED")
|
||||
return True
|
||||
|
||||
|
||||
def set_parameter(mavproxy, name, value):
|
||||
for i in range(1, 10):
|
||||
mavproxy.send("param set %s %s\n" % (name, str(value)))
|
||||
mavproxy.send("param fetch %s\n" % (name))
|
||||
mavproxy.expect("%s = (.*)" % (name,))
|
||||
returned_value = mavproxy.match.group(1)
|
||||
if float(returned_value) == float(value):
|
||||
# yes, exactly equal.
|
||||
break
|
||||
progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value))
|
||||
|
||||
|
||||
def get_parameter(mavproxy, name):
|
||||
mavproxy.send("param fetch %s\n" % (name))
|
||||
mavproxy.expect("%s = (.*)" % (name,))
|
||||
return float(mavproxy.match.group(1))
|
||||
|
||||
|
||||
#################################################
|
||||
# UTILITIES
|
||||
#################################################
|
||||
def get_distance(loc1, loc2):
|
||||
"""Get ground distance between two locations."""
|
||||
dlat = loc2.lat - loc1.lat
|
||||
@ -92,33 +215,42 @@ def get_bearing(loc1, loc2):
|
||||
return bearing
|
||||
|
||||
|
||||
def do_get_autopilot_capabilities(mavproxy, mav):
|
||||
mavproxy.send("long REQUEST_AUTOPILOT_CAPABILITIES 1\n")
|
||||
m = mav.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
progress("AUTOPILOT_VERSION not received")
|
||||
return False
|
||||
progress("AUTOPILOT_VERSION received")
|
||||
return True
|
||||
|
||||
|
||||
def do_set_mode_via_command_long(mavproxy, mav):
|
||||
base_mode = mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
|
||||
custom_mode = 4 # hold
|
||||
start = time.time()
|
||||
while time.time() - start < 5:
|
||||
mavproxy.send("long DO_SET_MODE %u %u\n" % (base_mode,custom_mode))
|
||||
m = mav.recv_match(type='HEARTBEAT', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
return False
|
||||
if m.custom_mode == custom_mode:
|
||||
return True
|
||||
time.sleep(0.1)
|
||||
return False
|
||||
|
||||
|
||||
#################################################
|
||||
# WAIT UTILITIES
|
||||
#################################################
|
||||
def wait_seconds(mav, seconds_to_wait):
|
||||
"""Wait some second in SITL time."""
|
||||
tstart = get_sim_time(mav)
|
||||
tnow = tstart
|
||||
while tstart + seconds_to_wait > tnow:
|
||||
tnow = get_sim_time(mav)
|
||||
|
||||
|
||||
def get_sim_time(mav):
|
||||
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
|
||||
return m.time_boot_ms * 1.0e-3
|
||||
|
||||
def set_parameter(mavproxy, name ,value):
|
||||
for i in range(1,10):
|
||||
mavproxy.send("param set %s %s\n" % (name, str(value)))
|
||||
mavproxy.send("param fetch %s\n" % (name))
|
||||
mavproxy.expect("%s = (.*)" % (name,))
|
||||
returned_value = mavproxy.match.group(1)
|
||||
if float(returned_value) == float(value):
|
||||
# yes, exactly equal.
|
||||
break
|
||||
progress("Param fetch returned incorrect value (%s) vs (%s)" % (returned_value, value))
|
||||
|
||||
def get_parameter(mavproxy, name):
|
||||
mavproxy.send("param fetch %s\n" % (name))
|
||||
mavproxy.expect("%s = (.*)" % (name,))
|
||||
return float(mavproxy.match.group(1))
|
||||
|
||||
def wait_altitude(mav, alt_min, alt_max, timeout=30):
|
||||
"""Wait for a given altitude range."""
|
||||
climb_rate = 0
|
||||
@ -280,49 +412,33 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
|
||||
return False
|
||||
|
||||
|
||||
def save_wp(mavproxy, mav):
|
||||
mavproxy.send('rc 7 1000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
mavproxy.send('rc 7 2000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==2000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
mavproxy.send('rc 7 1000\n')
|
||||
mav.recv_match(condition='RC_CHANNELS.chan7_raw==1000', blocking=True)
|
||||
wait_seconds(mav, 1)
|
||||
|
||||
|
||||
def wait_mode(mav, mode, timeout=None):
|
||||
"""Wait for mode to change."""
|
||||
progress("Waiting for mode %s" % mode)
|
||||
mav.wait_heartbeat()
|
||||
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
|
||||
progress("Got mode %s" % mode)
|
||||
return mav.flightmode
|
||||
|
||||
|
||||
def mission_count(filename):
|
||||
"""Load a mission from a file and return number of waypoints."""
|
||||
wploader = mavwp.MAVWPLoader()
|
||||
wploader.load(filename)
|
||||
num_wp = wploader.count()
|
||||
return num_wp
|
||||
def wait_ready_to_arm(mav, timeout=None):
|
||||
# wait for EKF checks to pass
|
||||
return wait_ekf_happy(mav, timeout=timeout)
|
||||
|
||||
|
||||
def sim_location(mav):
|
||||
"""Return current simulator location."""
|
||||
from pymavlink import mavutil
|
||||
m = mav.recv_match(type='SIMSTATE', blocking=True)
|
||||
return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
|
||||
def wait_ekf_happy(mav, timeout=30):
|
||||
"""Wait for EKF to be happy"""
|
||||
|
||||
|
||||
def log_download(mavproxy, mav, filename, timeout=360):
|
||||
"""Download latest log."""
|
||||
mavproxy.send("log list\n")
|
||||
mavproxy.expect("numLogs")
|
||||
mav.wait_heartbeat()
|
||||
mav.wait_heartbeat()
|
||||
mavproxy.send("set shownoise 0\n")
|
||||
mavproxy.send("log download latest %s\n" % filename)
|
||||
mavproxy.expect("Finished downloading", timeout=timeout)
|
||||
mav.wait_heartbeat()
|
||||
mav.wait_heartbeat()
|
||||
return True
|
||||
tstart = get_sim_time(mav)
|
||||
required_value = 831
|
||||
progress("Waiting for EKF value %u" % (required_value))
|
||||
while timeout is None or get_sim_time(mav) < tstart + timeout:
|
||||
m = mav.recv_match(type='EKF_STATUS_REPORT', blocking=True)
|
||||
current = m.flags
|
||||
if (tstart - get_sim_time(mav)) % 5 == 0:
|
||||
progress("Wait EKF.flags: required:%u current:%u" % (required_value, current))
|
||||
if current == required_value:
|
||||
progress("EKF Flags OK")
|
||||
return
|
||||
progress("Failed to get EKF.flags=%u" % required_value)
|
||||
raise AutoTestTimeoutException()
|
||||
|
||||
@ -37,8 +37,7 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
|
||||
mavproxy.expect('DISARMED')
|
||||
# wait for blood sample here
|
||||
mavproxy.send('wp set 20\n')
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
arm_vehicle(mavproxy, mav)
|
||||
if not wait_waypoint(mav, 20, 34, max_dist=60, timeout=1200):
|
||||
return False
|
||||
mavproxy.expect('DISARMED')
|
||||
@ -110,8 +109,7 @@ def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fals
|
||||
# wait for EKF and GPS checks to pass
|
||||
wait_seconds(mav, 30)
|
||||
|
||||
mavproxy.send('arm throttle\n')
|
||||
mavproxy.expect('ARMED')
|
||||
arm_vehicle(mavproxy, mav)
|
||||
|
||||
if not fly_mission(mavproxy, mav,
|
||||
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user