zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
mission-4.1.18
khancyr 8 years ago committed by Peter Barker
parent
da11241aa8
commit
fac89ed437
6 changed files with 468 additions and 430 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 104
      Tools/autotest/apmrover2.py
  2. 335
      Tools/autotest/arducopter.py
  3. 123
      Tools/autotest/arduplane.py
  4. 54
      Tools/autotest/ardusub.py
  5. 276
      Tools/autotest/common.py
  6. 6
      Tools/autotest/quadplane.py

104
Tools/autotest/apmrover2.py
Unescape View File

@ -4,60 +4,49 @@ @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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 @@ -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 @@ -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

335
Tools/autotest/arducopter.py
Unescape View File

@ -19,51 +19,32 @@ vinfo = vehicleinfo.VehicleInfo() @@ -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): @@ -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): @@ -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 @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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 @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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 @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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")):

123
Tools/autotest/arduplane.py
Unescape View File

@ -21,42 +21,41 @@ WIND = "0,180,0.2" # speed,direction,variance @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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): @@ -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'): @@ -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'): @@ -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'): @@ -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'): @@ -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 @@ -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 @@ -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

54
Tools/autotest/ardusub.py
Unescape View File

@ -19,55 +19,43 @@ HOME = mavutil.location(33.810313, -118.393867, 0, 185) @@ -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): @@ -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): @@ -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 @@ -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 @@ -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):

276
Tools/autotest/common.py
Unescape View File

@ -2,7 +2,7 @@ from __future__ import print_function @@ -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 @@ -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): @@ -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): @@ -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 @@ -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()

6
Tools/autotest/quadplane.py
Unescape View File

@ -37,8 +37,7 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): @@ -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 @@ -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"),

Write Preview
Loading…
Cancel
Save