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

Tools: update python coding style 

Tools: update PrintVersion.py coding style

autotest: update python coding style

pysim: update python coding style

jsb_sim: update Python coding style

param_metadata: update Python coding style
This commit is contained in:
Pierre Kancir 2016-07-31 12:22:06 +02:00 committed by Peter Barker
parent 38b3d3ff3a
commit 9e1ffcae5d
29 changed files with 1016 additions and 830 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Tools/PrintVersion.py
View File

@ -1,14 +1,15 @@
#!/usr/bin/env python #!/usr/bin/env python
''' """
Extract version information for the various vehicle types, print it Extract version information for the various vehicle types, print it
''' """
import sys
import re
import os import os
import re
import sys
from optparse import OptionParser from optparse import OptionParser
parser = OptionParser("print_version.py [options] ArduCopter|ArduPlane|APMrover2|AntennaTracker") parser = OptionParser("print_version.py [options] ArduCopter|ArduPlane|APMrover2|AntennaTracker")
(opts, args) = parser.parse_args() (opts, args) = parser.parse_args()
@ -25,7 +26,7 @@ if len(args) > 0:
if vehicle not in includefiles: if vehicle not in includefiles:
print("Unknown vehicle (%s) (be in a vehicle directory or supply a vehicle type as an argument)" % (vehicle,)) print("Unknown vehicle (%s) (be in a vehicle directory or supply a vehicle type as an argument)" % (vehicle,))
sys.exit(1) sys.exit(1)
includefilepath="%s/%s" % (vehicle, includefiles[vehicle]) includefilepath = "%s/%s" % (vehicle, includefiles[vehicle])
else: else:
# assume we are in e.g. APM/APMrover2/ # assume we are in e.g. APM/APMrover2/
vehicle = os.path.basename(os.getcwd()) vehicle = os.path.basename(os.getcwd())

1
Tools/autotest/__init__.py Normal file
View File

@ -0,0 +1 @@
"""Autotests tools suite"""

62
Tools/autotest/apmrover2.py
View File

@ -1,18 +1,23 @@
# drive APMrover2 in SITL # drive APMrover2 in SITL
import util, pexpect, sys, time, math, shutil, os import os
from common import * import shutil
import pexpect
from pymavlink import mavutil from pymavlink import mavutil
import random
from common import *
from pysim import util
# get location of scripts # get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__)) testdir = os.path.dirname(os.path.realpath(__file__))
#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) HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
homeloc = None homeloc = None
def arm_rover(mavproxy, mav): def arm_rover(mavproxy, mav):
# wait for EKF and GPS checks to pass # wait for EKF and GPS checks to pass
wait_seconds(mav, 30) wait_seconds(mav, 30)
@ -23,15 +28,16 @@ def arm_rover(mavproxy, mav):
print("ROVER ARMED") print("ROVER ARMED")
return True return True
def drive_left_circuit(mavproxy, mav): def drive_left_circuit(mavproxy, mav):
'''drive a left circuit, 50m on a side''' """Drive a left circuit, 50m on a side."""
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
wait_mode(mav, 'MANUAL') wait_mode(mav, 'MANUAL')
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
print("Driving left circuit") print("Driving left circuit")
# do 4 turns # do 4 turns
for i in range(0,4): for i in range(0, 4):
# hard left # hard left
print("Starting turn %u" % i) print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n') mavproxy.send('rc 1 1000\n')
@ -45,8 +51,9 @@ def drive_left_circuit(mavproxy, mav):
print("Circuit complete") print("Circuit complete")
return True return True
def drive_RTL(mavproxy, mav): def drive_RTL(mavproxy, mav):
'''drive to home''' """Drive to home."""
print("Driving home in RTL") print("Driving home in RTL")
mavproxy.send('switch 3\n') mavproxy.send('switch 3\n')
if not wait_location(mav, homeloc, accuracy=22, timeout=90): if not wait_location(mav, homeloc, accuracy=22, timeout=90):
@ -54,22 +61,23 @@ def drive_RTL(mavproxy, mav):
print("RTL Complete") print("RTL Complete")
return True return True
def setup_rc(mavproxy): def setup_rc(mavproxy):
'''setup RC override control''' """Setup RC override control."""
for chan in [1,2,3,4,5,6,7]: for chan in [1, 2, 3, 4, 5, 6, 7]:
mavproxy.send('rc %u 1500\n' % chan) mavproxy.send('rc %u 1500\n' % chan)
mavproxy.send('rc 8 1800\n') mavproxy.send('rc 8 1800\n')
def drive_mission(mavproxy, mav, filename): def drive_mission(mavproxy, mav, filename):
'''drive a mission from a file''' """Drive a mission from a file."""
global homeloc global homeloc
print("Driving mission %s" % filename) print("Driving mission %s" % filename)
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received') mavproxy.expect('Flight plan received')
mavproxy.send('wp list\n') mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints') mavproxy.expect('Requesting [0-9]+ waypoints')
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
mavproxy.send('rc 3 1500\n') mavproxy.send('rc 3 1500\n')
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
if not wait_waypoint(mav, 1, 4, max_dist=5): if not wait_waypoint(mav, 1, 4, max_dist=5):
@ -79,23 +87,23 @@ def drive_mission(mavproxy, mav, filename):
return True return True
def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False): def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''drive APMrover2 in SIL """Drive APMrover2 in SITL.
you can pass viewerip as an IP address to optionally send fg and you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the mission in real time mavproxy packets too for local viewing of the mission in real time
''' """
global homeloc global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip: if viewerip:
options += " --out=%s:14550" % viewerip options += " --out=%s:14550" % viewerip
if map: if use_map:
options += ' --map' options += ' --map'
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sil = util.start_SIL(binary, wipe=True, model='rover', home=home, speedup=10) sitl = util.start_SITL(binary, wipe=True, model='rover', home=home, speedup=10)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options) mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
print("WAITING FOR PARAMETERS") print("WAITING FOR PARAMETERS")
mavproxy.expect('Received [0-9]+ parameters') mavproxy.expect('Received [0-9]+ parameters')
@ -109,10 +117,10 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
# restart with new parms # restart with new parms
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
sil = util.start_SIL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb) sitl = util.start_SITL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options) mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
mavproxy.expect('Telemetry log: (\S+)') mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1) logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile) print("LOGFILE %s" % logfile)
@ -131,14 +139,14 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
util.expect_setup_callback(mavproxy, expect_callback) util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear() expect_list_clear()
expect_list_extend([sil, mavproxy]) expect_list_extend([sitl, mavproxy])
print("Started simulator") print("Started simulator")
# get a mavlink connection going # get a mavlink connection going
try: try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg: except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise raise
mav.message_hooks.append(message_hook) mav.message_hooks.append(message_hook)
@ -170,15 +178,15 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
# if not drive_RTL(mavproxy, mav): # if not drive_RTL(mavproxy, mav):
# print("Failed RTL") # print("Failed RTL")
# failed = True # failed = True
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT as e:
print("Failed with timeout") print("Failed with timeout")
failed = True failed = True
mav.close() mav.close()
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
valgrind_log = sil.valgrind_log_filepath() valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log): if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644) os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log")) shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log"))

284
Tools/autotest/arducopter.py
View File

@ -7,30 +7,38 @@
# switch 5 = Loiter # switch 5 = Loiter
# switch 6 = Stabilize # switch 6 = Stabilize
import util, pexpect, sys, time, math, shutil, os import math
from common import * import os
import shutil
import time
import pexpect
from pymavlink import mavutil, mavwp from pymavlink import mavutil, mavwp
import random
from common import *
from pysim import util
# get location of scripts # get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__)) testdir = os.path.dirname(os.path.realpath(__file__))
FRAME='+' FRAME = '+'
HOME=mavutil.location(-35.362938,149.165085,584,270) HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
AVCHOME=mavutil.location(40.072842,-105.230575,1586,0) AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
homeloc = None homeloc = None
num_wp = 0 num_wp = 0
speedup_default = 10 speedup_default = 10
def hover(mavproxy, mav, hover_throttle=1500): def hover(mavproxy, mav, hover_throttle=1500):
mavproxy.send('rc 3 %u\n' % hover_throttle) mavproxy.send('rc 3 %u\n' % hover_throttle)
return True return True
def arm_motors(mavproxy, mav): def arm_motors(mavproxy, mav):
'''arm motors''' """Arm motors."""
print("Arming motors") print("Arming motors")
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 2000\n') mavproxy.send('rc 4 2000\n')
@ -40,10 +48,11 @@ def arm_motors(mavproxy, mav):
print("MOTORS ARMED OK") print("MOTORS ARMED OK")
return True return True
def disarm_motors(mavproxy, mav): def disarm_motors(mavproxy, mav):
'''disarm motors''' """Disarm motors."""
print("Disarming motors") print("Disarming motors")
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 1000\n') mavproxy.send('rc 4 1000\n')
@ -54,9 +63,9 @@ def disarm_motors(mavproxy, mav):
return True return True
def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700): def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
'''takeoff get to 30m altitude''' """Takeoff get to 30m altitude."""
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 %u\n' % takeoff_throttle) mavproxy.send('rc 3 %u\n' % takeoff_throttle)
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
@ -66,10 +75,11 @@ def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
print("TAKEOFF COMPLETE") print("TAKEOFF COMPLETE")
return True return True
# loiter - fly south west, then hold loiter within 5m position and altitude # loiter - fly south west, then hold loiter within 5m position and altitude
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5): def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
'''hold loiter position''' """Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# first aim south east # first aim south east
@ -79,7 +89,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
return False return False
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
#fly south east 50m # fly south east 50m
mavproxy.send('rc 2 1100\n') mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 50): if not wait_distance(mav, 50):
return False return False
@ -114,8 +124,9 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
print("Loiter FAILED") print("Loiter FAILED")
return success return success
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080): def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
'''change altitude''' """Change altitude."""
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
if(m.alt < alt_min): if(m.alt < alt_min):
print("Rise to alt:%u from %u" % (alt_min, m.alt)) print("Rise to alt:%u from %u" % (alt_min, m.alt))
@ -124,13 +135,14 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
else: else:
print("Lower to alt:%u from %u" % (alt_min, m.alt)) print("Lower to alt:%u from %u" % (alt_min, m.alt))
mavproxy.send('rc 3 %u\n' % descend_throttle) mavproxy.send('rc 3 %u\n' % descend_throttle)
wait_altitude(mav, (alt_min -5), alt_min) wait_altitude(mav, (alt_min - 5), alt_min)
hover(mavproxy, mav) hover(mavproxy, mav)
return True return True
# fly a square in stabilize mode # fly a square in stabilize mode
def fly_square(mavproxy, mav, side=50, timeout=300): def fly_square(mavproxy, mav, side=50, timeout=300):
'''fly a square, flying N then E''' """Fly a square, flying N then E ."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
success = True success = True
@ -166,7 +178,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
print("Going north %u meters" % side) print("Going north %u meters" % side)
mavproxy.send('rc 2 1300\n') mavproxy.send('rc 2 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side print("Failed to reach distance of %u" % side)
success = False success = False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
@ -178,7 +190,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
print("Going east %u meters" % side) print("Going east %u meters" % side)
mavproxy.send('rc 1 1700\n') mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side print("Failed to reach distance of %u" % side)
success = False success = False
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
@ -190,7 +202,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
print("Going south %u meters" % side) print("Going south %u meters" % side)
mavproxy.send('rc 2 1700\n') mavproxy.send('rc 2 1700\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side print("Failed to reach distance of %u" % side)
success = False success = False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
@ -202,7 +214,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
print("Going west %u meters" % side) print("Going west %u meters" % side)
mavproxy.send('rc 1 1300\n') mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side): if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side print("Failed to reach distance of %u" % side)
success = False success = False
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
@ -212,7 +224,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
# descend to 10m # descend to 10m
print("Descend to 10m in Loiter") print("Descend to 10m in Loiter")
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
mavproxy.send('rc 3 1300\n') mavproxy.send('rc 3 1300\n')
time_left = timeout - (get_sim_time(mav) - tstart) time_left = timeout - (get_sim_time(mav) - tstart)
@ -226,8 +238,9 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
return success return success
def fly_RTL(mavproxy, mav, side=60, timeout=250): def fly_RTL(mavproxy, mav, side=60, timeout=250):
'''Return, land''' """Return, land."""
print("# Enter RTL") print("# Enter RTL")
mavproxy.send('switch 3\n') mavproxy.send('switch 3\n')
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
@ -240,8 +253,9 @@ def fly_RTL(mavproxy, mav, side=60, timeout=250):
return True return True
return False return False
def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180): def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
'''Fly east, Failsafe, return, land''' """Fly east, Failsafe, return, land."""
# switch to loiter mode temporarily to stop us from rising # switch to loiter mode temporarily to stop us from rising
mavproxy.send('switch 5\n') mavproxy.send('switch 5\n')
@ -285,9 +299,9 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
# reduce throttle # reduce throttle
mavproxy.send('rc 3 1100\n') mavproxy.send('rc 3 1100\n')
# switch back to stabilize # switch back to stabilize
mavproxy.send('switch 2\n') # land mode mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
print("Reached failsafe home OK") print("Reached failsafe home OK")
return True return True
@ -295,12 +309,13 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
# reduce throttle # reduce throttle
mavproxy.send('rc 3 1100\n') mavproxy.send('rc 3 1100\n')
# switch back to stabilize mode # switch back to stabilize mode
mavproxy.send('switch 2\n') # land mode mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
return False return False
def fly_battery_failsafe(mavproxy, mav, timeout=30): def fly_battery_failsafe(mavproxy, mav, timeout=30):
# assume failure # assume failure
success = False success = False
@ -332,10 +347,11 @@ def fly_battery_failsafe(mavproxy, mav, timeout=30):
return success return success
# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency # fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10): def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
'''hold loiter position''' """Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# first south # first south
@ -345,7 +361,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
return False return False
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
#fly west 80m # fly west 80m
mavproxy.send('rc 2 1100\n') mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 80): if not wait_distance(mav, 80):
return False return False
@ -384,16 +400,17 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
mavproxy.send('param set SIM_ENGINE_MUL 1.0\n') mavproxy.send('param set SIM_ENGINE_MUL 1.0\n')
if success: if success:
print("Stability patch and Loiter OK for %u seconds" % holdtime) print("Stability patch and Loiter OK for %u seconds" % holdtime)
else: else:
print("Stability Patch FAILED") print("Stability Patch FAILED")
return success return success
# fly_fence_test - fly east until you hit the horizontal circular fence # fly_fence_test - fly east until you hit the horizontal circular fence
def fly_fence_test(mavproxy, mav, timeout=180): def fly_fence_test(mavproxy, mav, timeout=180):
'''hold loiter position''' """Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# enable fence, disable avoidance # enable fence, disable avoidance
@ -421,7 +438,7 @@ def fly_fence_test(mavproxy, mav, timeout=180):
home_distance = get_distance(HOME, pos) home_distance = get_distance(HOME, pos)
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance)) print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
# recenter pitch sticks once we reach home so we don't fly off again # recenter pitch sticks once we reach home so we don't fly off again
if pitching_forward and home_distance < 10 : if pitching_forward and home_distance < 10:
pitching_forward = False pitching_forward = False
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# disable fence # disable fence
@ -430,9 +447,9 @@ def fly_fence_test(mavproxy, mav, timeout=180):
# reduce throttle # reduce throttle
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
# switch mode to stabilize # switch mode to stabilize
mavproxy.send('switch 2\n') # land mode mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
print("Reached home OK") print("Reached home OK")
return True return True
@ -444,15 +461,16 @@ def fly_fence_test(mavproxy, mav, timeout=180):
# reduce throttle # reduce throttle
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
# switch mode to stabilize # switch mode to stabilize
mavproxy.send('switch 2\n') # land mode mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
print("Fence test failed to reach home - timed out after %u seconds" % timeout) print("Fence test failed to reach home - timed out after %u seconds" % timeout)
return False return False
def show_gps_and_sim_positions(mavproxy, on_off): def show_gps_and_sim_positions(mavproxy, on_off):
if on_off == True: if on_off is True:
# turn on simulator display of gps and actual position # turn on simulator display of gps and actual position
mavproxy.send('map set showgpspos 1\n') mavproxy.send('map set showgpspos 1\n')
mavproxy.send('map set showsimpos 1\n') mavproxy.send('map set showsimpos 1\n')
@ -460,25 +478,26 @@ def show_gps_and_sim_positions(mavproxy, on_off):
# turn off simulator display of gps and actual position # turn off simulator display of gps and actual position
mavproxy.send('map set showgpspos 0\n') mavproxy.send('map set showgpspos 0\n')
mavproxy.send('map set showsimpos 0\n') mavproxy.send('map set showsimpos 0\n')
# fly_gps_glitch_loiter_test - fly south east in loiter and test reaction to gps glitch
def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20): def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
'''hold loiter position''' """fly_gps_glitch_loiter_test.
mavproxy.send('switch 5\n') # loiter mode
Fly south east in loiter and test reaction to gps glitch."""
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# turn on simulator display of gps and actual position # turn on simulator display of gps and actual position
if (use_map): if (use_map):
show_gps_and_sim_positions(mavproxy, True) show_gps_and_sim_positions(mavproxy, True)
# set-up gps glitch array # set-up gps glitch array
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221] glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
glitch_num = len(glitch_lat) glitch_num = len(glitch_lat)
print("GPS Glitches:") print("GPS Glitches:")
for i in range(1,glitch_num): for i in range(1, glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i])) print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
# turn south east # turn south east
print("turn south east") print("turn south east")
@ -510,7 +529,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
success = True success = True
# initialise current glitch # initialise current glitch
glitch_current = 0; glitch_current = 0
print("Apply first glitch") print("Apply first glitch")
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
@ -529,7 +548,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n') mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
else: else:
print("Applying glitch %u" % glitch_current) print("Applying glitch %u" % glitch_current)
#move onto the next glitch # move onto the next glitch
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
@ -557,16 +576,17 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
print("GPS glitch test FAILED!") print("GPS glitch test FAILED!")
return success return success
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch # fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120): def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
# set-up gps glitch array # set-up gps glitch array
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221] glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304] glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
glitch_num = len(glitch_lat) glitch_num = len(glitch_lat)
print("GPS Glitches:") print("GPS Glitches:")
for i in range(1,glitch_num): for i in range(1, glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i])) print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
# Fly mission #1 # Fly mission #1
print("# Load copter_glitch_mission") print("# Load copter_glitch_mission")
@ -585,7 +605,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
mavproxy.send('wp set 1\n') mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle # switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n') mavproxy.send('rc 3 1500\n')
@ -600,7 +620,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
tnow = tstart tnow = tstart
# initialise current glitch # initialise current glitch
glitch_current = 0; glitch_current = 0
print("Apply first glitch") print("Apply first glitch")
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current]) mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
@ -647,17 +667,18 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
return ret return ret
#fly_simple - assumes the simple bearing is initialised to be directly north
# fly_simple - assumes the simple bearing is initialised to be directly north
# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south # flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
def fly_simple(mavproxy, mav, side=50, timeout=120): def fly_simple(mavproxy, mav, side=50, timeout=120):
failed = False failed = False
# hold position in loiter # hold position in loiter
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
#set SIMPLE mode for all flight modes # set SIMPLE mode for all flight modes
mavproxy.send('param set SIMPLE 63\n') mavproxy.send('param set SIMPLE 63\n')
# switch to stabilize mode # switch to stabilize mode
@ -679,7 +700,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
while get_sim_time(mav) < (tstart + 8): while get_sim_time(mav) < (tstart + 8):
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (get_sim_time(mav) - tstart) delta = (get_sim_time(mav) - tstart)
#print("%u" % delta) # print("%u" % delta)
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# fly north 25 meters # fly north 25 meters
@ -696,23 +717,24 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
while get_sim_time(mav) < (tstart + 8): while get_sim_time(mav) < (tstart + 8):
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (get_sim_time(mav) - tstart) delta = (get_sim_time(mav) - tstart)
#print("%u" % delta) # print("%u" % delta)
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
#restore to default # restore to default
mavproxy.send('param set SIMPLE 0\n') mavproxy.send('param set SIMPLE 0\n')
#hover in place # hover in place
hover(mavproxy, mav) hover(mavproxy, mav)
return not failed return not failed
#fly_super_simple - flies a circle around home for 45 seconds
# fly_super_simple - flies a circle around home for 45 seconds
def fly_super_simple(mavproxy, mav, timeout=45): def fly_super_simple(mavproxy, mav, timeout=45):
failed = False failed = False
# hold position in loiter # hold position in loiter
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# fly forward 20m # fly forward 20m
@ -722,7 +744,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
failed = True failed = True
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
#set SUPER SIMPLE mode for all flight modes # set SUPER SIMPLE mode for all flight modes
mavproxy.send('param set SUPER_SIMPLE 63\n') mavproxy.send('param set SUPER_SIMPLE 63\n')
# switch to stabilize mode # switch to stabilize mode
@ -745,18 +767,19 @@ def fly_super_simple(mavproxy, mav, timeout=45):
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
#restore simple mode parameters to default # restore simple mode parameters to default
mavproxy.send('param set SUPER_SIMPLE 0\n') mavproxy.send('param set SUPER_SIMPLE 0\n')
#hover in place # hover in place
hover(mavproxy, mav) hover(mavproxy, mav)
return not failed return not failed
#fly_circle - flies a circle with 20m radius
# fly_circle - flies a circle with 20m radius
def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36): def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
# hold position in loiter # hold position in loiter
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
# face west # face west
@ -766,7 +789,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
return False return False
mavproxy.send('rc 4 1500\n') mavproxy.send('rc 4 1500\n')
#set CIRCLE radius # set CIRCLE radius
mavproxy.send('param set CIRCLE_RADIUS 3000\n') mavproxy.send('param set CIRCLE_RADIUS 3000\n')
# fly forward (east) at least 100m # fly forward (east) at least 100m
@ -778,10 +801,10 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
mavproxy.send('rc 2 1500\n') mavproxy.send('rc 2 1500\n')
# set CIRCLE mode # set CIRCLE mode
mavproxy.send('switch 1\n') # circle mode mavproxy.send('switch 1\n') # circle mode
wait_mode(mav, 'CIRCLE') wait_mode(mav, 'CIRCLE')
# wait # wait
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt start_altitude = m.alt
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
@ -794,6 +817,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
print("CIRCLE OK for %u seconds" % holdtime) print("CIRCLE OK for %u seconds" % holdtime)
return True return True
# fly_auto_test - fly mission which tests a significant number of commands # fly_auto_test - fly mission which tests a significant number of commands
def fly_auto_test(mavproxy, mav): def fly_auto_test(mavproxy, mav):
@ -810,7 +834,7 @@ def fly_auto_test(mavproxy, mav):
mavproxy.send('wp set 1\n') mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle # switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n') mavproxy.send('rc 3 1500\n')
@ -818,9 +842,9 @@ def fly_auto_test(mavproxy, mav):
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
# land if mission failed # land if mission failed
if ret == False: if ret is False:
land(mavproxy, mav) land(mavproxy, mav)
# set throttle to minimum # set throttle to minimum
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
@ -832,9 +856,10 @@ def fly_auto_test(mavproxy, mav):
return ret return ret
# fly_avc_test - fly AVC mission # fly_avc_test - fly AVC mission
def fly_avc_test(mavproxy, mav): def fly_avc_test(mavproxy, mav):
# upload mission from file # upload mission from file
print("# Load copter_AVC2013_mission") print("# Load copter_AVC2013_mission")
if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")): if not load_mission_from_file(mavproxy, mav, os.path.join(testdir, "copter_AVC2013_mission.txt")):
@ -848,7 +873,7 @@ def fly_avc_test(mavproxy, mav):
mavproxy.send('wp set 1\n') mavproxy.send('wp set 1\n')
# switch into AUTO mode and raise throttle # switch into AUTO mode and raise throttle
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
mavproxy.send('rc 3 1500\n') mavproxy.send('rc 3 1500\n')
@ -866,23 +891,25 @@ def fly_avc_test(mavproxy, mav):
return ret return ret
def land(mavproxy, mav, timeout=60): def land(mavproxy, mav, timeout=60):
'''land the quad''' """Land the quad."""
print("STARTING LANDING") print("STARTING LANDING")
mavproxy.send('switch 2\n') # land mode mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
print("Entered Landing Mode") print("Entered Landing Mode")
ret = wait_altitude(mav, -5, 1) ret = wait_altitude(mav, -5, 1)
print("LANDING: ok= %s" % ret) print("LANDING: ok= %s" % ret)
return ret return ret
def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file''' def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
"""Fly a mission from a file."""
global homeloc global homeloc
global num_wp global num_wp
print("test: Fly a mission from 1 to %u" % num_wp) print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n') mavproxy.send('wp set 1\n')
mavproxy.send('switch 4\n') # auto mode mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500) ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
expect_msg = "Reached command #%u" % (num_wp-1) expect_msg = "Reached command #%u" % (num_wp-1)
@ -890,12 +917,13 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
mavproxy.expect(expect_msg) mavproxy.expect(expect_msg)
print("test: MISSION COMPLETE: passed=%s" % ret) print("test: MISSION COMPLETE: passed=%s" % ret)
# wait here until ready # wait here until ready
mavproxy.send('switch 5\n') # loiter mode mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
return ret return ret
def load_mission_from_file(mavproxy, mav, filename): def load_mission_from_file(mavproxy, mav, filename):
'''Load a mission from a file to flight controller''' """Load a mission from a file to flight controller."""
global num_wp global num_wp
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received') mavproxy.expect('Flight plan received')
@ -908,6 +936,7 @@ def load_mission_from_file(mavproxy, mav, filename):
num_wp = wploader.count() num_wp = wploader.count()
return True return True
def save_mission_to_file(mavproxy, mav, filename): def save_mission_to_file(mavproxy, mav, filename):
global num_wp global num_wp
mavproxy.send('wp save %s\n' % filename) mavproxy.send('wp save %s\n' % filename)
@ -916,24 +945,26 @@ def save_mission_to_file(mavproxy, mav, filename):
print("num_wp: %d" % num_wp) print("num_wp: %d" % num_wp)
return True return True
def setup_rc(mavproxy): def setup_rc(mavproxy):
'''setup RC override control''' """Setup RC override control."""
for chan in range(1,9): for chan in range(1, 9):
mavproxy.send('rc %u 1500\n' % chan) mavproxy.send('rc %u 1500\n' % chan)
# zero throttle # zero throttle
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False): def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL """Fly ArduCopter in SITL.
you can pass viewerip as an IP address to optionally send fg and you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time mavproxy packets too for local viewing of the flight in real time
''' """
global homeloc global homeloc
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sil = util.start_SIL(binary, wipe=True, model='+', home=home, speedup=speedup_default) sitl = util.start_SITL(binary, wipe=True, model='+', home=home, speedup=speedup_default)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter') mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
mavproxy.expect('Received [0-9]+ parameters') mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters # setup test parameters
@ -945,29 +976,29 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
# reboot with new parameters # reboot with new parameters
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
sil = util.start_SIL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb) sitl = util.start_SITL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5' options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
if viewerip: if viewerip:
options += ' --out=%s:14550' % viewerip options += ' --out=%s:14550' % viewerip
if use_map: if use_map:
options += ' --map' options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)') mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1) logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile) print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog") buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog) print("buildlog=%s" % buildlog)
copyTLog = False copy_tlog = False
if os.path.exists(buildlog): if os.path.exists(buildlog):
os.unlink(buildlog) os.unlink(buildlog)
try: try:
os.link(logfile, buildlog) os.link(logfile, buildlog)
except Exception: except Exception:
print( "WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location" ) print("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location")
copyTLog = True copy_tlog = True
# the received parameters can come before or after the ready to fly message # the received parameters can come before or after the ready to fly message
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
@ -976,18 +1007,17 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
util.expect_setup_callback(mavproxy, expect_callback) util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear() expect_list_clear()
expect_list_extend([sil, mavproxy]) expect_list_extend([sitl, mavproxy])
# get a mavlink connection going # get a mavlink connection going
try: try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg: except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise raise
mav.message_hooks.append(message_hook) mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook) mav.idle_hooks.append(idle_hook)
failed = False failed = False
failed_test_msg = "None" failed_test_msg = "None"
@ -1030,7 +1060,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
# fly the stored mission # fly the stored mission
print("# Fly CH7 saved mission") print("# Fly CH7 saved mission")
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10): if not fly_mission(mavproxy, mav, height_accuracy=0.5, target_altitude=10):
failed_test_msg = "fly ch7_mission failed" failed_test_msg = "fly ch7_mission failed"
print(failed_test_msg) print(failed_test_msg)
failed = True failed = True
@ -1250,38 +1280,37 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
print(failed_test_msg) print(failed_test_msg)
failed = True failed = True
except pexpect.TIMEOUT, failed_test_msg: except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout" failed_test_msg = "Timeout"
failed = True failed = True
mav.close() mav.close()
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
valgrind_log = sil.valgrind_log_filepath() valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log): if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644) os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log")) shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
# [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link # [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link
# This flag tells me that I need to copy the data out # This flag tells me that I need to copy the data out
if copyTLog: if copy_tlog:
shutil.copy(logfile, buildlog) shutil.copy(logfile, buildlog)
if failed: if failed:
print("FAILED: %s" % failed_test_msg) print("FAILED: %s" % failed_test_msg)
return False return False
return True return True
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False): def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL for AVC2013 mission """Fly ArduCopter in SITL for AVC2013 mission."""
'''
global homeloc global homeloc
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading) home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
sil = util.start_SIL(binary, wipe=True, model='heli', home=home, speedup=speedup_default) sitl = util.start_SITL(binary, wipe=True, model='heli', home=home, speedup=speedup_default)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550') mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
mavproxy.expect('Received [0-9]+ parameters') mavproxy.expect('Received [0-9]+ parameters')
# setup test parameters # setup test parameters
@ -1293,15 +1322,15 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
# reboot with new parameters # reboot with new parameters
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
sil = util.start_SIL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb) sitl = util.start_SITL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5' options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
if viewerip: if viewerip:
options += ' --out=%s:14550' % viewerip options += ' --out=%s:14550' % viewerip
if map: if use_map:
options += ' --map' options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)') mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1) logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile) print("LOGFILE %s" % logfile)
@ -1322,22 +1351,21 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
util.expect_setup_callback(mavproxy, expect_callback) util.expect_setup_callback(mavproxy, expect_callback)
expect_list_clear() expect_list_clear()
expect_list_extend([sil, mavproxy]) expect_list_extend([sitl, mavproxy])
if map: if use_map:
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n') mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n') mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going # get a mavlink connection going
try: try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg: except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise raise
mav.message_hooks.append(message_hook) mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook) mav.idle_hooks.append(idle_hook)
failed = False failed = False
failed_test_msg = "None" failed_test_msg = "None"
@ -1373,21 +1401,21 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
print("Lowering rotor speed") print("Lowering rotor speed")
mavproxy.send('rc 8 1000\n') mavproxy.send('rc 8 1000\n')
#mission includes disarm at end so should be ok to download logs now # 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")): if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
failed_test_msg = "log_download failed" failed_test_msg = "log_download failed"
print(failed_test_msg) print(failed_test_msg)
failed = True failed = True
except pexpect.TIMEOUT, failed_test_msg: except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout" failed_test_msg = "Timeout"
failed = True failed = True
mav.close() mav.close()
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
valgrind_log = sil.valgrind_log_filepath() valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log): if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644) os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log")) shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log"))

93
Tools/autotest/arduplane.py
View File

@ -1,28 +1,34 @@
# fly ArduPlane in SIL # Fly ArduPlane in SITL
import util, pexpect, sys, time, math, shutil, os import math
from common import * import os
import shutil
import pexpect
from pymavlink import mavutil from pymavlink import mavutil
import random
from common import *
from pysim import util
# get location of scripts # get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__)) testdir = os.path.dirname(os.path.realpath(__file__))
HOME_LOCATION='-35.362938,149.165085,585,354' HOME_LOCATION = '-35.362938,149.165085,585,354'
WIND="0,180,0.2" # speed,direction,variance WIND = "0,180,0.2" # speed,direction,variance
homeloc = None homeloc = None
def takeoff(mavproxy, mav): def takeoff(mavproxy, mav):
'''takeoff get to 30m altitude''' """Takeoff get to 30m altitude."""
# wait for EKF and GPS checks to pass # wait for EKF and GPS checks to pass
wait_seconds(mav, 30) wait_seconds(mav, 30)
mavproxy.send('arm throttle\n') mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED') mavproxy.expect('ARMED')
mavproxy.send('switch 4\n') mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA') wait_mode(mav, 'FBWA')
@ -55,8 +61,9 @@ def takeoff(mavproxy, mav):
print("TAKEOFF COMPLETE") print("TAKEOFF COMPLETE")
return True return True
def fly_left_circuit(mavproxy, mav): def fly_left_circuit(mavproxy, mav):
'''fly a left circuit, 200m on a side''' """Fly a left circuit, 200m on a side."""
mavproxy.send('switch 4\n') mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA') wait_mode(mav, 'FBWA')
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
@ -65,7 +72,7 @@ def fly_left_circuit(mavproxy, mav):
print("Flying left circuit") print("Flying left circuit")
# do 4 turns # do 4 turns
for i in range(0,4): for i in range(0, 4):
# hard left # hard left
print("Starting turn %u" % i) print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n') mavproxy.send('rc 1 1000\n')
@ -78,8 +85,9 @@ def fly_left_circuit(mavproxy, mav):
print("Circuit complete") print("Circuit complete")
return True return True
def fly_RTL(mavproxy, mav): def fly_RTL(mavproxy, mav):
'''fly to home''' """Fly to home."""
print("Flying home in RTL") print("Flying home in RTL")
mavproxy.send('switch 2\n') mavproxy.send('switch 2\n')
wait_mode(mav, 'RTL') wait_mode(mav, 'RTL')
@ -90,8 +98,9 @@ def fly_RTL(mavproxy, mav):
print("RTL Complete") print("RTL Complete")
return True return True
def fly_LOITER(mavproxy, mav, num_circles=4): def fly_LOITER(mavproxy, mav, num_circles=4):
'''loiter where we are''' """Loiter where we are."""
print("Testing LOITER for %u turns" % num_circles) print("Testing LOITER for %u turns" % num_circles)
mavproxy.send('loiter\n') mavproxy.send('loiter\n')
wait_mode(mav, 'LOITER') wait_mode(mav, 'LOITER')
@ -122,8 +131,9 @@ def fly_LOITER(mavproxy, mav, num_circles=4):
print("Completed Loiter OK") print("Completed Loiter OK")
return True return True
def fly_CIRCLE(mavproxy, mav, num_circles=1): def fly_CIRCLE(mavproxy, mav, num_circles=1):
'''circle where we are''' """Circle where we are."""
print("Testing CIRCLE for %u turns" % num_circles) print("Testing CIRCLE for %u turns" % num_circles)
mavproxy.send('mode CIRCLE\n') mavproxy.send('mode CIRCLE\n')
wait_mode(mav, 'CIRCLE') wait_mode(mav, 'CIRCLE')
@ -156,7 +166,7 @@ def fly_CIRCLE(mavproxy, mav, num_circles=1):
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30): def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
'''wait for level flight''' """Wait for level flight."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for level flight") print("Waiting for level flight")
mavproxy.send('rc 1 1500\n') mavproxy.send('rc 1 1500\n')
@ -175,7 +185,7 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
def change_altitude(mavproxy, mav, altitude, accuracy=30): def change_altitude(mavproxy, mav, altitude, accuracy=30):
'''get to a given altitude''' """Get to a given altitude."""
mavproxy.send('mode FBWA\n') mavproxy.send('mode FBWA\n')
wait_mode(mav, 'FBWA') wait_mode(mav, 'FBWA')
alt_error = mav.messages['VFR_HUD'].alt - altitude alt_error = mav.messages['VFR_HUD'].alt - altitude
@ -191,7 +201,7 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
def axial_left_roll(mavproxy, mav, count=1): def axial_left_roll(mavproxy, mav, count=1):
'''fly a left axial roll''' """Fly a left axial roll."""
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
if not change_altitude(mavproxy, mav, homeloc.alt+300): if not change_altitude(mavproxy, mav, homeloc.alt+300):
@ -224,7 +234,7 @@ def axial_left_roll(mavproxy, mav, count=1):
def inside_loop(mavproxy, mav, count=1): def inside_loop(mavproxy, mav, count=1):
'''fly a inside loop''' """Fly a inside loop."""
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
if not change_altitude(mavproxy, mav, homeloc.alt+300): if not change_altitude(mavproxy, mav, homeloc.alt+300):
@ -252,7 +262,7 @@ def inside_loop(mavproxy, mav, count=1):
def test_stabilize(mavproxy, mav, count=1): def test_stabilize(mavproxy, mav, count=1):
'''fly stabilize mode''' """Fly stabilize mode."""
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
mavproxy.send('rc 2 1300\n') mavproxy.send('rc 2 1300\n')
@ -285,8 +295,9 @@ def test_stabilize(mavproxy, mav, count=1):
mavproxy.send('rc 3 1700\n') mavproxy.send('rc 3 1700\n')
return wait_level_flight(mavproxy, mav) return wait_level_flight(mavproxy, mav)
def test_acro(mavproxy, mav, count=1): def test_acro(mavproxy, mav, count=1):
'''fly ACRO mode''' """Fly ACRO mode."""
# full throttle! # full throttle!
mavproxy.send('rc 3 2000\n') mavproxy.send('rc 3 2000\n')
mavproxy.send('rc 2 1300\n') mavproxy.send('rc 2 1300\n')
@ -339,7 +350,7 @@ def test_acro(mavproxy, mav, count=1):
def test_FBWB(mavproxy, mav, count=1, mode='FBWB'): def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
'''fly FBWB or CRUISE mode''' """Fly FBWB or CRUISE mode."""
mavproxy.send("mode %s\n" % mode) mavproxy.send("mode %s\n" % mode)
wait_mode(mav, mode) wait_mode(mav, mode)
mavproxy.send('rc 3 1700\n') mavproxy.send('rc 3 1700\n')
@ -357,7 +368,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
print("Flying right circuit") print("Flying right circuit")
# do 4 turns # do 4 turns
for i in range(0,4): for i in range(0, 4):
# hard left # hard left
print("Starting turn %u" % i) print("Starting turn %u" % i)
mavproxy.send('rc 1 1800\n') mavproxy.send('rc 1 1800\n')
@ -372,7 +383,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
print("Flying rudder left circuit") print("Flying rudder left circuit")
# do 4 turns # do 4 turns
for i in range(0,4): for i in range(0, 4):
# hard left # hard left
print("Starting turn %u" % i) print("Starting turn %u" % i)
mavproxy.send('rc 4 1900\n') mavproxy.send('rc 4 1900\n')
@ -396,27 +407,27 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
if abs(final_alt - initial_alt) > 20: if abs(final_alt - initial_alt) > 20:
print("Failed to maintain altitude") print("Failed to maintain altitude")
return False return False
return wait_level_flight(mavproxy, mav) return wait_level_flight(mavproxy, mav)
def setup_rc(mavproxy): def setup_rc(mavproxy):
'''setup RC override control''' """Setup RC override control."""
for chan in [1,2,4,5,6,7]: for chan in [1, 2, 4, 5, 6, 7]:
mavproxy.send('rc %u 1500\n' % chan) mavproxy.send('rc %u 1500\n' % chan)
mavproxy.send('rc 3 1000\n') mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 8 1800\n') mavproxy.send('rc 8 1800\n')
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None): def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file''' """Fly a mission from a file."""
global homeloc global homeloc
print("Flying mission %s" % filename) print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received') mavproxy.expect('Flight plan received')
mavproxy.send('wp list\n') mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints') mavproxy.expect('Requesting [0-9]+ waypoints')
mavproxy.send('switch 1\n') # auto mode mavproxy.send('switch 1\n') # auto mode
wait_mode(mav, 'AUTO') wait_mode(mav, 'AUTO')
if not wait_waypoint(mav, 1, 7, max_dist=60): if not wait_waypoint(mav, 1, 7, max_dist=60):
return False return False
@ -426,24 +437,24 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non
return True return True
def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False): def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''fly ArduPlane in SIL """Fly ArduPlane in SITL.
you can pass viewerip as an IP address to optionally send fg and you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time mavproxy packets too for local viewing of the flight in real time
''' """
global homeloc global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip: if viewerip:
options += " --out=%s:14550" % viewerip options += " --out=%s:14550" % viewerip
if map: if use_map:
options += ' --map' options += ' --map'
sil = util.start_SIL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10, sitl = util.start_SITL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10,
valgrind=valgrind, gdb=gdb, valgrind=valgrind, gdb=gdb,
defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm')) defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'))
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options) mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)') mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1) logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile) print("LOGFILE %s" % logfile)
@ -462,14 +473,14 @@ def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
mavproxy.expect('Received [0-9]+ parameters') mavproxy.expect('Received [0-9]+ parameters')
expect_list_clear() expect_list_clear()
expect_list_extend([sil, mavproxy]) expect_list_extend([sitl, mavproxy])
print("Started simulator") print("Started simulator")
# get a mavlink connection going # get a mavlink connection going
try: try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg: except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise raise
mav.message_hooks.append(message_hook) mav.message_hooks.append(message_hook)
@ -543,16 +554,16 @@ def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
print("Failed log download") print("Failed log download")
failed = True failed = True
fail_list.append("log_download") fail_list.append("log_download")
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT as e:
print("Failed with timeout") print("Failed with timeout")
failed = True failed = True
fail_list.append("timeout") fail_list.append("timeout")
mav.close() mav.close()
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
valgrind_log = sil.valgrind_log_filepath() valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log): if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644) os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log")) shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log"))

157
Tools/autotest/autotest.py
View File

@ -1,87 +1,104 @@
#!/usr/bin/env python #!/usr/bin/env python
# APM automatic test suite """
# Andrew Tridgell, October 2011 APM automatic test suite
Andrew Tridgell, October 2011
"""
import pexpect, os, sys, shutil, atexit import atexit
import optparse, fnmatch, time, glob, traceback, signal import fnmatch
import glob
import optparse
import os
import shutil
import signal
import sys
import time
import traceback
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim')) import apmrover2
import arducopter
import util import arduplane
import quadplane
from pysim import util
os.environ['PYTHONUNBUFFERED'] = '1' os.environ['PYTHONUNBUFFERED'] = '1'
os.putenv('TMPDIR', util.reltopdir('tmp')) os.putenv('TMPDIR', util.reltopdir('tmp'))
def get_default_params(atype, binary): def get_default_params(atype, binary):
'''get default parameters''' """Get default parameters."""
# use rover simulator so SITL is not starved of input # use rover simulator so SITL is not starved of input
from pymavlink import mavutil from pymavlink import mavutil
HOME=mavutil.location(40.071374969556928,-105.22978898137808,1583.702759,246) HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
if binary.find("plane") != -1 or binary.find("rover") != -1: if binary.find("plane") != -1 or binary.find("rover") != -1:
frame = "rover" frame = "rover"
else: else:
frame = "+" frame = "+"
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading) home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
sil = util.start_SIL(binary, wipe=True, model=frame, home=home, speedup=10) sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=10)
mavproxy = util.start_MAVProxy_SIL(atype) mavproxy = util.start_MAVProxy_SITL(atype)
print("Dumping defaults") print("Dumping defaults")
idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)']) idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
if idx == 0: if idx == 0:
# we need to restart it after eeprom erase # we need to restart it after eeprom erase
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
sil = util.start_SIL(binary, model=frame, home=home, speedup=10) sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
mavproxy = util.start_MAVProxy_SIL(atype) mavproxy = util.start_MAVProxy_SITL(atype)
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)') idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
parmfile = mavproxy.match.group(1) parmfile = mavproxy.match.group(1)
dest = util.reltopdir('../buildlogs/%s-defaults.parm' % atype) dest = util.reltopdir('../buildlogs/%s-defaults.parm' % atype)
shutil.copy(parmfile, dest) shutil.copy(parmfile, dest)
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
print("Saved defaults for %s to %s" % (atype, dest)) print("Saved defaults for %s to %s" % (atype, dest))
return True return True
def build_all(): def build_all():
'''run the build_all.sh script''' """Run the build_all.sh script."""
print("Running build_all.sh") print("Running build_all.sh")
if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')) != 0: if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), directory=util.reltopdir('.')) != 0:
print("Failed build_all.sh") print("Failed build_all.sh")
return False return False
return True return True
def build_binaries(): def build_binaries():
'''run the build_binaries.sh script''' """Run the build_binaries.sh script."""
print("Running build_binaries.sh") print("Running build_binaries.sh")
import shutil import shutil
# copy the script as it changes git branch, which can change the script while running # copy the script as it changes git branch, which can change the script while running
orig=util.reltopdir('Tools/scripts/build_binaries.sh') orig = util.reltopdir('Tools/scripts/build_binaries.sh')
copy=util.reltopdir('./build_binaries.sh') copy = util.reltopdir('./build_binaries.sh')
shutil.copyfile(orig, copy) shutil.copyfile(orig, copy)
shutil.copymode(orig, copy) shutil.copymode(orig, copy)
if util.run_cmd(copy, dir=util.reltopdir('.')) != 0: if util.run_cmd(copy, directory=util.reltopdir('.')) != 0:
print("Failed build_binaries.sh") print("Failed build_binaries.sh")
return False return False
return True return True
def build_devrelease(): def build_devrelease():
'''run the build_devrelease.sh script''' """Run the build_devrelease.sh script."""
print("Running build_devrelease.sh") print("Running build_devrelease.sh")
import shutil import shutil
# copy the script as it changes git branch, which can change the script while running # copy the script as it changes git branch, which can change the script while running
orig=util.reltopdir('Tools/scripts/build_devrelease.sh') orig = util.reltopdir('Tools/scripts/build_devrelease.sh')
copy=util.reltopdir('./build_devrelease.sh') copy = util.reltopdir('./build_devrelease.sh')
shutil.copyfile(orig, copy) shutil.copyfile(orig, copy)
shutil.copymode(orig, copy) shutil.copymode(orig, copy)
if util.run_cmd(copy, dir=util.reltopdir('.')) != 0: if util.run_cmd(copy, directory=util.reltopdir('.')) != 0:
print("Failed build_devrelease.sh") print("Failed build_devrelease.sh")
return False return False
return True return True
def build_examples(): def build_examples():
'''build examples''' """Build examples."""
for target in 'px4-v2', 'navio': for target in 'px4-v2', 'navio':
print("Running build.examples for %s" % target) print("Running build.examples for %s" % target)
try: try:
@ -93,17 +110,18 @@ def build_examples():
return True return True
def build_parameters(): def build_parameters():
'''run the param_parse.py script''' """Run the param_parse.py script."""
print("Running param_parse.py") print("Running param_parse.py")
if util.run_cmd(util.reltopdir('Tools/autotest/param_metadata/param_parse.py'), dir=util.reltopdir('.')) != 0: if util.run_cmd(util.reltopdir('Tools/autotest/param_metadata/param_parse.py'), directory=util.reltopdir('.')) != 0:
print("Failed param_parse.py") print("Failed param_parse.py")
return False return False
return True return True
def convert_gpx(): def convert_gpx():
'''convert any tlog files to GPX and KML''' """Convert any tlog files to GPX and KML."""
import glob import glob
mavlog = glob.glob(util.reltopdir("../buildlogs/*.tlog")) mavlog = glob.glob(util.reltopdir("../buildlogs/*.tlog"))
for m in mavlog: for m in mavlog:
@ -112,18 +130,19 @@ def convert_gpx():
kml = m + '.kml' kml = m + '.kml'
util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1,extrude=1 -F %s' % (gpx, kml), checkfail=False) util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1,extrude=1 -F %s' % (gpx, kml), checkfail=False)
util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False) util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False)
util.run_cmd("mavflightview.py --imagefile=%s.png %s" % (m,m)) util.run_cmd("mavflightview.py --imagefile=%s.png %s" % (m, m))
return True return True
def test_prerequisites(): def test_prerequisites():
'''check we have the right directories and tools to run tests''' """Check we have the right directories and tools to run tests."""
print("Testing prerequisites") print("Testing prerequisites")
util.mkdir_p(util.reltopdir('../buildlogs')) util.mkdir_p(util.reltopdir('../buildlogs'))
return True return True
def alarm_handler(signum, frame): def alarm_handler(signum, frame):
'''handle test timeout''' """Handle test timeout."""
global results, opts global results, opts
try: try:
results.add('TIMEOUT', '<span class="failed-text">FAILED</span>', opts.timeout) results.add('TIMEOUT', '<span class="failed-text">FAILED</span>', opts.timeout)
@ -135,6 +154,7 @@ def alarm_handler(signum, frame):
pass pass
sys.exit(1) sys.exit(1)
############## main program ############# ############## main program #############
parser = optparse.OptionParser("autotest") parser = optparse.OptionParser("autotest")
parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)') parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)')
@ -150,13 +170,12 @@ parser.add_option("-j", default=None, type='int', help='build CPUs')
opts, args = parser.parse_args() opts, args = parser.parse_args()
import arducopter, arduplane, apmrover2, quadplane
steps = [ steps = [
'prerequisites', 'prerequisites',
'build.All', 'build.All',
'build.Binaries', 'build.Binaries',
# 'build.DevRelease', # 'build.DevRelease',
'build.Examples', 'build.Examples',
'build.Parameters', 'build.Parameters',
@ -192,13 +211,15 @@ if opts.list:
print(step) print(step)
sys.exit(0) sys.exit(0)
def skip_step(step): def skip_step(step):
'''see if a step should be skipped''' """See if a step should be skipped."""
for skip in skipsteps: for skip in skipsteps:
if fnmatch.fnmatch(step.lower(), skip.lower()): if fnmatch.fnmatch(step.lower(), skip.lower()):
return True return True
return False return False
def binary_path(step, debug=False): def binary_path(step, debug=False):
if step.find("ArduCopter") != -1: if step.find("ArduCopter") != -1:
binary_name = "arducopter-quad" binary_name = "arducopter-quad"
@ -224,14 +245,15 @@ def binary_path(step, debug=False):
binary = util.reltopdir(os.path.join('build', binary_basedir, 'bin', binary_name)) binary = util.reltopdir(os.path.join('build', binary_basedir, 'bin', binary_name))
if not os.path.exists(binary): if not os.path.exists(binary):
if os.path.exists(binary + ".exe"): if os.path.exists(binary + ".exe"):
binary_path += ".exe" binary += ".exe"
else: else:
raise ValueError("Binary (%s) does not exist" % (binary,)) raise ValueError("Binary (%s) does not exist" % (binary,))
return binary return binary
def run_step(step): def run_step(step):
'''run one step''' """Run one step."""
# remove old logs # remove old logs
util.run_cmd('/bin/rm -f logs/*.BIN logs/LASTLOG.TXT') util.run_cmd('/bin/rm -f logs/*.BIN logs/LASTLOG.TXT')
@ -240,19 +262,19 @@ def run_step(step):
return test_prerequisites() return test_prerequisites()
if step == 'build.ArduPlane': if step == 'build.ArduPlane':
return util.build_SIL('bin/arduplane', j=opts.j, debug=opts.debug) return util.build_SITL('bin/arduplane', j=opts.j, debug=opts.debug)
if step == 'build.APMrover2': if step == 'build.APMrover2':
return util.build_SIL('bin/ardurover', j=opts.j, debug=opts.debug) return util.build_SITL('bin/ardurover', j=opts.j, debug=opts.debug)
if step == 'build.ArduCopter': if step == 'build.ArduCopter':
return util.build_SIL('bin/arducopter-quad', j=opts.j, debug=opts.debug) return util.build_SITL('bin/arducopter-quad', j=opts.j, debug=opts.debug)
if step == 'build.AntennaTracker': if step == 'build.AntennaTracker':
return util.build_SIL('bin/antennatracker', j=opts.j, debug=opts.debug) return util.build_SITL('bin/antennatracker', j=opts.j, debug=opts.debug)
if step == 'build.Helicopter': if step == 'build.Helicopter':
return util.build_SIL('bin/arducopter-heli', j=opts.j, debug=opts.debug) return util.build_SITL('bin/arducopter-heli', j=opts.j, debug=opts.debug)
binary = binary_path(step, debug=opts.debug) binary = binary_path(step, debug=opts.debug)
@ -269,16 +291,16 @@ def run_step(step):
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb) return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.CopterAVC': if step == 'fly.CopterAVC':
return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb) return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.ArduPlane': if step == 'fly.ArduPlane':
return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb) return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'fly.QuadPlane': if step == 'fly.QuadPlane':
return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb) return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'drive.APMrover2': if step == 'drive.APMrover2':
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb) return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
if step == 'build.All': if step == 'build.All':
return build_all() return build_all()
@ -300,68 +322,71 @@ def run_step(step):
raise RuntimeError("Unknown step %s" % step) raise RuntimeError("Unknown step %s" % step)
class TestResult(object): class TestResult(object):
'''test result class''' """Test result class."""
def __init__(self, name, result, elapsed): def __init__(self, name, result, elapsed):
self.name = name self.name = name
self.result = result self.result = result
self.elapsed = "%.1f" % elapsed self.elapsed = "%.1f" % elapsed
class TestFile(object): class TestFile(object):
'''test result file''' """Test result file."""
def __init__(self, name, fname): def __init__(self, name, fname):
self.name = name self.name = name
self.fname = fname self.fname = fname
class TestResults(object): class TestResults(object):
'''test results class''' """Test results class."""
def __init__(self): def __init__(self):
self.date = time.asctime() self.date = time.asctime()
self.githash = util.run_cmd('git rev-parse HEAD', output=True, dir=util.reltopdir('.')).strip() self.githash = util.run_cmd('git rev-parse HEAD', output=True, directory=util.reltopdir('.')).strip()
self.tests = [] self.tests = []
self.files = [] self.files = []
self.images = [] self.images = []
def add(self, name, result, elapsed): def add(self, name, result, elapsed):
'''add a result''' """Add a result."""
self.tests.append(TestResult(name, result, elapsed)) self.tests.append(TestResult(name, result, elapsed))
def addfile(self, name, fname): def addfile(self, name, fname):
'''add a result file''' """Add a result file."""
self.files.append(TestFile(name, fname)) self.files.append(TestFile(name, fname))
def addimage(self, name, fname): def addimage(self, name, fname):
'''add a result image''' """Add a result image."""
self.images.append(TestFile(name, fname)) self.images.append(TestFile(name, fname))
def addglob(self, name, pattern): def addglob(self, name, pattern):
'''add a set of files''' """Add a set of files."""
import glob import glob
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)): for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
self.addfile(name, os.path.basename(f)) self.addfile(name, os.path.basename(f))
def addglobimage(self, name, pattern): def addglobimage(self, name, pattern):
'''add a set of images''' """Add a set of images."""
import glob import glob
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)): for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
self.addimage(name, os.path.basename(f)) self.addimage(name, os.path.basename(f))
def write_webresults(results_to_write):
def write_webresults(results): """Write webpage results."""
'''write webpage results'''
from pymavlink.generator import mavtemplate from pymavlink.generator import mavtemplate
t = mavtemplate.MAVTemplate() t = mavtemplate.MAVTemplate()
for h in glob.glob(util.reltopdir('Tools/autotest/web/*.html')): for h in glob.glob(util.reltopdir('Tools/autotest/web/*.html')):
html = util.loadfile(h) html = util.loadfile(h)
f = open(util.reltopdir("../buildlogs/%s" % os.path.basename(h)), mode='w') f = open(util.reltopdir("../buildlogs/%s" % os.path.basename(h)), mode='w')
t.write(f, html, results) t.write(f, html, results_to_write)
f.close() f.close()
for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')): for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')):
shutil.copy(f, util.reltopdir('../buildlogs/%s' % os.path.basename(f))) shutil.copy(f, util.reltopdir('../buildlogs/%s' % os.path.basename(f)))
def write_fullresults(): def write_fullresults():
'''write out full results set''' """Write out full results set."""
global results global results
results.addglob("Google Earth track", '*.kmz') results.addglob("Google Earth track", '*.kmz')
results.addfile('Full Logs', 'autotest-output.txt') results.addfile('Full Logs', 'autotest-output.txt')
@ -406,8 +431,9 @@ def write_fullresults():
results = TestResults() results = TestResults()
def check_logs(step): def check_logs(step):
'''check for log files from a step''' """Check for log files from a step."""
print("check step: ", step) print("check step: ", step)
if step.startswith('fly.'): if step.startswith('fly.'):
vehicle = step[4:] vehicle = step[4:]
@ -427,10 +453,11 @@ def check_logs(step):
newname = util.reltopdir("../buildlogs/%s.core" % vehicle) newname = util.reltopdir("../buildlogs/%s.core" % vehicle)
print("Renaming %s to %s" % (corefile, newname)) print("Renaming %s to %s" % (corefile, newname))
os.rename(corefile, newname) os.rename(corefile, newname)
util.run_cmd('/bin/cp A*/A*.elf ../buildlogs', dir=util.reltopdir('.')) util.run_cmd('/bin/cp A*/A*.elf ../buildlogs', directory=util.reltopdir('.'))
def run_tests(steps): def run_tests(steps):
'''run a list of steps''' """Run a list of steps."""
global results global results
passed = True passed = True
@ -449,7 +476,7 @@ def run_tests(steps):
failed.append(step) failed.append(step)
results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1) results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1)
continue continue
except Exception, msg: except Exception as msg:
passed = False passed = False
failed.append(step) failed.append(step)
print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg)) print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg))

79
Tools/autotest/common.py
View File

@ -1,53 +1,65 @@
import util, pexpect, time, math import math
import time
from pymavlink import mavwp from pymavlink import mavwp
from pysim import util
# a list of pexpect objects to read while waiting for # a list of pexpect objects to read while waiting for
# messages. This keeps the output to stdout flowing # messages. This keeps the output to stdout flowing
expect_list = [] expect_list = []
def expect_list_clear(): def expect_list_clear():
'''clear the expect list''' """clear the expect list."""
global expect_list global expect_list
for p in expect_list[:]: for p in expect_list[:]:
expect_list.remove(p) expect_list.remove(p)
def expect_list_extend(list):
'''extend the expect list''' def expect_list_extend(list_to_add):
"""Extend the expect list."""
global expect_list global expect_list
expect_list.extend(list) expect_list.extend(list_to_add)
def idle_hook(mav): def idle_hook(mav):
'''called when waiting for a mavlink message''' """Called when waiting for a mavlink message."""
global expect_list global expect_list
for p in expect_list: for p in expect_list:
util.pexpect_drain(p) util.pexpect_drain(p)
def message_hook(mav, msg): def message_hook(mav, msg):
'''called as each mavlink msg is received''' """Called as each mavlink msg is received."""
idle_hook(mav) idle_hook(mav)
def expect_callback(e): def expect_callback(e):
'''called when waiting for a expect pattern''' """Called when waiting for a expect pattern."""
global expect_list global expect_list
for p in expect_list: for p in expect_list:
if p == e: if p == e:
continue continue
util.pexpect_drain(p) util.pexpect_drain(p)
def get_distance(loc1, loc2): def get_distance(loc1, loc2):
'''get ground distance between two locations''' """Get ground distance between two locations."""
dlat = loc2.lat - loc1.lat dlat = loc2.lat - loc1.lat
dlong = loc2.lng - loc1.lng dlong = loc2.lng - loc1.lng
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5 return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
def get_bearing(loc1, loc2): def get_bearing(loc1, loc2):
'''get bearing from loc1 to loc2''' """Get bearing from loc1 to loc2."""
off_x = loc2.lng - loc1.lng off_x = loc2.lng - loc1.lng
off_y = loc2.lat - loc1.lat off_y = loc2.lat - loc1.lat
bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795 bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
if bearing < 0: if bearing < 0:
bearing += 360.00 bearing += 360.00
return bearing; return bearing
def wait_seconds(mav, seconds_to_wait): def wait_seconds(mav, seconds_to_wait):
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
@ -55,29 +67,33 @@ def wait_seconds(mav, seconds_to_wait):
while tstart + seconds_to_wait > tnow: while tstart + seconds_to_wait > tnow:
tnow = get_sim_time(mav) tnow = get_sim_time(mav)
def get_sim_time(mav): def get_sim_time(mav):
m = mav.recv_match(type='SYSTEM_TIME', blocking=True) m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
return m.time_boot_ms * 1.0e-3 return m.time_boot_ms * 1.0e-3
def wait_altitude(mav, alt_min, alt_max, timeout=30): def wait_altitude(mav, alt_min, alt_max, timeout=30):
"""Wait for a given altitude range."""
climb_rate = 0 climb_rate = 0
previous_alt = 0 previous_alt = 0
'''wait for a given altitude range'''
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for altitude between %u and %u" % (alt_min, alt_max)) print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
climb_rate = m.alt - previous_alt climb_rate = m.alt - previous_alt
previous_alt = m.alt previous_alt = m.alt
print("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min , climb_rate)) print("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate))
if m.alt >= alt_min and m.alt <= alt_max: if m.alt >= alt_min and m.alt <= alt_max:
print("Altitude OK") print("Altitude OK")
return True return True
print("Failed to attain altitude range") print("Failed to attain altitude range")
return False return False
def wait_groundspeed(mav, gs_min, gs_max, timeout=30): def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
'''wait for a given ground speed range''' """Wait for a given ground speed range."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max)) print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
@ -90,7 +106,7 @@ def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
def wait_roll(mav, roll, accuracy, timeout=30): def wait_roll(mav, roll, accuracy, timeout=30):
'''wait for a given roll in degrees''' """Wait for a given roll in degrees."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for roll of %d at %s" % (roll, time.ctime())) print("Waiting for roll of %d at %s" % (roll, time.ctime()))
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
@ -104,8 +120,9 @@ def wait_roll(mav, roll, accuracy, timeout=30):
print("Failed to attain roll %d" % roll) print("Failed to attain roll %d" % roll)
return False return False
def wait_pitch(mav, pitch, accuracy, timeout=30): def wait_pitch(mav, pitch, accuracy, timeout=30):
'''wait for a given pitch in degrees''' """Wait for a given pitch in degrees."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for pitch of %u at %s" % (pitch, time.ctime())) print("Waiting for pitch of %u at %s" % (pitch, time.ctime()))
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
@ -120,9 +137,8 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
return False return False
def wait_heading(mav, heading, accuracy=5, timeout=30): def wait_heading(mav, heading, accuracy=5, timeout=30):
'''wait for a given heading''' """Wait for a given heading."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
print("Waiting for heading %u with accuracy %u" % (heading, accuracy)) print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
@ -136,7 +152,7 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
def wait_distance(mav, distance, accuracy=5, timeout=30): def wait_distance(mav, distance, accuracy=5, timeout=30):
'''wait for flight of a given distance''' """Wait for flight of a given distance."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
start = mav.location() start = mav.location()
while get_sim_time(mav) < tstart + timeout: while get_sim_time(mav) < tstart + timeout:
@ -154,7 +170,7 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1): def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
'''wait for arrival at a location''' """Wait for arrival at a location."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
if target_altitude is None: if target_altitude is None:
target_altitude = loc.alt target_altitude = loc.alt
@ -172,8 +188,9 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
print("Failed to attain location") print("Failed to attain location")
return False return False
def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400): def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400):
'''wait for waypoint ranges''' """Wait for waypoint ranges."""
tstart = get_sim_time(mav) tstart = get_sim_time(mav)
# this message arrives after we set the current WP # this message arrives after we set the current WP
start_wp = mav.waypoint_current() start_wp = mav.waypoint_current()
@ -203,7 +220,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
current_wp = seq current_wp = seq
# the wp_dist check is a hack until we can sort out the right seqnum # the wp_dist check is a hack until we can sort out the right seqnum
# for end of mission # for end of mission
#if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2): # if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
if (current_wp == wpnum_end and wp_dist < max_dist): if (current_wp == wpnum_end and wp_dist < max_dist):
print("Reached final waypoint %u" % seq) print("Reached final waypoint %u" % seq)
return True return True
@ -216,6 +233,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end)) print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
return False return False
def save_wp(mavproxy, mav): def save_wp(mavproxy, mav):
mavproxy.send('rc 7 1000\n') mavproxy.send('rc 7 1000\n')
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
@ -227,27 +245,31 @@ def save_wp(mavproxy, mav):
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True) mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
wait_seconds(mav, 1) wait_seconds(mav, 1)
def wait_mode(mav, mode, timeout=None): def wait_mode(mav, mode, timeout=None):
print("Waiting for mode %s" % mode) print("Waiting for mode %s" % mode)
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True) mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
print("Got mode %s" % mode) print("Got mode %s" % mode)
return mav.flightmode return mav.flightmode
def mission_count(filename): def mission_count(filename):
'''load a mission from a file and return number of waypoints''' """Load a mission from a file and return number of waypoints."""
wploader = mavwp.MAVWPLoader() wploader = mavwp.MAVWPLoader()
wploader.load(filename) wploader.load(filename)
num_wp = wploader.count() num_wp = wploader.count()
return num_wp return num_wp
def sim_location(mav): def sim_location(mav):
'''return current simulator location''' """Return current simulator location."""
from pymavlink import mavutil from pymavlink import mavutil
m = mav.recv_match(type='SIMSTATE', blocking=True) 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)) return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
def log_download(mavproxy, mav, filename, timeout=360): def log_download(mavproxy, mav, filename, timeout=360):
'''download latest log''' """Download latest log."""
mavproxy.send("log list\n") mavproxy.send("log list\n")
mavproxy.expect("numLogs") mavproxy.expect("numLogs")
mav.wait_heartbeat() mav.wait_heartbeat()
@ -258,4 +280,3 @@ def log_download(mavproxy, mav, filename, timeout=360):
mav.wait_heartbeat() mav.wait_heartbeat()
mav.wait_heartbeat() mav.wait_heartbeat()
return True return True

25
Tools/autotest/dump_logs.py
View File

@ -1,12 +1,15 @@
#!/usr/bin/env python #!/usr/bin/env python
# dump flash logs from SITL """
# Andrew Tridgell, April 2013 dump flash logs from SITL
Andrew Tridgell, April 2013
"""
import pexpect, os, sys, shutil, atexit import optparse
import optparse, fnmatch, time, glob, traceback, signal import os
import sys
from pysim import util
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim'))
import util
############## main program ############# ############## main program #############
parser = optparse.OptionParser(sys.argv[0]) parser = optparse.OptionParser(sys.argv[0])
@ -17,13 +20,14 @@ opts, args = parser.parse_args()
os.environ['PYTHONUNBUFFERED'] = '1' os.environ['PYTHONUNBUFFERED'] = '1'
def dump_logs(atype): def dump_logs(atype):
'''dump DataFlash logs''' """Dump DataFlash logs."""
logfile = '%s.log' % atype logfile = '%s.log' % atype
print("Dumping logs for %s to %s" % (atype, logfile)) print("Dumping logs for %s to %s" % (atype, logfile))
sil = util.start_SIL(atype) sitl = util.start_SITL(atype)
log = open(logfile, mode='w') log = open(logfile, mode='w')
mavproxy = util.start_MAVProxy_SIL(atype, setup=True, logfile=log) mavproxy = util.start_MAVProxy_SITL(atype, setup=True, logfile=log)
mavproxy.send('\n\n\n') mavproxy.send('\n\n\n')
print("navigating menus") print("navigating menus")
mavproxy.expect(']') mavproxy.expect(']')
@ -48,11 +52,10 @@ def dump_logs(atype):
mavproxy.expect("logs enabled:", timeout=120) mavproxy.expect("logs enabled:", timeout=120)
mavproxy.expect("Log]") mavproxy.expect("Log]")
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
log.close() log.close()
print("Saved log for %s to %s" % (atype, logfile)) print("Saved log for %s to %s" % (atype, logfile))
return True return True
vehicle = os.path.basename(os.getcwd()) vehicle = os.path.basename(os.getcwd())
dump_logs(vehicle) dump_logs(vehicle)

42
Tools/autotest/fakepos.py
View File

@ -1,10 +1,15 @@
#!/usr/bin/env python #!/usr/bin/env python
import socket, struct, time, errno import errno
from math import * import socket
import struct
import sys
import time
from math import cos, fabs, radians, sin, sqrt
class udp_out(object): class udp_out(object):
'''a UDP output socket''' """A UDP output socket."""
def __init__(self, device): def __init__(self, device):
a = device.split(':') a = device.split(':')
if len(a) != 2: if len(a) != 2:
@ -15,11 +20,11 @@ class udp_out(object):
self.port.setblocking(0) self.port.setblocking(0)
self.last_address = None self.last_address = None
def recv(self,n=None): def recv(self, n=None):
try: try:
data, self.last_address = self.port.recvfrom(300) data, self.last_address = self.port.recvfrom(300)
except socket.error as e: except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
return "" return ""
raise raise
return data return data
@ -34,12 +39,15 @@ class udp_out(object):
def ft2m(x): def ft2m(x):
return x * 0.3048 return x * 0.3048
def m2ft(x): def m2ft(x):
return x / 0.3048 return x / 0.3048
def kt2mps(x): def kt2mps(x):
return x * 0.514444444 return x * 0.514444444
def mps2kt(x): def mps2kt(x):
return x / 0.514444444 return x / 0.514444444
@ -81,19 +89,19 @@ while True:
xAccel = sin(radians(pitchDeg)) xAccel = sin(radians(pitchDeg))
yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg)) yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg))
zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg)) zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg))
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel)) scale = 9.81 / sqrt((xAccel*xAccel) + (yAccel*yAccel) + (zAccel*zAccel))
xAccel *= scale; xAccel *= scale
yAccel *= scale; yAccel *= scale
zAccel *= scale; zAccel *= scale
buf = struct.pack('<17dI', struc_buf = struct.pack('<17dI',
latitude, longitude, altitude, heading, latitude, longitude, altitude, heading,
speedN, speedE, speedD, speedN, speedE, speedD,
xAccel, yAccel, zAccel, xAccel, yAccel, zAccel,
rollRate, pitchRate, yawRate, rollRate, pitchRate, yawRate,
rollDeg, pitchDeg, yawDeg, rollDeg, pitchDeg, yawDeg,
airspeed, magic) airspeed, magic)
udp.write(buf) udp.write(struc_buf)
time.sleep(deltaT) time.sleep(deltaT)
yawDeg += yawRate * deltaT yawDeg += yawRate * deltaT

0
Tools/autotest/jsb_sim/__init__.py Normal file
View File

90
Tools/autotest/jsb_sim/runsim.py
View File

@ -1,14 +1,26 @@
#!/usr/bin/env python #!/usr/bin/env python
# run a jsbsim model as a child process """
Run a jsbsim model as a child process.
"""
import sys, os, pexpect, socket import atexit
import math, time, select, struct, signal, errno import errno
import fdpexpect
import math
import os
import select
import signal
import socket
import struct
import sys
import time
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pysim')) import pexpect
import util, atexit, fdpexpect
from pymavlink import fgFDM from pymavlink import fgFDM
from .. pysim import util
class control_state(object): class control_state(object):
def __init__(self): def __init__(self):
self.aileron = 0 self.aileron = 0
@ -21,18 +33,20 @@ sitl_state = control_state()
def interpret_address(addrstr): def interpret_address(addrstr):
'''interpret a IP:port string''' """Interpret a IP:port string."""
a = addrstr.split(':') a = addrstr.split(':')
a[1] = int(a[1]) a[1] = int(a[1])
return tuple(a) return tuple(a)
def jsb_set(variable, value): def jsb_set(variable, value):
'''set a JSBSim variable''' """Set a JSBSim variable."""
global jsb_console global jsb_console
jsb_console.send('set %s %s\r\n' % (variable, value)) jsb_console.send('set %s %s\r\n' % (variable, value))
def setup_template(home): def setup_template(home):
'''setup aircraft/Rascal/reset.xml''' """Setup aircraft/Rascal/reset.xml ."""
global opts global opts
v = home.split(',') v = home.split(',')
if len(v) != 4: if len(v) != 4:
@ -45,50 +59,50 @@ def setup_template(home):
sitl_state.ground_height = altitude sitl_state.ground_height = altitude
template = os.path.join('aircraft', 'Rascal', 'reset_template.xml') template = os.path.join('aircraft', 'Rascal', 'reset_template.xml')
reset = os.path.join('aircraft', 'Rascal', 'reset.xml') reset = os.path.join('aircraft', 'Rascal', 'reset.xml')
xml = open(template).read() % { 'LATITUDE' : str(latitude), xml = open(template).read() % {'LATITUDE': str(latitude),
'LONGITUDE' : str(longitude), 'LONGITUDE': str(longitude),
'HEADING' : str(heading) } 'HEADING': str(heading)}
open(reset, mode='w').write(xml) open(reset, mode='w').write(xml)
print("Wrote %s" % reset) print("Wrote %s" % reset)
baseport = int(opts.simout.split(':')[1]) baseport = int(opts.simout.split(':')[1])
template = os.path.join('jsb_sim', 'fgout_template.xml') template = os.path.join('jsb_sim', 'fgout_template.xml')
out = os.path.join('jsb_sim', 'fgout.xml') out = os.path.join('jsb_sim', 'fgout.xml')
xml = open(template).read() % { 'FGOUTPORT' : str(baseport+3) } xml = open(template).read() % {'FGOUTPORT': str(baseport+3)}
open(out, mode='w').write(xml) open(out, mode='w').write(xml)
print("Wrote %s" % out) print("Wrote %s" % out)
template = os.path.join('jsb_sim', 'rascal_test_template.xml') template = os.path.join('jsb_sim', 'rascal_test_template.xml')
out = os.path.join('jsb_sim', 'rascal_test.xml') out = os.path.join('jsb_sim', 'rascal_test.xml')
xml = open(template).read() % { 'JSBCONSOLEPORT' : str(baseport+4) } xml = open(template).read() % {'JSBCONSOLEPORT': str(baseport+4)}
open(out, mode='w').write(xml) open(out, mode='w').write(xml)
print("Wrote %s" % out) print("Wrote %s" % out)
def process_sitl_input(buf): def process_sitl_input(buf):
'''process control changes from SITL sim''' """Process control changes from SITL sim."""
control = list(struct.unpack('<14H', buf)) control = list(struct.unpack('<14H', buf))
pwm = control[:11] pwm = control[:11]
(speed, direction, turbulance) = control[11:] (speed, direction, turbulance) = control[11:]
global wind global wind
wind.speed = speed*0.01 wind.speed = speed*0.01
wind.direction = direction*0.01 wind.direction = direction*0.01
wind.turbulance = turbulance*0.01 wind.turbulance = turbulance*0.01
aileron = (pwm[0]-1500)/500.0 aileron = (pwm[0]-1500)/500.0
elevator = (pwm[1]-1500)/500.0 elevator = (pwm[1]-1500)/500.0
throttle = (pwm[2]-1000)/1000.0 throttle = (pwm[2]-1000)/1000.0
if opts.revthr: if opts.revthr:
throttle = 1.0 - throttle throttle = 1.0 - throttle
rudder = (pwm[3]-1500)/500.0 rudder = (pwm[3]-1500)/500.0
if opts.elevon: if opts.elevon:
# fake an elevon plane # fake an elevon plane
ch1 = aileron ch1 = aileron
ch2 = elevator ch2 = elevator
aileron = (ch2-ch1)/2.0 aileron = (ch2-ch1)/2.0
# the minus does away with the need for RC2_REV=-1 # the minus does away with the need for RC2_REV=-1
elevator = -(ch2+ch1)/2.0 elevator = -(ch2+ch1)/2.0
@ -98,7 +112,7 @@ def process_sitl_input(buf):
ch2 = rudder ch2 = rudder
# this matches VTAIL_OUTPUT==2 # this matches VTAIL_OUTPUT==2
elevator = (ch2-ch1)/2.0 elevator = (ch2-ch1)/2.0
rudder = (ch2+ch1)/2.0 rudder = (ch2+ch1)/2.0
buf = '' buf = ''
if aileron != sitl_state.aileron: if aileron != sitl_state.aileron:
@ -117,14 +131,16 @@ def process_sitl_input(buf):
global jsb_console global jsb_console
jsb_console.send(buf) jsb_console.send(buf)
def update_wind(wind): def update_wind(wind):
'''update wind simulation''' """Update wind simulation."""
(speed, direction) = wind.current() (speed, direction) = wind.current()
jsb_set('atmosphere/psiw-rad', math.radians(direction)) jsb_set('atmosphere/psiw-rad', math.radians(direction))
jsb_set('atmosphere/wind-mag-fps', speed/0.3048) jsb_set('atmosphere/wind-mag-fps', speed/0.3048)
def process_jsb_input(buf, simtime): def process_jsb_input(buf, simtime):
'''process FG FDM input from JSBSim''' """Process FG FDM input from JSBSim."""
global fdm, fg_out, sim_out global fdm, fg_out, sim_out
fdm.parse(buf) fdm.parse(buf)
if fg_out: if fg_out:
@ -134,7 +150,7 @@ def process_jsb_input(buf, simtime):
fdm.set('rpm', sitl_state.throttle*1000) fdm.set('rpm', sitl_state.throttle*1000)
fg_out.send(fdm.pack()) fg_out.send(fdm.pack())
except socket.error as e: except socket.error as e:
if e.errno not in [ errno.ECONNREFUSED ]: if e.errno not in [errno.ECONNREFUSED]:
raise raise
timestamp = int(simtime*1.0e6) timestamp = int(simtime*1.0e6)
@ -162,13 +178,11 @@ def process_jsb_input(buf, simtime):
try: try:
sim_out.send(simbuf) sim_out.send(simbuf)
except socket.error as e: except socket.error as e:
if e.errno not in [ errno.ECONNREFUSED ]: if e.errno not in [errno.ECONNREFUSED]:
raise raise
################## main program ##################
##################
# main program
from optparse import OptionParser from optparse import OptionParser
parser = OptionParser("runsim.py [options]") parser = OptionParser("runsim.py [options]")
parser.add_option("--simin", help="SITL input (IP:port)", default="127.0.0.1:5502") parser.add_option("--simin", help="SITL input (IP:port)", default="127.0.0.1:5502")
@ -186,7 +200,7 @@ parser.add_option("--speedup", type='float', default=1.0, help="speedup from rea
(opts, args) = parser.parse_args() (opts, args) = parser.parse_args()
for m in [ 'home', 'script' ]: for m in ['home', 'script']:
if not opts.__dict__[m]: if not opts.__dict__[m]:
print("Missing required option '%s'" % m) print("Missing required option '%s'" % m)
parser.print_help() parser.print_help()
@ -233,7 +247,7 @@ jsb_in.setblocking(0)
# socket addresses # socket addresses
sim_out_address = interpret_address(opts.simout) sim_out_address = interpret_address(opts.simout)
sim_in_address = interpret_address(opts.simin) sim_in_address = interpret_address(opts.simin)
# setup input from SITL sim # setup input from SITL sim
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@ -259,15 +273,16 @@ fdm = fgFDM.fgFDM()
jsb_console.send('info\n') jsb_console.send('info\n')
jsb_console.send('resume\n') jsb_console.send('resume\n')
jsb.expect(["trim computation time","Trim Results"]) jsb.expect(["trim computation time", "Trim Results"])
time.sleep(1.5) time.sleep(1.5)
jsb_console.send('step\n') jsb_console.send('step\n')
jsb_console.logfile = None jsb_console.logfile = None
print("Simulator ready to fly") print("Simulator ready to fly")
def main_loop(): def main_loop():
'''run main loop''' """Run main loop."""
tnow = time.time() tnow = time.time()
last_report = tnow last_report = tnow
last_sim_input = tnow last_sim_input = tnow
@ -348,8 +363,9 @@ def main_loop():
last_wall_time = now last_wall_time = now
def exit_handler(): def exit_handler():
'''exit the sim''' """Exit the sim."""
print("running exit handler") print("running exit handler")
signal.signal(signal.SIGINT, signal.SIG_IGN) signal.signal(signal.SIGINT, signal.SIG_IGN)
signal.signal(signal.SIGTERM, signal.SIG_IGN) signal.signal(signal.SIGTERM, signal.SIG_IGN)

0
Tools/autotest/param_metadata/__init__.py Normal file
View File

18
Tools/autotest/param_metadata/emit.py
View File

@ -1,21 +1,25 @@
#!/usr/bin/env python #!/usr/bin/env python
"""
The standard interface emitters must implement
"""
import re import re
from param import *
# The standard interface emitters must implement
class Emit: class Emit:
def __init__(self):
pass
prog_values_field = re.compile(r"\s*(-?\w+:\w+)+,*") prog_values_field = re.compile(r"\s*(-?\w+:\w+)+,*")
def close(self): def close(self):
pass pass
def start_libraries(self): def start_libraries(self):
pass pass
def emit(self, g, f): def emit(self, g, f):
pass pass
def set_annotate_with_vehicle(self, value): def set_annotate_with_vehicle(self, value):
self.annotate_with_vehicle = value self.annotate_with_vehicle = value

30
Tools/autotest/param_metadata/htmlemit.py
View File

@ -1,17 +1,20 @@
#!/usr/bin/env python #!/usr/bin/env python
"""
Emit docs in a form acceptable to the Ardupilot wordpress docs site
"""
import re from param import known_param_fields
from param import *
from emit import Emit from emit import Emit
import cgi import cgi
# Emit docs in a form acceptable to the APM wordpress docs site
class HtmlEmit(Emit): class HtmlEmit(Emit):
def __init__(self): def __init__(self):
Emit.__init__(self)
html_fname = 'Parameters.html' html_fname = 'Parameters.html'
self.f = open(html_fname, mode='w') self.f = open(html_fname, mode='w')
self.preamble = '''<!-- Dynamically generated list of documented parameters self.preamble = """<!-- Dynamically generated list of documented parameters
This page was generated using Tools/autotest/param_metadata/param_parse.py This page was generated using Tools/autotest/param_metadata/param_parse.py
DO NOT EDIT DO NOT EDIT
@ -26,7 +29,7 @@ DO NOT EDIT
<!-- add auto-generated table of contents with "Table of Contents Plus" plugin --> <!-- add auto-generated table of contents with "Table of Contents Plus" plugin -->
[toc exclude="Complete Parameter List"] [toc exclude="Complete Parameter List"]
''' """
self.t = '' self.t = ''
def escape(self, s): def escape(self, s):
@ -40,25 +43,25 @@ DO NOT EDIT
self.f.write(self.preamble) self.f.write(self.preamble)
self.f.write(self.t) self.f.write(self.t)
self.f.close() self.f.close()
def start_libraries(self): def start_libraries(self):
pass pass
def emit(self, g, f): def emit(self, g, f):
tag = '%s Parameters' % g.name tag = '%s Parameters' % g.name
t = '\n\n<h1>%s</h1>\n' % tag t = '\n\n<h1>%s</h1>\n' % tag
for param in g.params: for param in g.params:
if not hasattr(param, 'DisplayName') or not hasattr(param, 'Description'): if not hasattr(param, 'DisplayName') or not hasattr(param, 'Description'):
continue continue
d = param.__dict__ d = param.__dict__
tag = '%s (%s)' % (param.DisplayName, param.name) tag = '%s (%s)' % (param.DisplayName, param.name)
t += '\n\n<h2>%s</h2>' % tag t += '\n\n<h2>%s</h2>' % tag
if d.get('User',None) == 'Advanced': if d.get('User', None) == 'Advanced':
t += '<em>Note: This parameter is for advanced users</em><br>' t += '<em>Note: This parameter is for advanced users</em><br>'
t += "\n\n<p>%s</p>\n" % cgi.escape(param.Description) t += "\n\n<p>%s</p>\n" % cgi.escape(param.Description)
t += "<ul>\n" t += "<ul>\n"
for field in param.__dict__.keys(): for field in param.__dict__.keys():
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
@ -72,4 +75,3 @@ DO NOT EDIT
t += "<li>%s: %s</li>\n" % (field, cgi.escape(param.__dict__[field])) t += "<li>%s: %s</li>\n" % (field, cgi.escape(param.__dict__[field]))
t += "</ul>\n" t += "</ul>\n"
self.t += t self.t += t

17
Tools/autotest/param_metadata/param.py
View File

@ -2,19 +2,20 @@
class Parameter(object): class Parameter(object):
def __init__(self, name): def __init__(self, name):
self.name = name self.name = name
class Vehicle(object): class Vehicle(object):
def __init__ (self, name, path): def __init__(self, name, path):
self.name = name self.name = name
self.path = path self.path = path
self.params = [] self.params = []
class Library(object): class Library(object):
def __init__ (self, name): def __init__(self, name):
self.name = name self.name = name
self.params = [] self.params = []
known_param_fields = [ known_param_fields = [
'Description', 'Description',
'DisplayName', 'DisplayName',
@ -26,15 +27,15 @@ known_param_fields = [
'RebootRequired', 'RebootRequired',
'Bitmask', 'Bitmask',
'Volatile', 'Volatile',
'ReadOnly' 'ReadOnly',
] ]
required_param_fields = [ required_param_fields = [
'Description', 'Description',
'DisplayName', 'DisplayName',
'User' 'User',
] ]
known_group_fields = [ known_group_fields = [
'Path' 'Path',
] ]

86
Tools/autotest/param_metadata/param_parse.py
View File

@ -1,14 +1,18 @@
#!/usr/bin/env python #!/usr/bin/env python
import os, glob, re, sys import glob
import os
import re
import sys
from optparse import OptionParser
from param import * from param import (Library, Parameter, Vehicle, known_group_fields,
from wikiemit import WikiEmit known_param_fields, required_param_fields)
from xmlemit import XmlEmit
from htmlemit import HtmlEmit from htmlemit import HtmlEmit
from rstemit import RSTEmit from rstemit import RSTEmit
from wikiemit import WikiEmit
from xmlemit import XmlEmit
from optparse import OptionParser
parser = OptionParser("param_parse.py [options]") parser = OptionParser("param_parse.py [options]")
parser.add_option("-v", "--verbose", dest='verbose', action='store_true', default=False, help="show debugging output") parser.add_option("-v", "--verbose", dest='verbose', action='store_true', default=False, help="show debugging output")
parser.add_option("--vehicle", default='*', help="Vehicle type to generate for") parser.add_option("--vehicle", default='*', help="Vehicle type to generate for")
@ -20,9 +24,9 @@ parser.add_option("--vehicle", default='*', help="Vehicle type to generate for"
prog_param = re.compile(r"@Param: *(\w+).*((?:\n[ \t]*// @(\w+): (.*))+)(?:\n\n|\n[ \t]+[A-Z])", re.MULTILINE) prog_param = re.compile(r"@Param: *(\w+).*((?:\n[ \t]*// @(\w+): (.*))+)(?:\n\n|\n[ \t]+[A-Z])", re.MULTILINE)
prog_param_fields = re.compile(r"[ \t]*// @(\w+): (.*)") prog_param_fields = re.compile(r"[ \t]*// @(\w+): (.*)")
prog_groups = re.compile(r"@Group: *(\w+).*((?:\n[ \t]*// @(Path): (\S+))+)", re.MULTILINE) prog_groups = re.compile(r"@Group: *(\w+).*((?:\n[ \t]*// @(Path): (\S+))+)", re.MULTILINE)
prog_group_param = re.compile(r"@Param: (\w+).*((?:\n[ \t]*// @(\w+): (.*))+)(?:\n\n|\n[ \t]+[A-Z])", re.MULTILINE) prog_group_param = re.compile(r"@Param: (\w+).*((?:\n[ \t]*// @(\w+): (.*))+)(?:\n\n|\n[ \t]+[A-Z])", re.MULTILINE)
apm_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../../') apm_path = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../../')
@ -38,33 +42,35 @@ libraries = []
error_count = 0 error_count = 0
def debug(str):
'''debug output if verbose is set'''
if opts.verbose:
print(str)
def error(str): def debug(str_to_print):
'''show errors''' """Debug output if verbose is set."""
if opts.verbose:
print(str_to_print)
def error(str_to_print):
"""Show errors."""
global error_count global error_count
error_count += 1 error_count += 1
print(str) print(str_to_print)
for vehicle_path in vehicle_paths: for vehicle_path in vehicle_paths:
name = os.path.basename(os.path.dirname(vehicle_path)) name = os.path.basename(os.path.dirname(vehicle_path))
path = os.path.normpath(os.path.dirname(vehicle_path)) path = os.path.normpath(os.path.dirname(vehicle_path))
vehicles.append(Vehicle(name, path)) vehicles.append(Vehicle(name, path))
debug('Found vehicle type %s' % name) debug('Found vehicle type %s' % name)
for vehicle in vehicles: for vehicle in vehicles:
debug("===\n\n\nProcessing %s" % vehicle.name) debug("===\n\n\nProcessing %s" % vehicle.name)
f = open(vehicle.path+'/Parameters.' + extension) f = open(vehicle.path+'/Parameters.' + extension)
p_text = f.read() p_text = f.read()
f.close() f.close()
param_matches = prog_param.findall(p_text) param_matches = prog_param.findall(p_text)
group_matches = prog_groups.findall(p_text) group_matches = prog_groups.findall(p_text)
debug(group_matches) debug(group_matches)
for group_match in group_matches: for group_match in group_matches:
l = Library(group_match[0]) l = Library(group_match[0])
@ -76,9 +82,7 @@ for vehicle in vehicles:
error("unknown parameter metadata field '%s'" % field[0]) error("unknown parameter metadata field '%s'" % field[0])
if not any(l.name == parsed_l.name for parsed_l in libraries): if not any(l.name == parsed_l.name for parsed_l in libraries):
libraries.append(l) libraries.append(l)
for param_match in param_matches: for param_match in param_matches:
p = Parameter(vehicle.name+":"+param_match[0]) p = Parameter(vehicle.name+":"+param_match[0])
debug(p.name + ' ') debug(p.name + ' ')
@ -92,20 +96,19 @@ for vehicle in vehicles:
else: else:
error("unknown parameter metadata field '%s'" % field[0]) error("unknown parameter metadata field '%s'" % field[0])
for req_field in required_param_fields: for req_field in required_param_fields:
if not req_field in field_list: if req_field not in field_list:
error("missing parameter metadata field '%s' in %s" % (req_field, field_text)) error("missing parameter metadata field '%s' in %s" % (req_field, field_text))
vehicle.params.append(p) vehicle.params.append(p)
debug("Processed %u params" % len(vehicle.params)) debug("Processed %u params" % len(vehicle.params))
debug("Found %u documented libraries" % len(libraries)) debug("Found %u documented libraries" % len(libraries))
for library in libraries: for library in libraries:
debug("===\n\n\nProcessing library %s" % library.name) debug("===\n\n\nProcessing library %s" % library.name)
if hasattr(library, 'Path'): if hasattr(library, 'Path'):
paths = library.Path.split(',') paths = library.Path.split(',')
for path in paths: for path in paths:
path = path.strip() path = path.strip()
@ -124,7 +127,7 @@ for library in libraries:
else: else:
error("Path %s not found for library %s" % (path, library.name)) error("Path %s not found for library %s" % (path, library.name))
continue continue
param_matches = prog_group_param.findall(p_text) param_matches = prog_group_param.findall(p_text)
debug("Found %u documented parameters" % len(param_matches)) debug("Found %u documented parameters" % len(param_matches))
for param_match in param_matches: for param_match in param_matches:
@ -137,7 +140,7 @@ for library in libraries:
setattr(p, field[0], field[1]) setattr(p, field[0], field[1])
else: else:
error("unknown parameter metadata field %s" % field[0]) error("unknown parameter metadata field %s" % field[0])
library.params.append(p) library.params.append(p)
else: else:
error("Skipped: no Path found") error("Skipped: no Path found")
@ -161,37 +164,36 @@ for library in libraries:
if (len(rangeValues) != 2): if (len(rangeValues) != 2):
error("Invalid Range values for %s" % (param.name)) error("Invalid Range values for %s" % (param.name))
return return
min = rangeValues[0] min_value = rangeValues[0]
max = rangeValues[1] max_value = rangeValues[1]
if not is_number(min): if not is_number(min_value):
error("Min value not number: %s %s" % (param.name, min)) error("Min value not number: %s %s" % (param.name, min_value))
return return
if not is_number(max): if not is_number(max_value):
error("Max value not number: %s %s" % (param.name, max)) error("Max value not number: %s %s" % (param.name, max_value))
return return
for vehicle in vehicles: for vehicle in vehicles:
for param in vehicle.params: for param in vehicle.params:
validate(param) validate(param)
for library in libraries: for library in libraries:
for param in library.params: for param in library.params:
validate(param) validate(param)
def do_emit(emit): def do_emit(emit):
emit.set_annotate_with_vehicle(len(vehicles) > 1) emit.set_annotate_with_vehicle(len(vehicles) > 1)
for vehicle in vehicles: for vehicle in vehicles:
emit.emit(vehicle, f) emit.emit(vehicle, f)
emit.start_libraries() emit.start_libraries()
for library in libraries: for library in libraries:
if library.params: if library.params:
emit.emit(library, f) emit.emit(library, f)
emit.close() emit.close()
do_emit(XmlEmit()) do_emit(XmlEmit())
do_emit(WikiEmit()) do_emit(WikiEmit())
do_emit(HtmlEmit()) do_emit(HtmlEmit())

81
Tools/autotest/param_metadata/rstemit.py
View File

@ -1,27 +1,29 @@
#!/usr/bin/env python #!/usr/bin/env python
import re import re
from param import * from param import known_param_fields
from emit import Emit from emit import Emit
import cgi import cgi
# Emit docs in a RST format # Emit docs in a RST format
class RSTEmit(Emit): class RSTEmit(Emit):
def blurb(self): def blurb(self):
return '''This is a complete list of the parameters which can be set (e.g. via the MAVLink protocol) to control vehicle behaviour. They are stored in persistent storage on the vehicle. return """This is a complete list of the parameters which can be set (e.g. via the MAVLink protocol) to control vehicle behaviour. They are stored in persistent storage on the vehicle.
This list is automatically generated from the latest ardupilot source code, and so may contain parameters which are not yet in the stable released versions of the code. This list is automatically generated from the latest ardupilot source code, and so may contain parameters which are not yet in the stable released versions of the code.
''' """
def toolname(self): def toolname(self):
return "Tools/autotest/param_metadata/param_parse.py" return "Tools/autotest/param_metadata/param_parse.py"
def __init__(self): def __init__(self):
Emit.__init__(self)
output_fname = 'Parameters.rst' output_fname = 'Parameters.rst'
self.f = open(output_fname, mode='w') self.f = open(output_fname, mode='w')
self.spacer = re.compile("^", re.MULTILINE) self.spacer = re.compile("^", re.MULTILINE)
self.rstescape = re.compile("([^a-zA-Z0-9\n ])") self.rstescape = re.compile("([^a-zA-Z0-9\n ])")
self.preamble = '''.. Dynamically generated list of documented parameters self.preamble = """.. Dynamically generated list of documented parameters
.. This page was generated using {toolname} .. This page was generated using {toolname}
.. DO NOT EDIT .. DO NOT EDIT
@ -34,7 +36,7 @@ Complete Parameter List
{blurb} {blurb}
'''.format(blurb=self.escape(self.blurb()), """.format(blurb=self.escape(self.blurb()),
toolname=self.escape(self.toolname())) toolname=self.escape(self.toolname()))
self.t = '' self.t = ''
@ -47,7 +49,6 @@ Complete Parameter List
self.f.write(self.t) self.f.write(self.t)
self.f.close() self.f.close()
def start_libraries(self): def start_libraries(self):
pass pass
@ -55,34 +56,34 @@ Complete Parameter List
ret = "" ret = ""
joiner = "|" joiner = "|"
row_lines = [ x.split("\n") for x in row ] row_lines = [x.split("\n") for x in row]
for row_line in row_lines: for row_line in row_lines:
row_line.extend([""] * (height-len(row_line))) row_line.extend([""] * (height - len(row_line)))
if rowheading is not None: if rowheading is not None:
rowheading_lines = rowheading.split("\n") rowheading_lines = rowheading.split("\n")
rowheading_lines.extend([""] * (height-len(rowheading_lines))) rowheading_lines.extend([""] * (height - len(rowheading_lines)))
out_lines = [] out_lines = []
for i in range(0,height): for i in range(0, height):
out_line = "" out_line = ""
if rowheading is not None: if rowheading is not None:
rowheading_line = rowheading_lines[i] rowheading_line = rowheading_lines[i]
out_line += joiner + " " + rowheading_line + " "*(widths[0]-len(rowheading_line)-1) out_line += joiner + " " + rowheading_line + " " * (widths[0] - len(rowheading_line) - 1)
joiner = "#" joiner = "#"
j=0 j = 0
for item in row_lines: for item in row_lines:
widthnum = j widthnum = j
if rowheading is not None: if rowheading is not None:
widthnum += 1 widthnum += 1
line = item[i] line = item[i]
out_line += joiner + " " + line + " "*(widths[widthnum]-len(line)-1) out_line += joiner + " " + line + " " * (widths[widthnum] - len(line) - 1)
joiner = "|" joiner = "|"
j += 1 j += 1
out_line += "|" out_line += "|"
out_lines.append(out_line) out_lines.append(out_line)
return "\n".join(out_lines) return "\n".join(out_lines)
def tablify_longest_row_length(self, rows, rowheadings,headings): def tablify_longest_row_length(self, rows, rowheadings, headings):
check_width_rows = rows[:] check_width_rows = rows[:]
if headings is not None: if headings is not None:
check_width_rows.append(headings) check_width_rows.append(headings)
@ -109,7 +110,7 @@ Complete Parameter List
heights = [0] * len(rows_to_check) heights = [0] * len(rows_to_check)
longest_row_length = self.tablify_longest_row_length(rows,rowheadings,headings) longest_row_length = self.tablify_longest_row_length(rows, rowheadings, headings)
widths = [0] * longest_row_length widths = [0] * longest_row_length
all_rowheadings = [] all_rowheadings = []
@ -118,7 +119,7 @@ Complete Parameter List
all_rowheadings.append("") all_rowheadings.append("")
all_rowheadings.extend(rowheadings) all_rowheadings.extend(rowheadings)
for rownum in range(0,len(rows_to_check)): for rownum in range(0, len(rows_to_check)):
row = rows_to_check[rownum] row = rows_to_check[rownum]
values_to_check = [] values_to_check = []
if rowheadings is not None: if rowheadings is not None:
@ -130,15 +131,15 @@ Complete Parameter List
if height > heights[rownum]: if height > heights[rownum]:
heights[rownum] = height heights[rownum] = height
longest_line = self.longest_line_in_string(value) longest_line = self.longest_line_in_string(value)
width = longest_line + 2 # +2 for leading/trailing ws width = longest_line + 2 # +2 for leading/trailing ws
if width > widths[colnum]: if width > widths[colnum]:
widths[colnum] = width widths[colnum] = width
colnum += 1 colnum += 1
return (widths,heights) return (widths, heights)
def tablify(self, rows, headings=None, rowheadings=None): def tablify(self, rows, headings=None, rowheadings=None):
(widths,heights) = self.tablify_calc_row_widths_heights(rows, rowheadings, headings) (widths, heights) = self.tablify_calc_row_widths_heights(rows, rowheadings, headings)
# create dividing lines # create dividing lines
bar = "" bar = ""
@ -159,7 +160,7 @@ Complete Parameter List
rowheading = "" rowheading = ""
ret += self.tablify_row(rowheading, headings, widths, heights[0]) + "\n" ret += self.tablify_row(rowheading, headings, widths, heights[0]) + "\n"
ret += heading_bar + "\n" ret += heading_bar + "\n"
for i in range(0,len(rows)): for i in range(0, len(rows)):
rowheading = None rowheading = None
height = i height = i
if rowheadings is not None: if rowheadings is not None:
@ -171,13 +172,11 @@ Complete Parameter List
return ret return ret
def render_prog_values_field(self, render_info, param, field): def render_prog_values_field(self, render_info, param, field):
values = (param.__dict__[field]).split(',') values = (param.__dict__[field]).split(',')
rows = [] rows = []
for value in values: for value in values:
v = [ x.strip() for x in value.split(':') ] v = [x.strip() for x in value.split(':')]
rows.append(v) rows.append(v)
return self.tablify(rows, headings=render_info["headings"]) return self.tablify(rows, headings=render_info["headings"])
@ -187,20 +186,20 @@ Complete Parameter List
field_table_info = { field_table_info = {
"Values": { "Values": {
"headings": ['Value','Meaning'] "headings": ['Value', 'Meaning'],
}, },
"Bitmask": { "Bitmask": {
"headings": ['Bit', 'Meaning'] "headings": ['Bit', 'Meaning'],
} },
} }
ret = ''' ret = """
.. _{reference}: .. _{reference}:
{tag} {tag}
{underline} {underline}
'''.format(tag=tag,underline="-" * len(tag), """.format(tag=tag, underline="-" * len(tag),
reference=reference) reference=reference)
for param in g.params: for param in g.params:
@ -211,7 +210,7 @@ Complete Parameter List
name = param.name name = param.name
else: else:
name = param.name.split(':')[-1] name = param.name.split(':')[-1]
tag = '%s: %s' % (self.escape(name), self.escape(param.DisplayName), ) tag = '%s: %s' % (self.escape(name), self.escape(param.DisplayName),)
tag = tag.strip() tag = tag.strip()
reference = param.name reference = param.name
# remove e.g. "ArduPlane:" from start of parameter name: # remove e.g. "ArduPlane:" from start of parameter name:
@ -220,15 +219,15 @@ Complete Parameter List
else: else:
reference = reference.split(":")[-1] reference = reference.split(":")[-1]
ret += ''' ret += """
.. _{reference}: .. _{reference}:
{tag} {tag}
{tag_underline} {tag_underline}
'''.format(tag=tag, tag_underline='~'*len(tag), reference=reference) """.format(tag=tag, tag_underline='~' * len(tag), reference=reference)
if d.get('User',None) == 'Advanced': if d.get('User', None) == 'Advanced':
ret += '\n| *Note: This parameter is for advanced users*' ret += '\n| *Note: This parameter is for advanced users*'
ret += "\n\n%s\n" % self.escape(param.Description) ret += "\n\n%s\n" % self.escape(param.Description)
@ -240,30 +239,32 @@ Complete Parameter List
if field in field_table_info and Emit.prog_values_field.match(param.__dict__[field]): if field in field_table_info and Emit.prog_values_field.match(param.__dict__[field]):
row.append(self.render_prog_values_field(field_table_info[field], param, field)) row.append(self.render_prog_values_field(field_table_info[field], param, field))
elif field == "Range": elif field == "Range":
(min,max) = (param.__dict__[field]).split(' ') (param_min, param_max) = (param.__dict__[field]).split(' ')
row.append("%s - %s" % (min,max,)) row.append("%s - %s" % (param_min, param_max,))
else: else:
row.append(cgi.escape(param.__dict__[field])) row.append(cgi.escape(param.__dict__[field]))
if len(row): if len(row):
ret += "\n\n" + self.tablify([row], headings=headings) + "\n\n" ret += "\n\n" + self.tablify([row], headings=headings) + "\n\n"
self.t += ret + "\n" self.t += ret + "\n"
def table_test(): def table_test():
e = RSTEmit() e = RSTEmit()
print("Test 1") print("Test 1")
print e.tablify([["A","B"],["C","D"]]) print(e.tablify([["A", "B"], ["C", "D"]]))
print("Test 2") print("Test 2")
print e.tablify([["A","B"],["CD\nE","FG"]]) print e.tablify([["A", "B"], ["CD\nE", "FG"]])
print("Test 3") print("Test 3")
print e.tablify([["A","B"],["CD\nEF","GH"]], rowheadings=["r1","row2"]) print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], rowheadings=["r1", "row2"]))
print("Test 4") print("Test 4")
print e.tablify([["A","B"],["CD\nEF","GH"]], headings=["c1","col2"]) print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], headings=["c1", "col2"]))
print("Test 5") print("Test 5")
print e.tablify([["A","B"],["CD\nEF","GH"]], headings=["c1","col2"], rowheadings=["r1","row2"]) print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], headings=["c1", "col2"], rowheadings=["r1", "row2"]))
if __name__ == '__main__': if __name__ == '__main__':
table_test() table_test()

54
Tools/autotest/param_metadata/wikiemit.py
View File

@ -1,13 +1,15 @@
#!/usr/bin/env python #!/usr/bin/env python
import re import re
from param import *
from emit import Emit from emit import Emit
from param import known_param_fields
# Emit docs in a form acceptable to the APM wiki site # Emit docs in a form acceptable to the APM wiki site
class WikiEmit(Emit): class WikiEmit(Emit):
def __init__(self): def __init__(self):
Emit.__init__(self)
wiki_fname = 'Parameters.wiki' wiki_fname = 'Parameters.wiki'
self.f = open(wiki_fname, mode='w') self.f = open(wiki_fname, mode='w')
preamble = '''#summary Dynamically generated list of documented parameters preamble = '''#summary Dynamically generated list of documented parameters
@ -17,56 +19,52 @@ class WikiEmit(Emit):
= Vehicles = = Vehicles =
''' '''
self.f.write(preamble) self.f.write(preamble)
def close(self): def close(self):
self.f.close self.f.close()
def camelcase_escape(self, word): def camelcase_escape(self, word):
if re.match(r"([A-Z][a-z]+[A-Z][a-z]*)", word.strip()): if re.match(r"([A-Z][a-z]+[A-Z][a-z]*)", word.strip()):
return "!"+word return "!" + word
else: else:
return word return word
def wikichars_escape(self, text): def wikichars_escape(self, text):
for c in "*,{,},[,],_,=,#,^,~,!,@,$,|,<,>,&,|,\,/".split(','): for c in "*,{,},[,],_,=,#,^,~,!,@,$,|,<,>,&,|,\,/".split(','):
text = re.sub("\\"+c, '`'+c+'`', text) text = re.sub("\\" + c, '`' + c + '`', text)
return text return text
def emit_comment(self, s): def emit_comment(self, s):
self.f.write("\n\n=" + s + "=\n\n") self.f.write("\n\n=" + s + "=\n\n")
def start_libraries(self): def start_libraries(self):
self.emit_comment("Libraries") self.emit_comment("Libraries")
def emit(self, g, f): def emit(self, g, f):
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
for param in g.params: for param in g.params:
if hasattr(param, 'DisplayName'): if hasattr(param, 'DisplayName'):
t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName),self.camelcase_escape(param.name)) t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName), self.camelcase_escape(param.name))
else: else:
t += "\n\n=== %s ===" % self.camelcase_escape(param.name) t += "\n\n=== %s ===" % self.camelcase_escape(param.name)
if hasattr(param, 'Description'): if hasattr(param, 'Description'):
t += "\n\n_%s_\n" % self.wikichars_escape(param.Description) t += "\n\n_%s_\n" % self.wikichars_escape(param.Description)
else: else:
t += "\n\n_TODO: description_\n" t += "\n\n_TODO: description_\n"
for field in param.__dict__.keys(): for field in param.__dict__.keys():
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
t+= " * Values \n" t += " * Values \n"
values = (param.__dict__[field]).split(',') values = (param.__dict__[field]).split(',')
t+="|| *Value* || *Meaning* ||\n" t += "|| *Value* || *Meaning* ||\n"
for value in values: for value in values:
v = value.split(':') v = value.split(':')
t+="|| "+v[0]+" || "+self.camelcase_escape(v[1])+" ||\n" t += "|| " + v[0] + " || " + self.camelcase_escape(v[1]) + " ||\n"
else: else:
t += " * %s: %s\n" % (self.camelcase_escape(field), self.wikichars_escape(param.__dict__[field])) t += " * %s: %s\n" % (self.camelcase_escape(field), self.wikichars_escape(param.__dict__[field]))
#print t # print t
self.f.write(t) self.f.write(t)

54
Tools/autotest/param_metadata/xmlemit.py
View File

@ -2,13 +2,14 @@
from xml.sax.saxutils import escape, quoteattr from xml.sax.saxutils import escape, quoteattr
from param import *
from emit import Emit from emit import Emit
from param import known_param_fields
# Emit APM documentation in an machine readable XML format # Emit APM documentation in an machine readable XML format
class XmlEmit(Emit): class XmlEmit(Emit):
def __init__(self): def __init__(self):
Emit.__init__(self)
wiki_fname = 'apm.pdef.xml' wiki_fname = 'apm.pdef.xml'
self.f = open(wiki_fname, mode='w') self.f = open(wiki_fname, mode='w')
preamble = '''<?xml version="1.0" encoding="utf-8"?> preamble = '''<?xml version="1.0" encoding="utf-8"?>
@ -17,56 +18,53 @@ class XmlEmit(Emit):
<vehicles> <vehicles>
''' '''
self.f.write(preamble) self.f.write(preamble)
def close(self): def close(self):
self.f.write('</libraries>') self.f.write('</libraries>')
self.f.write('''</paramfile>\n''') self.f.write('''</paramfile>\n''')
self.f.close self.f.close()
def emit_comment(self, s): def emit_comment(self, s):
self.f.write("<!-- " + s + " -->") self.f.write("<!-- " + s + " -->")
def start_libraries(self): def start_libraries(self):
self.f.write('</vehicles>') self.f.write('</vehicles>')
self.f.write('<libraries>') self.f.write('<libraries>')
def emit(self, g, f): def emit(self, g, f):
t = '''<parameters name=%s>\n''' % quoteattr(g.name) # i.e. ArduPlane t = '''<parameters name=%s>\n''' % quoteattr(g.name) # i.e. ArduPlane
for param in g.params: for param in g.params:
# Begin our parameter node # Begin our parameter node
if hasattr(param, 'DisplayName'): if hasattr(param, 'DisplayName'):
t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName),quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM) t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName), quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM)
else: else:
t += '<param name=%s' % quoteattr(param.name) t += '<param name=%s' % quoteattr(param.name)
if hasattr(param, 'Description'): if hasattr(param, 'Description'):
t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs
if hasattr(param, 'User'): if hasattr(param, 'User'):
t += ' user=%s' % quoteattr(param.User) # i.e. Standard or Advanced t += ' user=%s' % quoteattr(param.User) # i.e. Standard or Advanced
t += ">\n" t += ">\n"
# Add values as chidren of this node # Add values as chidren of this node
for field in param.__dict__.keys(): for field in param.__dict__.keys():
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
t+= "<values>\n" t += "<values>\n"
values = (param.__dict__[field]).split(',') values = (param.__dict__[field]).split(',')
for value in values: for value in values:
v = value.split(':') v = value.split(':')
t+='''<value code=%s>%s</value>\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label t += '''<value code=%s>%s</value>\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label
t += "</values>\n" t += "</values>\n"
else: else:
t += '''<field name=%s>%s</field>\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10 t += '''<field name=%s>%s</field>\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10
t += '''</param>\n''' t += '''</param>\n'''
t += '''</parameters>\n''' t += '''</parameters>\n'''
#print t # print t
self.f.write(t) self.f.write(t)

1
Tools/autotest/pysim/__init__.py Normal file
View File

@ -0,0 +1 @@
"""pysim tools"""

47
Tools/autotest/pysim/aircraft.py
View File

@ -1,9 +1,13 @@
import math, util, rotmat, time import math
import random import random
import time
import util
from rotmat import Vector3, Matrix3 from rotmat import Vector3, Matrix3
class Aircraft(object): class Aircraft(object):
'''a basic aircraft class''' """A basic aircraft class."""
def __init__(self): def __init__(self):
self.home_latitude = 0 self.home_latitude = 0
self.home_longitude = 0 self.home_longitude = 0
@ -11,20 +15,20 @@ class Aircraft(object):
self.ground_level = 0 self.ground_level = 0
self.frame_height = 0.0 self.frame_height = 0.0
self.latitude = self.home_latitude self.latitude = self.home_latitude
self.longitude = self.home_longitude self.longitude = self.home_longitude
self.altitude = self.home_altitude self.altitude = self.home_altitude
self.dcm = Matrix3() self.dcm = Matrix3()
# rotation rate in body frame # rotation rate in body frame
self.gyro = Vector3(0,0,0) # rad/s self.gyro = Vector3(0, 0, 0) # rad/s
self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
self.position = Vector3(0, 0, 0) # m North, East, Down self.position = Vector3(0, 0, 0) # m North, East, Down
self.mass = 0.0 self.mass = 0.0
self.update_frequency = 50 # in Hz self.update_frequency = 50 # in Hz
self.gravity = 9.80665 # m/s/s self.gravity = 9.80665 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity) self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0') self.wind = util.Wind('0,0,0')
@ -35,13 +39,13 @@ class Aircraft(object):
self.accel_noise = 0.3 self.accel_noise = 0.3
def on_ground(self, position=None): def on_ground(self, position=None):
'''return true if we are on the ground''' """Return true if we are on the ground."""
if position is None: if position is None:
position = self.position position = self.position
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
def update_position(self): def update_position(self):
'''update lat/lon/alt from position''' """Update lat/lon/alt from position."""
bearing = math.degrees(math.atan2(self.position.y, self.position.x)) bearing = math.degrees(math.atan2(self.position.y, self.position.x))
distance = math.sqrt(self.position.x**2 + self.position.y**2) distance = math.sqrt(self.position.x**2 + self.position.y**2)
@ -49,24 +53,24 @@ class Aircraft(object):
(self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude, (self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
bearing, distance) bearing, distance)
self.altitude = self.home_altitude - self.position.z self.altitude = self.home_altitude - self.position.z
velocity_body = self.dcm.transposed() * self.velocity velocity_body = self.dcm.transposed() * self.velocity
self.accelerometer = self.accel_body.copy() self.accelerometer = self.accel_body.copy()
def set_yaw_degrees(self, yaw_degrees): def set_yaw_degrees(self, yaw_degrees):
'''rotate to the given yaw''' """Rotate to the given yaw."""
(roll, pitch, yaw) = self.dcm.to_euler() (roll, pitch, yaw) = self.dcm.to_euler()
yaw = math.radians(yaw_degrees) yaw = math.radians(yaw_degrees)
self.dcm.from_euler(roll, pitch, yaw) self.dcm.from_euler(roll, pitch, yaw)
def time_advance(self, deltat): def time_advance(self, deltat):
'''advance time by deltat in seconds''' """Advance time by deltat in seconds."""
self.time_now += deltat self.time_now += deltat
def setup_frame_time(self, rate, speedup): def setup_frame_time(self, rate, speedup):
'''setup frame_time calculation''' """Setup frame_time calculation."""
self.rate = rate self.rate = rate
self.speedup = speedup self.speedup = speedup
self.frame_time = 1.0/rate self.frame_time = 1.0/rate
@ -75,14 +79,14 @@ class Aircraft(object):
self.achieved_rate = rate self.achieved_rate = rate
def adjust_frame_time(self, rate): def adjust_frame_time(self, rate):
'''adjust frame_time calculation''' """Adjust frame_time calculation."""
self.rate = rate self.rate = rate
self.frame_time = 1.0/rate self.frame_time = 1.0/rate
self.scaled_frame_time = self.frame_time/self.speedup self.scaled_frame_time = self.frame_time/self.speedup
def sync_frame_time(self): def sync_frame_time(self):
'''try to synchronise simulation time with wall clock time, taking """Try to synchronise simulation time with wall clock time, taking
into account desired speedup''' into account desired speedup."""
now = time.time() now = time.time()
if now < self.last_wall_time + self.scaled_frame_time: if now < self.last_wall_time + self.scaled_frame_time:
time.sleep(self.last_wall_time+self.scaled_frame_time - now) time.sleep(self.last_wall_time+self.scaled_frame_time - now)
@ -99,13 +103,10 @@ class Aircraft(object):
self.last_wall_time = now self.last_wall_time = now
def add_noise(self, throttle): def add_noise(self, throttle):
'''add noise based on throttle level (from 0..1)''' """Add noise based on throttle level (from 0..1)."""
self.gyro += Vector3(random.gauss(0, 1), self.gyro += Vector3(random.gauss(0, 1),
random.gauss(0, 1), random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.gyro_noise random.gauss(0, 1)) * throttle * self.gyro_noise
self.accel_body += Vector3(random.gauss(0, 1), self.accel_body += Vector3(random.gauss(0, 1),
random.gauss(0, 1), random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.accel_noise random.gauss(0, 1)) * throttle * self.accel_noise

37
Tools/autotest/pysim/fdpexpect.py
View File

@ -4,36 +4,38 @@ So you are responsible for opening and close the file descriptor.
$Id: fdpexpect.py 505 2007-12-26 21:33:50Z noah $ $Id: fdpexpect.py 505 2007-12-26 21:33:50Z noah $
""" """
from pexpect import *
import os import os
from pexpect import ExceptionPexpect, spawn
__all__ = ['fdspawn'] __all__ = ['fdspawn']
class fdspawn (spawn):
class fdspawn(spawn):
"""This is like pexpect.spawn but allows you to supply your own open file """This is like pexpect.spawn but allows you to supply your own open file
descriptor. For example, you could use it to read through a file looking descriptor. For example, you could use it to read through a file looking
for patterns, or to control a modem or serial device. """ for patterns, or to control a modem or serial device. """
def __init__ (self, fd, args=[], timeout=30, maxread=2000, searchwindowsize=None, logfile=None): def __init__(self, fd, args=[], timeout=30, maxread=2000, searchwindowsize=None, logfile=None):
"""This takes a file descriptor (an int) or an object that support the """This takes a file descriptor (an int) or an object that support the
fileno() method (returning an int). All Python file-like objects fileno() method (returning an int). All Python file-like objects
support fileno(). """ support fileno(). """
### TODO: Add better handling of trying to use fdspawn in place of spawn # TODO: Add better handling of trying to use fdspawn in place of spawn
### TODO: (overload to allow fdspawn to also handle commands as spawn does. # TODO: (overload to allow fdspawn to also handle commands as spawn does.
if type(fd) != type(0) and hasattr(fd, 'fileno'): if not isinstance(fd, int) and hasattr(fd, 'fileno'):
fd = fd.fileno() fd = fd.fileno()
if type(fd) != type(0): if not isinstance(fd, int):
raise ExceptionPexpect ('The fd argument is not an int. If this is a command string then maybe you want to use pexpect.spawn.') raise ExceptionPexpect(
"The fd argument is not an int. If this is a command string then maybe you want to use pexpect.spawn.")
try: # make sure fd is a valid file descriptor try: # make sure fd is a valid file descriptor
os.fstat(fd) os.fstat(fd)
except OSError: except OSError:
raise ExceptionPexpect, 'The fd argument is not a valid file descriptor.' raise ExceptionPexpect("The fd argument is not a valid file descriptor.")
self.args = None self.args = None
self.command = None self.command = None
@ -43,23 +45,23 @@ class fdspawn (spawn):
self.closed = False self.closed = False
self.name = '<file descriptor %d>' % fd self.name = '<file descriptor %d>' % fd
def __del__ (self): def __del__(self):
return return
def close (self): def close(self):
if self.child_fd == -1: if self.child_fd == -1:
return return
if self.own_fd: if self.own_fd:
self.close (self) self.close(self)
else: else:
self.flush() self.flush()
os.close(self.child_fd) os.close(self.child_fd)
self.child_fd = -1 self.child_fd = -1
self.closed = True self.closed = True
def isalive (self): def isalive(self):
"""This checks if the file descriptor is still valid. If os.fstat() """This checks if the file descriptor is still valid. If os.fstat()
does not raise an exception then we assume it is alive. """ does not raise an exception then we assume it is alive. """
@ -72,11 +74,10 @@ class fdspawn (spawn):
except: except:
return False return False
def terminate (self, force=False): def terminate(self, force=False):
raise ExceptionPexpect ('This method is not valid for file descriptors.') raise ExceptionPexpect('This method is not valid for file descriptors.')
def kill (self, sig): def kill(self, sig):
return return

29
Tools/autotest/pysim/fg_display.py
View File

@ -1,17 +1,22 @@
#!/usr/bin/env python #!/usr/bin/env python
import socket, struct, time, math, errno import errno
import socket
import sys
import time
from pymavlink import fgFDM from pymavlink import fgFDM
class udp_socket(object): class udp_socket(object):
'''a UDP socket''' """A UDP socket."""
def __init__(self, device, blocking=True, input=True): def __init__(self, device, blocking=True, is_input=True):
a = device.split(':') a = device.split(':')
if len(a) != 2: if len(a) != 2:
print("UDP ports must be specified as host:port") print("UDP ports must be specified as host:port")
sys.exit(1) sys.exit(1)
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
if input: if is_input:
self.port.bind((a[0], int(a[1]))) self.port.bind((a[0], int(a[1])))
self.destination_addr = None self.destination_addr = None
else: else:
@ -20,11 +25,11 @@ class udp_socket(object):
self.port.setblocking(0) self.port.setblocking(0)
self.last_address = None self.last_address = None
def recv(self,n=1000): def recv(self, n=1000):
try: try:
data, self.last_address = self.port.recvfrom(n) data, self.last_address = self.port.recvfrom(n)
except socket.error as e: except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]: if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
return "" return ""
raise raise
return data return data
@ -39,21 +44,23 @@ class udp_socket(object):
pass pass
def ft2m(x): def ft2m(x):
return x * 0.3048 return x * 0.3048
def m2ft(x): def m2ft(x):
return x / 0.3048 return x / 0.3048
def kt2mps(x): def kt2mps(x):
return x * 0.514444444 return x * 0.514444444
def mps2kt(x): def mps2kt(x):
return x / 0.514444444 return x / 0.514444444
udp = udp_socket("127.0.0.1:5123") udp = udp_socket("127.0.0.1:5123")
fgout = udp_socket("127.0.0.1:5124", input=False) fgout = udp_socket("127.0.0.1:5124", is_input=False)
tlast = time.time() tlast = time.time()
count = 0 count = 0
@ -61,12 +68,12 @@ count = 0
fg = fgFDM.fgFDM() fg = fgFDM.fgFDM()
while True: while True:
buf = udp.recv(1000) udp_buffer = udp.recv(1000)
fg.parse(buf) fg.parse(udp_buffer)
fgout.write(fg.pack()) fgout.write(fg.pack())
count += 1 count += 1
if time.time() - tlast > 1.0: if time.time() - tlast > 1.0:
print("%u FPS len=%u" % (count, len(buf))) print("%u FPS len=%u" % (count, len(udp_buffer)))
count = 0 count = 0
tlast = time.time() tlast = time.time()
print(fg.get('latitude', units='degrees'), print(fg.get('latitude', units='degrees'),

40
Tools/autotest/pysim/iris_ros.py
View File

@ -1,23 +1,23 @@
#!/usr/bin/env python #!/usr/bin/env python
''' """
Python interface to euroc ROS multirotor simulator Python interface to euroc ROS multirotor simulator
See https://pixhawk.org/dev/ros/sitl See https://pixhawk.org/dev/ros/sitl
''' """
import time
import mav_msgs.msg as mav_msgs
import px4.msg as px4
import rosgraph_msgs.msg as rosgraph_msgs
import rospy
import sensor_msgs.msg as sensor_msgs
from aircraft import Aircraft from aircraft import Aircraft
import util, time, math
from math import degrees, radians
from rotmat import Vector3, Matrix3 from rotmat import Vector3, Matrix3
from pymavlink.quaternion import Quaternion
import rospy
import std_msgs.msg as std_msgs
import mav_msgs.msg as mav_msgs
import rosgraph_msgs.msg as rosgraph_msgs
import sensor_msgs.msg as sensor_msgs
import px4.msg as px4
def quat_to_dcm(q1, q2, q3, q4): def quat_to_dcm(q1, q2, q3, q4):
'''convert quaternion to DCM''' """Convert quaternion to DCM."""
q3q3 = q3 * q3 q3q3 = q3 * q3
q3q4 = q3 * q4 q3q4 = q3 * q4
q2q2 = q2 * q2 q2q2 = q2 * q2
@ -33,7 +33,7 @@ def quat_to_dcm(q1, q2, q3, q4):
m.a.y = 2.0*(q2q3 - q1q4) m.a.y = 2.0*(q2q3 - q1q4)
m.a.z = 2.0*(q2q4 + q1q3) m.a.z = 2.0*(q2q4 + q1q3)
m.b.x = 2.0*(q2q3 + q1q4) m.b.x = 2.0*(q2q3 + q1q4)
m.b.y = 1.0-2.0*(q2q2 + q4q4) m.b.y = 1.0-2.0*(q2q2 + q4q4)
m.b.z = 2.0*(q3q4 - q1q2) m.b.z = 2.0*(q3q4 - q1q2)
m.c.x = 2.0*(q2q4 - q1q3) m.c.x = 2.0*(q2q4 - q1q3)
m.c.y = 2.0*(q3q4 + q1q2) m.c.y = 2.0*(q3q4 + q1q2)
@ -42,7 +42,7 @@ def quat_to_dcm(q1, q2, q3, q4):
class IrisRos(Aircraft): class IrisRos(Aircraft):
'''a IRIS MultiCopter from ROS''' """A IRIS MultiCopter from ROS."""
def __init__(self): def __init__(self):
Aircraft.__init__(self) Aircraft.__init__(self)
self.max_rpm = 1200 self.max_rpm = 1200
@ -51,11 +51,11 @@ class IrisRos(Aircraft):
self.have_new_pos = False self.have_new_pos = False
topics = { topics = {
"/clock" : (self.clock_cb, rosgraph_msgs.Clock), "/clock" : (self.clock_cb, rosgraph_msgs.Clock),
"/iris/imu" : (self.imu_cb, sensor_msgs.Imu), "/iris/imu" : (self.imu_cb, sensor_msgs.Imu),
"/iris/vehicle_local_position" : (self.pos_cb, px4.vehicle_local_position), "/iris/vehicle_local_position" : (self.pos_cb, px4.vehicle_local_position),
} }
rospy.init_node('ArduPilot', anonymous=True) rospy.init_node('ArduPilot', anonymous=True)
for topic in topics.keys(): for topic in topics.keys():
(callback, msgtype) = topics[topic] (callback, msgtype) = topics[topic]
@ -65,9 +65,9 @@ class IrisRos(Aircraft):
mav_msgs.CommandMotorSpeed, mav_msgs.CommandMotorSpeed,
queue_size=1) queue_size=1)
self.last_time = 0 self.last_time = 0
# spin() simply keeps python from exiting until this node is stopped # spin() simply keeps python from exiting until this node is stopped
#rospy.spin() # rospy.spin()
def clock_cb(self, msg): def clock_cb(self, msg):
self.time_now = self.time_base + msg.clock.secs + msg.clock.nsecs*1.0e-9 self.time_now = self.time_base + msg.clock.secs + msg.clock.nsecs*1.0e-9
@ -85,7 +85,7 @@ class IrisRos(Aircraft):
-msg.orientation.y, -msg.orientation.y,
-msg.orientation.z) -msg.orientation.z)
self.have_new_imu = True self.have_new_imu = True
def pos_cb(self, msg): def pos_cb(self, msg):
self.velocity = Vector3(msg.vx, msg.vy, msg.vz) self.velocity = Vector3(msg.vx, msg.vy, msg.vz)
self.position = Vector3(msg.x, msg.y, msg.z) self.position = Vector3(msg.x, msg.y, msg.z)

144
Tools/autotest/pysim/rotmat.py
View File

@ -1,42 +1,43 @@
#!/usr/bin/env python #!/usr/bin/env python
# """
# vector3 and rotation matrix classes vector3 and rotation matrix classes
# This follows the conventions in the ArduPilot code, This follows the conventions in the ArduPilot code,
# and is essentially a python version of the AP_Math library and is essentially a python version of the AP_Math library
#
# Andrew Tridgell, March 2012
#
# This library is free software; you can redistribute it and/or modify it
# under the terms of the GNU Lesser General Public License as published by the
# Free Software Foundation; either version 2.1 of the License, or (at your
# option) any later version.
#
# This library is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
# for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this library; if not, write to the Free Software Foundation,
# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
'''rotation matrix class Andrew Tridgell, March 2012
'''
This library is free software; you can redistribute it and/or modify it
under the terms of the GNU Lesser General Public License as published by the
Free Software Foundation; either version 2.1 of the License, or (at your
option) any later version.
This library is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
for more details.
You should have received a copy of the GNU Lesser General Public License
along with this library; if not, write to the Free Software Foundation,
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
"""
from math import acos, asin, atan2, cos, pi, radians, sin, sqrt
from math import sin, cos, sqrt, asin, atan2, pi, radians, acos
class Vector3: class Vector3:
'''a vector''' """A vector."""
def __init__(self, x=None, y=None, z=None): def __init__(self, x=None, y=None, z=None):
if x != None and y != None and z != None: if x is not None and y is not None and z is not None:
self.x = float(x) self.x = float(x)
self.y = float(y) self.y = float(y)
self.z = float(z) self.z = float(z)
elif x != None and len(x) == 3: elif x is not None and len(x) == 3:
self.x = float(x[0]) self.x = float(x[0])
self.y = float(x[1]) self.y = float(x[1])
self.z = float(x[2]) self.z = float(x[2])
elif x != None: elif x is not None:
raise ValueError('bad initialiser') raise ValueError('bad initialiser')
else: else:
self.x = float(0) self.x = float(0)
@ -70,8 +71,8 @@ class Vector3:
def __mul__(self, v): def __mul__(self, v):
if isinstance(v, Vector3): if isinstance(v, Vector3):
'''dot product''' """dot product"""
return self.x*v.x + self.y*v.y + self.z*v.z return self.x * v.x + self.y * v.y + self.z * v.z
return Vector3(self.x * v, return Vector3(self.x * v,
self.y * v, self.y * v,
self.z * v) self.z * v)
@ -84,10 +85,10 @@ class Vector3:
self.z / v) self.z / v)
def __mod__(self, v): def __mod__(self, v):
'''cross product''' """Cross product."""
return Vector3(self.y*v.z - self.z*v.y, return Vector3(self.y * v.z - self.z * v.y,
self.z*v.x - self.x*v.z, self.z * v.x - self.x * v.z,
self.x*v.y - self.y*v.x) self.x * v.y - self.y * v.x)
def __copy__(self): def __copy__(self):
return Vector3(self.x, self.y, self.z) return Vector3(self.x, self.y, self.z)
@ -95,13 +96,13 @@ class Vector3:
copy = __copy__ copy = __copy__
def length(self): def length(self):
return sqrt(self.x**2 + self.y**2 + self.z**2) return sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
def zero(self): def zero(self):
self.x = self.y = self.z = 0 self.x = self.y = self.z = 0
def angle(self, v): def angle(self, v):
'''return the angle between this vector and another vector''' """Return the angle between this vector and another vector."""
return acos(self * v) / (self.length() * v.length()) return acos(self * v) / (self.length() * v.length())
def normalized(self): def normalized(self):
@ -113,8 +114,10 @@ class Vector3:
self.y = v.y self.y = v.y
self.z = v.z self.z = v.z
class Matrix3: class Matrix3:
'''a 3x3 matrix, intended as a rotation matrix''' """A 3x3 matrix, intended as a rotation matrix."""
def __init__(self, a=None, b=None, c=None): def __init__(self, a=None, b=None, c=None):
if a is not None and b is not None and c is not None: if a is not None and b is not None and c is not None:
self.a = a.copy() self.a = a.copy()
@ -130,18 +133,17 @@ class Matrix3:
self.c.x, self.c.y, self.c.z) self.c.x, self.c.y, self.c.z)
def identity(self): def identity(self):
self.a = Vector3(1,0,0) self.a = Vector3(1, 0, 0)
self.b = Vector3(0,1,0) self.b = Vector3(0, 1, 0)
self.c = Vector3(0,0,1) self.c = Vector3(0, 0, 1)
def transposed(self): def transposed(self):
return Matrix3(Vector3(self.a.x, self.b.x, self.c.x), return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
Vector3(self.a.y, self.b.y, self.c.y), Vector3(self.a.y, self.b.y, self.c.y),
Vector3(self.a.z, self.b.z, self.c.z)) Vector3(self.a.z, self.b.z, self.c.z))
def from_euler(self, roll, pitch, yaw): def from_euler(self, roll, pitch, yaw):
'''fill the matrix from Euler angles in radians''' """Fill the matrix from Euler angles in radians."""
cp = cos(pitch) cp = cos(pitch)
sp = sin(pitch) sp = sin(pitch)
sr = sin(roll) sr = sin(roll)
@ -159,9 +161,8 @@ class Matrix3:
self.c.y = sr * cp self.c.y = sr * cp
self.c.z = cr * cp self.c.z = cr * cp
def to_euler(self): def to_euler(self):
'''find Euler angles (321 convention) for the matrix''' """Find Euler angles (321 convention) for the matrix."""
if self.c.x >= 1.0: if self.c.x >= 1.0:
pitch = pi pitch = pi
elif self.c.x <= -1.0: elif self.c.x <= -1.0:
@ -169,14 +170,13 @@ class Matrix3:
else: else:
pitch = -asin(self.c.x) pitch = -asin(self.c.x)
roll = atan2(self.c.y, self.c.z) roll = atan2(self.c.y, self.c.z)
yaw = atan2(self.b.x, self.a.x) yaw = atan2(self.b.x, self.a.x)
return (roll, pitch, yaw) return (roll, pitch, yaw)
def to_euler312(self): def to_euler312(self):
'''find Euler angles (312 convention) for the matrix. """Find Euler angles (312 convention) for the matrix.
See http://www.atacolorado.com/eulersequences.doc See http://www.atacolorado.com/eulersequences.doc
''' """
T21 = self.a.y T21 = self.a.y
T22 = self.b.y T22 = self.b.y
T23 = self.c.y T23 = self.c.y
@ -188,7 +188,7 @@ class Matrix3:
return (roll, pitch, yaw) return (roll, pitch, yaw)
def from_euler312(self, roll, pitch, yaw): def from_euler312(self, roll, pitch, yaw):
'''fill the matrix from Euler angles in radians in 312 convention''' """Fill the matrix from Euler angles in radians in 312 convention."""
c3 = cos(pitch) c3 = cos(pitch)
s3 = sin(pitch) s3 = sin(pitch)
s2 = sin(roll) s2 = sin(roll)
@ -199,11 +199,11 @@ class Matrix3:
self.a.x = c1 * c3 - s1 * s2 * s3 self.a.x = c1 * c3 - s1 * s2 * s3
self.b.y = c1 * c2 self.b.y = c1 * c2
self.c.z = c3 * c2 self.c.z = c3 * c2
self.a.y = -c2*s1 self.a.y = -c2 * s1
self.a.z = s3*c1 + c3*s2*s1 self.a.z = s3 * c1 + c3 * s2 * s1
self.b.x = c3*s1 + s3*s2*c1 self.b.x = c3 * s1 + s3 * s2 * c1
self.b.z = s1*s3 - s2*c1*c3 self.b.z = s1 * s3 - s2 * c1 * c3
self.c.x = -s3*c2 self.c.x = -s3 * c2
self.c.y = s2 self.c.y = s2
def __add__(self, m): def __add__(self, m):
@ -249,7 +249,7 @@ class Matrix3:
copy = __copy__ copy = __copy__
def rotate(self, g): def rotate(self, g):
'''rotate the matrix by a given amount on 3 axes''' """Rotate the matrix by a given amount on 3 axes."""
temp_matrix = Matrix3() temp_matrix = Matrix3()
a = self.a a = self.a
b = self.b b = self.b
@ -268,7 +268,7 @@ class Matrix3:
self.c += temp_matrix.c self.c += temp_matrix.c
def normalize(self): def normalize(self):
'''re-normalise a rotation matrix''' """Re-normalise a rotation matrix."""
error = self.a * self.b error = self.a * self.b
t0 = self.a - (self.b * (0.5 * error)) t0 = self.a - (self.b * (0.5 * error))
t1 = self.b - (self.a * (0.5 * error)) t1 = self.b - (self.a * (0.5 * error))
@ -278,11 +278,12 @@ class Matrix3:
self.c = t2 * (1.0 / t2.length()) self.c = t2 * (1.0 / t2.length())
def trace(self): def trace(self):
'''the trace of the matrix''' """The trace of the matrix."""
return self.a.x + self.b.y + self.c.z return self.a.x + self.b.y + self.c.z
def test_euler(): def test_euler():
'''check that from_euler() and to_euler() are consistent''' """Check that from_euler() and to_euler() are consistent."""
m = Matrix3() m = Matrix3()
from math import radians, degrees from math import radians, degrees
for r in range(-179, 179, 3): for r in range(-179, 179, 3):
@ -290,38 +291,41 @@ def test_euler():
for y in range(-179, 179, 3): for y in range(-179, 179, 3):
m.from_euler(radians(r), radians(p), radians(y)) m.from_euler(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler() (r2, p2, y2) = m.to_euler()
v1 = Vector3(r,p,y) v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2)) v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2 diff = v1 - v2
if diff.length() > 1.0e-12: if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length()) print('EULER ERROR:', v1, v2, diff.length())
def test_euler312_single(r,p,y):
'''check that from_euler312() and to_euler312() are consistent for one set of values''' def test_euler312_single(r, p, y):
"""Check that from_euler312() and to_euler312() are consistent for one set of values."""
from math import degrees, radians from math import degrees, radians
m = Matrix3() m = Matrix3()
m.from_euler312(radians(r), radians(p), radians(y)) m.from_euler312(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler312() (r2, p2, y2) = m.to_euler312()
v1 = Vector3(r,p,y) v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2)) v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2 diff = v1 - v2
if diff.length() > 1.0e-12: if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length()) print('EULER ERROR:', v1, v2, diff.length())
def test_one_axis(r,p,y):
'''check that from_euler312() and from_euler() are consistent for one set of values on one axis''' def test_one_axis(r, p, y):
"""Check that from_euler312() and from_euler() are consistent for one set of values on one axis."""
from math import degrees, radians from math import degrees, radians
m = Matrix3() m = Matrix3()
m.from_euler312(radians(r), radians(p), radians(y)) m.from_euler312(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler() (r2, p2, y2) = m.to_euler()
v1 = Vector3(r,p,y) v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2)) v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2 diff = v1 - v2
if diff.length() > 1.0e-12: if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length()) print('EULER ERROR:', v1, v2, diff.length())
def test_euler312(): def test_euler312():
'''check that from_euler312() and to_euler312() are consistent''' """Check that from_euler312() and to_euler312() are consistent."""
m = Matrix3() m = Matrix3()
for x in range(-89, 89, 3): for x in range(-89, 89, 3):
@ -331,10 +335,12 @@ def test_euler312():
for r in range(-89, 89, 3): for r in range(-89, 89, 3):
for p in range(-179, 179, 3): for p in range(-179, 179, 3):
for y in range(-179, 179, 3): for y in range(-179, 179, 3):
test_euler312_single(r,p,y) test_euler312_single(r, p, y)
if __name__ == "__main__": if __name__ == "__main__":
import doctest import doctest
doctest.testmod() doctest.testmod()
test_euler() test_euler()
test_euler312() test_euler312()

9
Tools/autotest/pysim/testwind.py
View File

@ -1,13 +1,16 @@
#!/usr/bin/env python #!/usr/bin/env python
# simple test of wind generation code """
simple test of wind generation code
"""
import util, time, random import time
import util
from rotmat import Vector3 from rotmat import Vector3
wind = util.Wind('7,90,0.1') wind = util.Wind('7,90,0.1')
t0 = time.time() t0 = time.time()
velocity = Vector3(0,0,0) velocity = Vector3(0, 0, 0)
t = 0 t = 0
deltat = 0.01 deltat = 0.01

299
Tools/autotest/pysim/util.py
View File

@ -1,82 +1,100 @@
import math import math
from math import sqrt, acos, cos, pi, sin, atan2 import os
import os, sys, time, random import random
from rotmat import Vector3, Matrix3 import re
from subprocess import call, check_call,Popen, PIPE import sys
import time
from math import acos, atan2, cos, pi, sqrt
from subprocess import PIPE, Popen, call, check_call
import pexpect
from rotmat import Matrix3, Vector3
def m2ft(x): def m2ft(x):
'''meters to feet''' """Meters to feet."""
return float(x) / 0.3048 return float(x) / 0.3048
def ft2m(x): def ft2m(x):
'''feet to meters''' """Feet to meters."""
return float(x) * 0.3048 return float(x) * 0.3048
def kt2mps(x): def kt2mps(x):
return x * 0.514444444 return x * 0.514444444
def mps2kt(x): def mps2kt(x):
return x / 0.514444444 return x / 0.514444444
def topdir(): def topdir():
'''return top of git tree where autotest is running from''' """Return top of git tree where autotest is running from."""
d = os.path.dirname(os.path.realpath(__file__)) d = os.path.dirname(os.path.realpath(__file__))
assert(os.path.basename(d)=='pysim') assert(os.path.basename(d) == 'pysim')
d = os.path.dirname(d) d = os.path.dirname(d)
assert(os.path.basename(d)=='autotest') assert(os.path.basename(d) == 'autotest')
d = os.path.dirname(d) d = os.path.dirname(d)
assert(os.path.basename(d)=='Tools') assert(os.path.basename(d) == 'Tools')
d = os.path.dirname(d) d = os.path.dirname(d)
return d return d
def reltopdir(path): def reltopdir(path):
'''return a path relative to topdir()''' """Return a path relative to topdir()."""
return os.path.normpath(os.path.join(topdir(), path)) return os.path.normpath(os.path.join(topdir(), path))
def run_cmd(cmd, dir=".", show=True, output=False, checkfail=True): def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
'''run a shell command''' """Run a shell command."""
shell = False shell = False
if not isinstance(cmd, list): if not isinstance(cmd, list):
cmd = [cmd] cmd = [cmd]
shell = True shell = True
if show: if show:
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), dir,)) print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
if output: if output:
return Popen(cmd, shell=shell, stdout=PIPE, cwd=dir).communicate()[0] return Popen(cmd, shell=shell, stdout=PIPE, cwd=directory).communicate()[0]
elif checkfail: elif checkfail:
return check_call(cmd, shell=shell, cwd=dir) return check_call(cmd, shell=shell, cwd=directory)
else: else:
return call(cmd, shell=shell, cwd=dir) return call(cmd, shell=shell, cwd=directory)
def rmfile(path): def rmfile(path):
'''remove a file if it exists''' """Remove a file if it exists."""
try: try:
os.unlink(path) os.unlink(path)
except Exception: except Exception:
pass pass
def deltree(path): def deltree(path):
'''delete a tree of files''' """Delete a tree of files."""
run_cmd('rm -rf %s' % path) run_cmd('rm -rf %s' % path)
def relwaf(): def relwaf():
return "./modules/waf/waf-light" return "./modules/waf/waf-light"
def waf_configure(board, j=None, debug=False): def waf_configure(board, j=None, debug=False):
cmd_configure = [relwaf(), "configure", "--board", board] cmd_configure = [relwaf(), "configure", "--board", board]
if debug: if debug:
cmd_configure.append('--debug') cmd_configure.append('--debug')
if j is not None: if j is not None:
cmd_configure.extend(['-j', str(j)]) cmd_configure.extend(['-j', str(j)])
run_cmd(cmd_configure, dir=topdir(), checkfail=True) run_cmd(cmd_configure, directory=topdir(), checkfail=True)
def waf_clean(): def waf_clean():
run_cmd([relwaf(), "clean"], dir=topdir(), checkfail=True) run_cmd([relwaf(), "clean"], directory=topdir(), checkfail=True)
def build_SIL(build_target, j=None, debug=False, board='sitl'):
'''build desktop SIL''' def build_SITL(build_target, j=None, debug=False, board='sitl'):
"""Build desktop SITL."""
# first configure # first configure
waf_configure(board, j=j, debug=debug) waf_configure(board, j=j, debug=debug)
@ -88,9 +106,10 @@ def build_SIL(build_target, j=None, debug=False, board='sitl'):
cmd_make = [relwaf(), "build", "--target", build_target] cmd_make = [relwaf(), "build", "--target", build_target]
if j is not None: if j is not None:
cmd_make.extend(['-j', str(j)]) cmd_make.extend(['-j', str(j)])
run_cmd(cmd_make, dir=topdir(), checkfail=True, show=True) run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
return True return True
def build_examples(board, j=None, debug=False, clean=False): def build_examples(board, j=None, debug=False, clean=False):
# first configure # first configure
waf_configure(board, j=j, debug=debug) waf_configure(board, j=j, debug=debug)
@ -101,20 +120,22 @@ def build_examples(board, j=None, debug=False, clean=False):
# then build # then build
cmd_make = [relwaf(), "examples"] cmd_make = [relwaf(), "examples"]
run_cmd(cmd_make, dir=topdir(), checkfail=True, show=True) run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
return True return True
# list of pexpect children to close on exit # list of pexpect children to close on exit
close_list = [] close_list = []
def pexpect_autoclose(p): def pexpect_autoclose(p):
'''mark for autoclosing''' """Mark for autoclosing."""
global close_list global close_list
close_list.append(p) close_list.append(p)
def pexpect_close(p): def pexpect_close(p):
'''close a pexpect child''' """Close a pexpect child."""
global close_list global close_list
try: try:
@ -128,44 +149,47 @@ def pexpect_close(p):
if p in close_list: if p in close_list:
close_list.remove(p) close_list.remove(p)
def pexpect_close_all(): def pexpect_close_all():
'''close all pexpect children''' """Close all pexpect children."""
global close_list global close_list
for p in close_list[:]: for p in close_list[:]:
pexpect_close(p) pexpect_close(p)
def pexpect_drain(p): def pexpect_drain(p):
'''drain any pending input''' """Drain any pending input."""
import pexpect import pexpect
try: try:
p.read_nonblocking(1000, timeout=0) p.read_nonblocking(1000, timeout=0)
except pexpect.TIMEOUT: except pexpect.TIMEOUT:
pass pass
def cmd_as_shell(cmd):
return (" ".join([ '"%s"' % x for x in cmd ]))
import re def cmd_as_shell(cmd):
return (" ".join(['"%s"' % x for x in cmd]))
def make_safe_filename(text): def make_safe_filename(text):
'''return a version of text safe for use as a filename''' """Return a version of text safe for use as a filename."""
r = re.compile("([^a-zA-Z0-9_.+-])") r = re.compile("([^a-zA-Z0-9_.+-])")
text.replace('/','-') text.replace('/', '-')
filename = r.sub(lambda m : "%" + str(hex(ord(str(m.group(1))))).upper(), text) filename = r.sub(lambda m: "%" + str(hex(ord(str(m.group(1))))).upper(), text)
return filename return filename
import pexpect
class SIL(pexpect.spawn): class SITL(pexpect.spawn):
def __init__(self, binary, valgrind=False, gdb=False, wipe=False, synthetic_clock=True, home=None, model=None, speedup=1, defaults_file=None): def __init__(self, binary, valgrind=False, gdb=False, wipe=False, synthetic_clock=True, home=None, model=None, speedup=1, defaults_file=None):
self.binary = binary self.binary = binary
self.model = model self.model = model
cmd=[] cmd = []
if valgrind and os.path.exists('/usr/bin/valgrind'): if valgrind and os.path.exists('/usr/bin/valgrind'):
cmd.extend(['valgrind', '-q', '--log-file=%s' % self.valgrind_log_filepath()]) cmd.extend(['valgrind', '-q', '--log-file=%s' % self.valgrind_log_filepath()])
if gdb: if gdb:
f = open("/tmp/x.gdb", "w") f = open("/tmp/x.gdb", "w")
f.write("r\n"); f.write("r\n")
f.close() f.close()
cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args']) cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])
@ -185,7 +209,7 @@ class SIL(pexpect.spawn):
print("Running: %s" % cmd_as_shell(cmd)) print("Running: %s" % cmd_as_shell(cmd))
first = cmd[0] first = cmd[0]
rest = cmd[1:] rest = cmd[1:]
super(SIL,self).__init__(first, rest, logfile=sys.stdout, timeout=5) super(SITL, self).__init__(first, rest, logfile=sys.stdout, timeout=5)
delaybeforesend = 0 delaybeforesend = 0
pexpect_autoclose(self) pexpect_autoclose(self)
# give time for parameters to properly setup # give time for parameters to properly setup
@ -193,24 +217,25 @@ class SIL(pexpect.spawn):
if gdb: if gdb:
# if we run GDB we do so in an xterm. "Waiting for # if we run GDB we do so in an xterm. "Waiting for
# connection" is never going to appear on xterm's output. # connection" is never going to appear on xterm's output.
#... so let's give it another magic second. # ... so let's give it another magic second.
time.sleep(1) time.sleep(1)
# TODO: have a SITL-compiled ardupilot able to have its # TODO: have a SITL-compiled ardupilot able to have its
# console on an output fd. # console on an output fd.
else: else:
self.expect('Waiting for connection',timeout=300) self.expect('Waiting for connection', timeout=300)
def valgrind_log_filepath(self): def valgrind_log_filepath(self):
return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(self.binary),self.model,)) return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(self.binary), self.model,))
def start_SIL(binary, **kwargs):
'''launch a SIL instance'''
return SIL(binary, **kwargs)
def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760', def start_SITL(binary, **kwargs):
options=None, logfile=sys.stdout): """Launch a SITL instance."""
'''launch mavproxy connected to a SIL instance''' return SITL(binary, **kwargs)
def start_MAVProxy_SITL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760',
options=None, logfile=sys.stdout):
"""Launch mavproxy connected to a SITL instance."""
import pexpect import pexpect
global close_list global close_list
MAVPROXY = os.getenv('MAVPROXY_CMD', 'mavproxy.py') MAVPROXY = os.getenv('MAVPROXY_CMD', 'mavproxy.py')
@ -229,9 +254,10 @@ def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:
def expect_setup_callback(e, callback): def expect_setup_callback(e, callback):
'''setup a callback that is called once a second while waiting for """Setup a callback that is called once a second while waiting for
patterns''' patterns."""
import pexpect import pexpect
def _expect_callback(pattern, timeout=e.timeout): def _expect_callback(pattern, timeout=e.timeout):
tstart = time.time() tstart = time.time()
while time.time() < tstart + timeout: while time.time() < tstart + timeout:
@ -248,27 +274,30 @@ def expect_setup_callback(e, callback):
e.expect_saved = e.expect e.expect_saved = e.expect
e.expect = _expect_callback e.expect = _expect_callback
def mkdir_p(dir):
'''like mkdir -p''' def mkdir_p(directory):
if not dir: """Like mkdir -p ."""
if not directory:
return return
if dir.endswith("/"): if directory.endswith("/"):
mkdir_p(dir[:-1]) mkdir_p(directory[:-1])
return return
if os.path.isdir(dir): if os.path.isdir(directory):
return return
mkdir_p(os.path.dirname(dir)) mkdir_p(os.path.dirname(directory))
os.mkdir(dir) os.mkdir(directory)
def loadfile(fname): def loadfile(fname):
'''load a file as a string''' """Load a file as a string."""
f = open(fname, mode='r') f = open(fname, mode='r')
r = f.read() r = f.read()
f.close() f.close()
return r return r
def lock_file(fname): def lock_file(fname):
'''lock a file''' """Lock a file."""
import fcntl import fcntl
f = open(fname, mode='w') f = open(fname, mode='w')
try: try:
@ -277,8 +306,9 @@ def lock_file(fname):
return None return None
return f return f
def check_parent(parent_pid=None): def check_parent(parent_pid=None):
'''check our parent process is still alive''' """Check our parent process is still alive."""
if parent_pid is None: if parent_pid is None:
try: try:
parent_pid = os.getppid() parent_pid = os.getppid()
@ -294,13 +324,13 @@ def check_parent(parent_pid=None):
def EarthRatesToBodyRates(dcm, earth_rates): def EarthRatesToBodyRates(dcm, earth_rates):
'''convert the angular velocities from earth frame to """Convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula body frame. Thanks to James Goppert for the formula
all inputs and outputs are in radians all inputs and outputs are in radians
returns a gyro vector in body frame, in rad/s returns a gyro vector in body frame, in rad/s .
''' """
from math import sin, cos from math import sin, cos
(phi, theta, psi) = dcm.to_euler() (phi, theta, psi) = dcm.to_euler()
@ -308,19 +338,20 @@ def EarthRatesToBodyRates(dcm, earth_rates):
thetaDot = earth_rates.y thetaDot = earth_rates.y
psiDot = earth_rates.z psiDot = earth_rates.z
p = phiDot - psiDot*sin(theta) p = phiDot - psiDot * sin(theta)
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta) q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta)
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot
return Vector3(p, q, r) return Vector3(p, q, r)
def BodyRatesToEarthRates(dcm, gyro): def BodyRatesToEarthRates(dcm, gyro):
'''convert the angular velocities from body frame to """Convert the angular velocities from body frame to
earth frame. earth frame.
all inputs and outputs are in radians/s all inputs and outputs are in radians/s
returns a earth rate vector returns a earth rate vector.
''' """
from math import sin, cos, tan, fabs from math import sin, cos, tan, fabs
p = gyro.x p = gyro.x
@ -329,37 +360,38 @@ def BodyRatesToEarthRates(dcm, gyro):
(phi, theta, psi) = dcm.to_euler() (phi, theta, psi) = dcm.to_euler()
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi)) phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi))
thetaDot = q*cos(phi) - r*sin(phi) thetaDot = q * cos(phi) - r * sin(phi)
if fabs(cos(theta)) < 1.0e-20: if fabs(cos(theta)) < 1.0e-20:
theta += 1.0e-10 theta += 1.0e-10
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta) psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta)
return Vector3(phiDot, thetaDot, psiDot) return Vector3(phiDot, thetaDot, psiDot)
radius_of_earth = 6378100.0 # in meters radius_of_earth = 6378100.0 # in meters
def gps_newpos(lat, lon, bearing, distance): def gps_newpos(lat, lon, bearing, distance):
'''extrapolate latitude/longitude given a heading and distance """Extrapolate latitude/longitude given a heading and distance
thanks to http://www.movable-type.co.uk/scripts/latlong.html thanks to http://www.movable-type.co.uk/scripts/latlong.html .
''' """
from math import sin, asin, cos, atan2, radians, degrees from math import sin, asin, cos, atan2, radians, degrees
lat1 = radians(lat) lat1 = radians(lat)
lon1 = radians(lon) lon1 = radians(lon)
brng = radians(bearing) brng = radians(bearing)
dr = distance/radius_of_earth dr = distance / radius_of_earth
lat2 = asin(sin(lat1)*cos(dr) + lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1)*sin(dr)*cos(brng)) cos(lat1) * sin(dr) * cos(brng))
lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
cos(dr)-sin(lat1)*sin(lat2)) cos(dr) - sin(lat1) * sin(lat2))
return (degrees(lat2), degrees(lon2)) return (degrees(lat2), degrees(lon2))
def gps_distance(lat1, lon1, lat2, lon2): def gps_distance(lat1, lon1, lat2, lon2):
'''return distance between two points in meters, """Return distance between two points in meters,
coordinates are in degrees coordinates are in degrees
thanks to http://www.movable-type.co.uk/scripts/latlong.html''' thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
lat1 = math.radians(lat1) lat1 = math.radians(lat1)
lat2 = math.radians(lat2) lat2 = math.radians(lat2)
lon1 = math.radians(lon1) lon1 = math.radians(lon1)
@ -367,35 +399,36 @@ def gps_distance(lat1, lon1, lat2, lon2):
dLat = lat2 - lat1 dLat = lat2 - lat1
dLon = lon2 - lon1 dLon = lon2 - lon1
a = math.sin(0.5*dLat)**2 + math.sin(0.5*dLon)**2 * math.cos(lat1) * math.cos(lat2) a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0-a)) c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
return radius_of_earth * c return radius_of_earth * c
def gps_bearing(lat1, lon1, lat2, lon2): def gps_bearing(lat1, lon1, lat2, lon2):
'''return bearing between two points in degrees, in range 0-360 """Return bearing between two points in degrees, in range 0-360
thanks to http://www.movable-type.co.uk/scripts/latlong.html''' thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
lat1 = math.radians(lat1) lat1 = math.radians(lat1)
lat2 = math.radians(lat2) lat2 = math.radians(lat2)
lon1 = math.radians(lon1) lon1 = math.radians(lon1)
lon2 = math.radians(lon2) lon2 = math.radians(lon2)
dLat = lat2 - lat1
dLon = lon2 - lon1 dLon = lon2 - lon1
y = math.sin(dLon) * math.cos(lat2) y = math.sin(dLon) * math.cos(lat2)
x = math.cos(lat1)*math.sin(lat2) - math.sin(lat1)*math.cos(lat2)*math.cos(dLon) x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
bearing = math.degrees(math.atan2(y, x)) bearing = math.degrees(math.atan2(y, x))
if bearing < 0: if bearing < 0:
bearing += 360.0 bearing += 360.0
return bearing return bearing
class Wind(object): class Wind(object):
'''a wind generation object''' """A wind generation object."""
def __init__(self, windstring, cross_section=0.1): def __init__(self, windstring, cross_section=0.1):
a = windstring.split(',') a = windstring.split(',')
if len(a) != 3: if len(a) != 3:
raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring) raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring)
self.speed = float(a[0]) # m/s self.speed = float(a[0]) # m/s
self.direction = float(a[1]) # direction the wind is going in self.direction = float(a[1]) # direction the wind is going in
self.turbulance= float(a[2]) # turbulance factor (standard deviation) self.turbulance = float(a[2]) # turbulance factor (standard deviation)
# the cross-section of the aircraft to wind. This is multiplied by the # the cross-section of the aircraft to wind. This is multiplied by the
# difference in the wind and the velocity of the aircraft to give the acceleration # difference in the wind and the velocity of the aircraft to give the acceleration
@ -412,26 +445,24 @@ class Wind(object):
self.turbulance_mul = 1.0 self.turbulance_mul = 1.0
def current(self, deltat=None): def current(self, deltat=None):
'''return current wind speed and direction as a tuple """Return current wind speed and direction as a tuple
speed is in m/s, direction in degrees speed is in m/s, direction in degrees."""
'''
if deltat is None: if deltat is None:
tnow = time.time() tnow = time.time()
deltat = tnow - self.tlast deltat = tnow - self.tlast
self.tlast = tnow self.tlast = tnow
# update turbulance random walk # update turbulance random walk
w_delta = math.sqrt(deltat)*(1.0-random.gauss(1.0, self.turbulance)) w_delta = math.sqrt(deltat) * (1.0 - random.gauss(1.0, self.turbulance))
w_delta -= (self.turbulance_mul-1.0)*(deltat/self.turbulance_time_constant) w_delta -= (self.turbulance_mul - 1.0) * (deltat / self.turbulance_time_constant)
self.turbulance_mul += w_delta self.turbulance_mul += w_delta
speed = self.speed * math.fabs(self.turbulance_mul) speed = self.speed * math.fabs(self.turbulance_mul)
return (speed, self.direction) return (speed, self.direction)
# Calculate drag. # Calculate drag.
def drag(self, velocity, deltat=None, testing=None): def drag(self, velocity, deltat=None, testing=None):
'''return current wind force in Earth frame. The velocity parameter is """Return current wind force in Earth frame. The velocity parameter is
a Vector3 of the current velocity of the aircraft in earth frame, m/s''' a Vector3 of the current velocity of the aircraft in earth frame, m/s ."""
from math import radians from math import radians
# (m/s, degrees) : wind vector as a magnitude and angle. # (m/s, degrees) : wind vector as a magnitude and angle.
@ -447,9 +478,9 @@ class Wind(object):
# Compute the angle between the object vector and wind vector by taking # Compute the angle between the object vector and wind vector by taking
# the dot product and dividing by the magnitudes. # the dot product and dividing by the magnitudes.
d = w.length() * obj_speed d = w.length() * obj_speed
if d == 0: if d == 0:
alpha = 0 alpha = 0
else: else:
alpha = acos((w * velocity) / d) alpha = acos((w * velocity) / d)
# Get the relative wind speed and angle from the object. Note that the # Get the relative wind speed and angle from the object. Note that the
@ -463,16 +494,15 @@ class Wind(object):
relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x)) relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x))
# Combine them to get the acceleration vector. # Combine them to get the acceleration vector.
return Vector3( acc(relWindVec.x, drag_force(self, relWindVec.x)) return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), acc(relWindVec.y, drag_force(self, relWindVec.y)), 0)
, acc(relWindVec.y, drag_force(self, relWindVec.y))
, 0 )
# http://en.wikipedia.org/wiki/Apparent_wind
#
# Returns apparent wind speed and angle of apparent wind. Alpha is the angle
# between the object and the true wind. alpha of 0 rads is a headwind; pi a
# tailwind. Speeds should always be positive.
def apparent_wind(wind_sp, obj_speed, alpha): def apparent_wind(wind_sp, obj_speed, alpha):
"""http://en.wikipedia.org/wiki/Apparent_wind
Returns apparent wind speed and angle of apparent wind. Alpha is the angle
between the object and the true wind. alpha of 0 rads is a headwind; pi a
tailwind. Speeds should always be positive."""
delta = wind_sp * cos(alpha) delta = wind_sp * cos(alpha)
x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta
rel_speed = sqrt(x) rel_speed = sqrt(x)
@ -483,37 +513,41 @@ def apparent_wind(wind_sp, obj_speed, alpha):
return (rel_speed, beta) return (rel_speed, beta)
# See http://en.wikipedia.org/wiki/Drag_equation
# def drag_force(wind, sp):
# Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient """See http://en.wikipedia.org/wiki/Drag_equation
# (let's assume it's low, .e.g., 0.2), p : density of air (assume about 1
# kg/m^3, the density just over 1500m elevation), v : relative speed of wind Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient
# (to the body), a : area acted on (this is captured by the cross_section (let's assume it's low, .e.g., 0.2), p : density of air (assume about 1
# parameter). kg/m^3, the density just over 1500m elevation), v : relative speed of wind
# (to the body), a : area acted on (this is captured by the cross_section
# So then we have parameter).
# F(a) = 0.2 * 1/2 * v^2 * cross_section = 0.1 * v^2 * cross_section
def drag_force(wind, sp): So then we have
F(a) = 0.2 * 1/2 * v^2 * cross_section = 0.1 * v^2 * cross_section."""
return (sp**2.0) * 0.1 * wind.cross_section return (sp**2.0) * 0.1 * wind.cross_section
# Function to make the force vector. relWindVec is the direction the apparent
# wind comes *from*. We want to compute the accleration vector in the direction
# the wind blows to.
def acc(val, mag): def acc(val, mag):
""" Function to make the force vector. relWindVec is the direction the apparent
wind comes *from*. We want to compute the accleration vector in the direction
the wind blows to."""
if val == 0: if val == 0:
return mag return mag
else: else:
return (val / abs(val)) * (0 - mag) return (val / abs(val)) * (0 - mag)
# Converts a magnitude and angle (radians) to a vector in the xy plane.
def toVec(magnitude, angle): def toVec(magnitude, angle):
"""Converts a magnitude and angle (radians) to a vector in the xy plane."""
v = Vector3(magnitude, 0, 0) v = Vector3(magnitude, 0, 0)
m = Matrix3() m = Matrix3()
m.from_euler(0, 0, angle) m.from_euler(0, 0, angle)
return m.transposed() * v return m.transposed() * v
def constrain(value, minv, maxv): def constrain(value, minv, maxv):
'''constrain a value to a range''' """Constrain a value to a range."""
if value < minv: if value < minv:
value = minv value = minv
if value > maxv: if value > maxv:
@ -523,4 +557,3 @@ def constrain(value, minv, maxv):
if __name__ == "__main__": if __name__ == "__main__":
import doctest import doctest
doctest.testmod() doctest.testmod()

50
Tools/autotest/quadplane.py
View File

@ -1,23 +1,27 @@
# fly ArduPlane QuadPlane in SITL # fly ArduPlane QuadPlane in SITL
import util, pexpect, sys, time, math, shutil, os import os
from common import * import pexpect
import shutil
from pymavlink import mavutil from pymavlink import mavutil
import random
from common import *
from pysim import util
# get location of scripts # get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__)) testdir = os.path.dirname(os.path.realpath(__file__))
HOME_LOCATION='-27.274439,151.290064,343,8.7' HOME_LOCATION = '-27.274439,151.290064,343,8.7'
MISSION='ArduPlane-Missions/Dalby-OBC2016.txt' MISSION = 'ArduPlane-Missions/Dalby-OBC2016.txt'
FENCE='ArduPlane-Missions/Dalby-OBC2016-fence.txt' FENCE = 'ArduPlane-Missions/Dalby-OBC2016-fence.txt'
WIND="0,180,0.2" # speed,direction,variance WIND = "0,180,0.2" # speed,direction,variance
homeloc = None homeloc = None
def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1): def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
'''fly a mission from a file''' """Fly a mission from a file."""
print("Flying mission %s" % filename) print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename) mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received') mavproxy.expect('Flight plan received')
@ -40,23 +44,23 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
return True return True
def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False): def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''fly QuadPlane in SIL """Fly QuadPlane in SITL.
you can pass viewerip as an IP address to optionally send fg and you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time mavproxy packets too for local viewing of the flight in real time.
''' """
global homeloc global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10' options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip: if viewerip:
options += " --out=%s:14550" % viewerip options += " --out=%s:14550" % viewerip
if map: if use_map:
options += ' --map' options += ' --map'
sil = util.start_SIL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10, sitl = util.start_SITL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb) defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('QuadPlane', options=options) mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)') mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1) logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile) print("LOGFILE %s" % logfile)
@ -75,14 +79,14 @@ def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
mavproxy.expect('Received [0-9]+ parameters') mavproxy.expect('Received [0-9]+ parameters')
expect_list_clear() expect_list_clear()
expect_list_extend([sil, mavproxy]) expect_list_extend([sitl, mavproxy])
print("Started simulator") print("Started simulator")
# get a mavlink connection going # get a mavlink connection going
try: try:
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True) mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
except Exception, msg: except Exception as msg:
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg) print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
raise raise
mav.message_hooks.append(message_hook) mav.message_hooks.append(message_hook)
@ -106,21 +110,21 @@ def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
mavproxy.send('arm throttle\n') mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED') mavproxy.expect('ARMED')
if not fly_mission(mavproxy, mav, if not fly_mission(mavproxy, mav,
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"), os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"),
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")): os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")):
print("Failed mission") print("Failed mission")
failed = True failed = True
except pexpect.TIMEOUT, e: except pexpect.TIMEOUT as e:
print("Failed with timeout") print("Failed with timeout")
failed = True failed = True
mav.close() mav.close()
util.pexpect_close(mavproxy) util.pexpect_close(mavproxy)
util.pexpect_close(sil) util.pexpect_close(sitl)
valgrind_log = sil.valgrind_log_filepath() valgrind_log = sitl.valgrind_log_filepath()
if os.path.exists(valgrind_log): if os.path.exists(valgrind_log):
os.chmod(valgrind_log, 0644) os.chmod(valgrind_log, 0644)
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log")) shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log"))

6
Tools/autotest/sim_vehicle.py
View File

@ -142,7 +142,7 @@ def kill_tasks():
"""Clean up stray processes by name. This is a somewhat shotgun approach""" """Clean up stray processes by name. This is a somewhat shotgun approach"""
progress("Killing tasks") progress("Killing tasks")
try: try:
victim_names = set([ victim_names = {
'JSBSim', 'JSBSim',
'lt-JSBSim', 'lt-JSBSim',
'ArduPlane.elf', 'ArduPlane.elf',
@ -153,9 +153,9 @@ def kill_tasks():
'MAVProxy.exe', 'MAVProxy.exe',
'runsim.py', 'runsim.py',
'AntennaTracker.elf', 'AntennaTracker.elf',
]) }
for frame in _options_for_frame.keys(): for frame in _options_for_frame.keys():
if not _options_for_frame[frame].has_key("waf_target"): if "waf_target" not in _options_for_frame[frame]:
continue continue
exe_name = os.path.basename(_options_for_frame[frame]["waf_target"]) exe_name = os.path.basename(_options_for_frame[frame]["waf_target"])
victim_names.add(exe_name) victim_names.add(exe_name)