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
'''
"""
Extract version information for the various vehicle types, print it
'''
"""
import sys
import re
import os
import re
import sys
from optparse import OptionParser
parser = OptionParser("print_version.py [options] ArduCopter|ArduPlane|APMrover2|AntennaTracker")
(opts, args) = parser.parse_args()
@ -25,7 +26,7 @@ if len(args) > 0:
if vehicle not in includefiles:
print("Unknown vehicle (%s) (be in a vehicle directory or supply a vehicle type as an argument)" % (vehicle,))
sys.exit(1)
includefilepath="%s/%s" % (vehicle, includefiles[vehicle])
includefilepath = "%s/%s" % (vehicle, includefiles[vehicle])
else:
# assume we are in e.g. APM/APMrover2/
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
import util, pexpect, sys, time, math, shutil, os
from common import *
import os
import shutil
import pexpect
from pymavlink import mavutil
import random
from common import *
from pysim import util
# 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(40.071374969556928,-105.22978898137808,1583.702759,246)
# HOME=mavutil.location(-35.362938,149.165085,584,270)
HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
homeloc = None
def arm_rover(mavproxy, mav):
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
@ -23,15 +28,16 @@ def arm_rover(mavproxy, mav):
print("ROVER ARMED")
return True
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')
wait_mode(mav, 'MANUAL')
mavproxy.send('rc 3 2000\n')
print("Driving left circuit")
# do 4 turns
for i in range(0,4):
for i in range(0, 4):
# hard left
print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n')
@ -45,8 +51,9 @@ def drive_left_circuit(mavproxy, mav):
print("Circuit complete")
return True
def drive_RTL(mavproxy, mav):
'''drive to home'''
"""Drive to home."""
print("Driving home in RTL")
mavproxy.send('switch 3\n')
if not wait_location(mav, homeloc, accuracy=22, timeout=90):
@ -54,22 +61,23 @@ def drive_RTL(mavproxy, mav):
print("RTL Complete")
return True
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in [1,2,3,4,5,6,7]:
"""Setup RC override control."""
for chan in [1, 2, 3, 4, 5, 6, 7]:
mavproxy.send('rc %u 1500\n' % chan)
mavproxy.send('rc 8 1800\n')
def drive_mission(mavproxy, mav, filename):
'''drive a mission from a file'''
"""Drive a mission from a file."""
global homeloc
print("Driving mission %s" % filename)
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received')
mavproxy.send('wp list\n')
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')
wait_mode(mav, 'AUTO')
if not wait_waypoint(mav, 1, 4, max_dist=5):
@ -79,23 +87,23 @@ def drive_mission(mavproxy, mav, filename):
return True
def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''drive APMrover2 in SIL
def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
"""Drive APMrover2 in SITL.
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
'''
"""
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
if use_map:
options += ' --map'
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)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
sitl = util.start_SITL(binary, wipe=True, model='rover', home=home, speedup=10)
mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
print("WAITING FOR 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
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)
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
sitl = util.start_SITL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
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)
expect_list_clear()
expect_list_extend([sil, mavproxy])
expect_list_extend([sitl, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
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)
raise
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):
# print("Failed RTL")
# failed = True
except pexpect.TIMEOUT, e:
except pexpect.TIMEOUT as e:
print("Failed with timeout")
failed = True
mav.close()
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):
os.chmod(valgrind_log, 0644)
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 6 = Stabilize
import util, pexpect, sys, time, math, shutil, os
from common import *
import math
import os
import shutil
import time
import pexpect
from pymavlink import mavutil, mavwp
import random
from common import *
from pysim import util
# get location of scripts
testdir=os.path.dirname(os.path.realpath(__file__))
testdir = os.path.dirname(os.path.realpath(__file__))
FRAME='+'
HOME=mavutil.location(-35.362938,149.165085,584,270)
AVCHOME=mavutil.location(40.072842,-105.230575,1586,0)
FRAME = '+'
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
homeloc = None
num_wp = 0
speedup_default = 10
def hover(mavproxy, mav, hover_throttle=1500):
mavproxy.send('rc 3 %u\n' % hover_throttle)
return True
def arm_motors(mavproxy, mav):
'''arm motors'''
"""Arm motors."""
print("Arming motors")
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 2000\n')
@ -40,10 +48,11 @@ def arm_motors(mavproxy, mav):
print("MOTORS ARMED OK")
return True
def disarm_motors(mavproxy, mav):
'''disarm motors'''
"""Disarm motors."""
print("Disarming motors")
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 4 1000\n')
@ -54,9 +63,9 @@ def disarm_motors(mavproxy, mav):
return True
def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
'''takeoff get to 30m altitude'''
mavproxy.send('switch 6\n') # stabilize mode
def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
"""Takeoff get to 30m altitude."""
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 %u\n' % takeoff_throttle)
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")
return True
# loiter - fly south west, then hold loiter within 5m position and altitude
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
"""Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# first aim south east
@ -79,7 +89,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
return False
mavproxy.send('rc 4 1500\n')
#fly south east 50m
# fly south east 50m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 50):
return False
@ -114,8 +124,9 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
print("Loiter FAILED")
return success
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)
if(m.alt < alt_min):
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:
print("Lower to alt:%u from %u" % (alt_min, m.alt))
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)
return True
# fly a square in stabilize mode
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)
success = True
@ -166,7 +178,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
print("Going north %u meters" % side)
mavproxy.send('rc 2 1300\n')
if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side
print("Failed to reach distance of %u" % side)
success = False
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)
mavproxy.send('rc 1 1700\n')
if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side
print("Failed to reach distance of %u" % side)
success = False
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)
mavproxy.send('rc 2 1700\n')
if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side
print("Failed to reach distance of %u" % side)
success = False
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)
mavproxy.send('rc 1 1300\n')
if not wait_distance(mav, side):
print("Failed to reach distance of %u") % side
print("Failed to reach distance of %u" % side)
success = False
mavproxy.send('rc 1 1500\n')
@ -212,7 +224,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
# descend to 10m
print("Descend to 10m in Loiter")
mavproxy.send('switch 5\n') # loiter mode
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
mavproxy.send('rc 3 1300\n')
time_left = timeout - (get_sim_time(mav) - tstart)
@ -226,8 +238,9 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
return success
def fly_RTL(mavproxy, mav, side=60, timeout=250):
'''Return, land'''
"""Return, land."""
print("# Enter RTL")
mavproxy.send('switch 3\n')
tstart = get_sim_time(mav)
@ -240,8 +253,9 @@ def fly_RTL(mavproxy, mav, side=60, timeout=250):
return True
return False
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
mavproxy.send('switch 5\n')
@ -285,9 +299,9 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
# reduce throttle
mavproxy.send('rc 3 1100\n')
# switch back to stabilize
mavproxy.send('switch 2\n') # land mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Reached failsafe home OK")
return True
@ -295,12 +309,13 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
# reduce throttle
mavproxy.send('rc 3 1100\n')
# switch back to stabilize mode
mavproxy.send('switch 2\n') # land mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
return False
def fly_battery_failsafe(mavproxy, mav, timeout=30):
# assume failure
success = False
@ -332,10 +347,11 @@ def fly_battery_failsafe(mavproxy, mav, timeout=30):
return success
# 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):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
"""Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# first south
@ -345,7 +361,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
return False
mavproxy.send('rc 4 1500\n')
#fly west 80m
# fly west 80m
mavproxy.send('rc 2 1100\n')
if not wait_distance(mav, 80):
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')
if success:
print("Stability patch and Loiter OK for %u seconds" % holdtime)
print("Stability patch and Loiter OK for %u seconds" % holdtime)
else:
print("Stability Patch FAILED")
return success
# fly_fence_test - fly east until you hit the horizontal circular fence
def fly_fence_test(mavproxy, mav, timeout=180):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
"""Hold loiter position."""
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# enable fence, disable avoidance
@ -421,7 +438,7 @@ def fly_fence_test(mavproxy, mav, timeout=180):
home_distance = get_distance(HOME, pos)
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
# 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
mavproxy.send('rc 2 1500\n')
# disable fence
@ -430,9 +447,9 @@ def fly_fence_test(mavproxy, mav, timeout=180):
# reduce throttle
mavproxy.send('rc 3 1000\n')
# switch mode to stabilize
mavproxy.send('switch 2\n') # land mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Reached home OK")
return True
@ -444,15 +461,16 @@ def fly_fence_test(mavproxy, mav, timeout=180):
# reduce throttle
mavproxy.send('rc 3 1000\n')
# switch mode to stabilize
mavproxy.send('switch 2\n') # land mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
mavproxy.send('switch 6\n') # stabilize mode
mavproxy.send('switch 6\n') # stabilize mode
wait_mode(mav, 'STABILIZE')
print("Fence test failed to reach home - timed out after %u seconds" % timeout)
return False
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
mavproxy.send('map set showgpspos 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
mavproxy.send('map set showgpspos 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):
'''hold loiter position'''
mavproxy.send('switch 5\n') # loiter mode
"""fly_gps_glitch_loiter_test.
Fly south east in loiter and test reaction to gps glitch."""
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# turn on simulator display of gps and actual position
if (use_map):
show_gps_and_sim_positions(mavproxy, True)
# set-up gps glitch array
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_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_num = len(glitch_lat)
print("GPS Glitches:")
for i in range(1,glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
for i in range(1, glitch_num):
print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
# 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
# initialise current glitch
glitch_current = 0;
glitch_current = 0
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_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')
else:
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_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!")
return success
# 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):
# set-up gps glitch array
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_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_num = len(glitch_lat)
print("GPS Glitches:")
for i in range(1,glitch_num):
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
for i in range(1, glitch_num):
print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
# Fly mission #1
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')
# 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')
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
# initialise current glitch
glitch_current = 0;
glitch_current = 0
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_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
#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
def fly_simple(mavproxy, mav, side=50, timeout=120):
failed = False
# hold position in loiter
mavproxy.send('switch 5\n') # loiter mode
mavproxy.send('switch 5\n') # loiter mode
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')
# switch to stabilize mode
@ -679,7 +700,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
while get_sim_time(mav) < (tstart + 8):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (get_sim_time(mav) - tstart)
#print("%u" % delta)
# print("%u" % delta)
mavproxy.send('rc 2 1500\n')
# fly north 25 meters
@ -696,23 +717,24 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
while get_sim_time(mav) < (tstart + 8):
m = mav.recv_match(type='VFR_HUD', blocking=True)
delta = (get_sim_time(mav) - tstart)
#print("%u" % delta)
# print("%u" % delta)
mavproxy.send('rc 2 1500\n')
#restore to default
# restore to default
mavproxy.send('param set SIMPLE 0\n')
#hover in place
# hover in place
hover(mavproxy, mav)
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):
failed = False
# hold position in loiter
mavproxy.send('switch 5\n') # loiter mode
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# fly forward 20m
@ -722,7 +744,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
failed = True
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')
# 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 4 1500\n')
#restore simple mode parameters to default
# restore simple mode parameters to default
mavproxy.send('param set SUPER_SIMPLE 0\n')
#hover in place
# hover in place
hover(mavproxy, mav)
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):
# hold position in loiter
mavproxy.send('switch 5\n') # loiter mode
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
# face west
@ -766,7 +789,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
return False
mavproxy.send('rc 4 1500\n')
#set CIRCLE radius
# set CIRCLE radius
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
# 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')
# set CIRCLE mode
mavproxy.send('switch 1\n') # circle mode
mavproxy.send('switch 1\n') # circle mode
wait_mode(mav, 'CIRCLE')
# wait
# wait
m = mav.recv_match(type='VFR_HUD', blocking=True)
start_altitude = m.alt
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)
return True
# fly_auto_test - fly mission which tests a significant number of commands
def fly_auto_test(mavproxy, mav):
@ -810,7 +834,7 @@ def fly_auto_test(mavproxy, mav):
mavproxy.send('wp set 1\n')
# 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')
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)
# land if mission failed
if ret == False:
if ret is False:
land(mavproxy, mav)
# set throttle to minimum
mavproxy.send('rc 3 1000\n')
@ -832,9 +856,10 @@ def fly_auto_test(mavproxy, mav):
return ret
# fly_avc_test - fly AVC mission
def fly_avc_test(mavproxy, mav):
# upload mission from file
print("# Load copter_AVC2013_mission")
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')
# 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')
mavproxy.send('rc 3 1500\n')
@ -866,23 +891,25 @@ def fly_avc_test(mavproxy, mav):
return ret
def land(mavproxy, mav, timeout=60):
'''land the quad'''
"""Land the quad."""
print("STARTING LANDING")
mavproxy.send('switch 2\n') # land mode
mavproxy.send('switch 2\n') # land mode
wait_mode(mav, 'LAND')
print("Entered Landing Mode")
ret = wait_altitude(mav, -5, 1)
print("LANDING: ok= %s" % 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 num_wp
print("test: Fly a mission from 1 to %u" % num_wp)
mavproxy.send('wp set 1\n')
mavproxy.send('switch 4\n') # auto mode
mavproxy.send('switch 4\n') # auto mode
wait_mode(mav, 'AUTO')
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
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)
print("test: MISSION COMPLETE: passed=%s" % ret)
# wait here until ready
mavproxy.send('switch 5\n') # loiter mode
mavproxy.send('switch 5\n') # loiter mode
wait_mode(mav, 'LOITER')
return ret
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
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received')
@ -908,6 +936,7 @@ def load_mission_from_file(mavproxy, mav, filename):
num_wp = wploader.count()
return True
def save_mission_to_file(mavproxy, mav, filename):
global num_wp
mavproxy.send('wp save %s\n' % filename)
@ -916,24 +945,26 @@ def save_mission_to_file(mavproxy, mav, filename):
print("num_wp: %d" % num_wp)
return True
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in range(1,9):
"""Setup RC override control."""
for chan in range(1, 9):
mavproxy.send('rc %u 1500\n' % chan)
# zero throttle
mavproxy.send('rc 3 1000\n')
def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL
"""Fly ArduCopter in SITL.
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time
'''
"""
global homeloc
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)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
sitl = util.start_SITL(binary, wipe=True, model='+', home=home, speedup=speedup_default)
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')
# setup test parameters
@ -945,29 +976,29 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
# reboot with new parameters
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'
if viewerip:
options += ' --out=%s:14550' % viewerip
if use_map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
print("LOGFILE %s" % logfile)
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
print("buildlog=%s" % buildlog)
copyTLog = False
copy_tlog = False
if os.path.exists(buildlog):
os.unlink(buildlog)
try:
os.link(logfile, buildlog)
except Exception:
print( "WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location" )
copyTLog = True
print("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location")
copy_tlog = True
# the received parameters can come before or after the ready to fly message
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)
expect_list_clear()
expect_list_extend([sil, mavproxy])
expect_list_extend([sitl, mavproxy])
# get a mavlink connection going
try:
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)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
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
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"
print(failed_test_msg)
failed = True
@ -1250,38 +1280,37 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
print(failed_test_msg)
failed = True
except pexpect.TIMEOUT, failed_test_msg:
except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout"
failed = True
mav.close()
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):
os.chmod(valgrind_log, 0644)
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
# This flag tells me that I need to copy the data out
if copyTLog:
if copy_tlog:
shutil.copy(logfile, buildlog)
if failed:
print("FAILED: %s" % failed_test_msg)
return False
return True
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly ArduCopter in SIL for AVC2013 mission
'''
def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
"""Fly ArduCopter in SITL for AVC2013 mission."""
global homeloc
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)
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
sitl = util.start_SITL(binary, wipe=True, model='heli', home=home, speedup=speedup_default)
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')
# setup test parameters
@ -1293,15 +1322,15 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
# reboot with new parameters
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'
if viewerip:
options += ' --out=%s:14550' % viewerip
if map:
if use_map:
options += ' --map'
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
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)
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.072600990533829 -105.23146100342274\n')
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
# get a mavlink connection going
try:
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)
raise
mav.message_hooks.append(message_hook)
mav.idle_hooks.append(idle_hook)
failed = False
failed_test_msg = "None"
@ -1373,21 +1401,21 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
print("Lowering rotor speed")
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")):
failed_test_msg = "log_download failed"
print(failed_test_msg)
failed = True
except pexpect.TIMEOUT, failed_test_msg:
except pexpect.TIMEOUT as failed_test_msg:
failed_test_msg = "Timeout"
failed = True
mav.close()
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):
os.chmod(valgrind_log, 0644)
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
from common import *
import math
import os
import shutil
import pexpect
from pymavlink import mavutil
import random
from common import *
from pysim import util
# 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'
WIND="0,180,0.2" # speed,direction,variance
HOME_LOCATION = '-35.362938,149.165085,585,354'
WIND = "0,180,0.2" # speed,direction,variance
homeloc = None
def takeoff(mavproxy, mav):
'''takeoff get to 30m altitude'''
"""Takeoff get to 30m altitude."""
# wait for EKF and GPS checks to pass
wait_seconds(mav, 30)
mavproxy.send('arm throttle\n')
mavproxy.expect('ARMED')
mavproxy.send('switch 4\n')
wait_mode(mav, 'FBWA')
@ -55,8 +61,9 @@ def takeoff(mavproxy, mav):
print("TAKEOFF COMPLETE")
return True
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')
wait_mode(mav, 'FBWA')
mavproxy.send('rc 3 2000\n')
@ -65,7 +72,7 @@ def fly_left_circuit(mavproxy, mav):
print("Flying left circuit")
# do 4 turns
for i in range(0,4):
for i in range(0, 4):
# hard left
print("Starting turn %u" % i)
mavproxy.send('rc 1 1000\n')
@ -78,8 +85,9 @@ def fly_left_circuit(mavproxy, mav):
print("Circuit complete")
return True
def fly_RTL(mavproxy, mav):
'''fly to home'''
"""Fly to home."""
print("Flying home in RTL")
mavproxy.send('switch 2\n')
wait_mode(mav, 'RTL')
@ -90,8 +98,9 @@ def fly_RTL(mavproxy, mav):
print("RTL Complete")
return True
def fly_LOITER(mavproxy, mav, num_circles=4):
'''loiter where we are'''
"""Loiter where we are."""
print("Testing LOITER for %u turns" % num_circles)
mavproxy.send('loiter\n')
wait_mode(mav, 'LOITER')
@ -122,8 +131,9 @@ def fly_LOITER(mavproxy, mav, num_circles=4):
print("Completed Loiter OK")
return True
def fly_CIRCLE(mavproxy, mav, num_circles=1):
'''circle where we are'''
"""Circle where we are."""
print("Testing CIRCLE for %u turns" % num_circles)
mavproxy.send('mode CIRCLE\n')
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):
'''wait for level flight'''
"""Wait for level flight."""
tstart = get_sim_time(mav)
print("Waiting for level flight")
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):
'''get to a given altitude'''
"""Get to a given altitude."""
mavproxy.send('mode FBWA\n')
wait_mode(mav, 'FBWA')
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):
'''fly a left axial roll'''
"""Fly a left axial roll."""
# full throttle!
mavproxy.send('rc 3 2000\n')
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):
'''fly a inside loop'''
"""Fly a inside loop."""
# full throttle!
mavproxy.send('rc 3 2000\n')
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):
'''fly stabilize mode'''
"""Fly stabilize mode."""
# full throttle!
mavproxy.send('rc 3 2000\n')
mavproxy.send('rc 2 1300\n')
@ -285,8 +295,9 @@ def test_stabilize(mavproxy, mav, count=1):
mavproxy.send('rc 3 1700\n')
return wait_level_flight(mavproxy, mav)
def test_acro(mavproxy, mav, count=1):
'''fly ACRO mode'''
"""Fly ACRO mode."""
# full throttle!
mavproxy.send('rc 3 2000\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'):
'''fly FBWB or CRUISE mode'''
"""Fly FBWB or CRUISE mode."""
mavproxy.send("mode %s\n" % mode)
wait_mode(mav, mode)
mavproxy.send('rc 3 1700\n')
@ -357,7 +368,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
print("Flying right circuit")
# do 4 turns
for i in range(0,4):
for i in range(0, 4):
# hard left
print("Starting turn %u" % i)
mavproxy.send('rc 1 1800\n')
@ -372,7 +383,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
print("Flying rudder left circuit")
# do 4 turns
for i in range(0,4):
for i in range(0, 4):
# hard left
print("Starting turn %u" % i)
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:
print("Failed to maintain altitude")
return False
return wait_level_flight(mavproxy, mav)
def setup_rc(mavproxy):
'''setup RC override control'''
for chan in [1,2,4,5,6,7]:
"""Setup RC override control."""
for chan in [1, 2, 4, 5, 6, 7]:
mavproxy.send('rc %u 1500\n' % chan)
mavproxy.send('rc 3 1000\n')
mavproxy.send('rc 8 1800\n')
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
'''fly a mission from a file'''
"""Fly a mission from a file."""
global homeloc
print("Flying mission %s" % filename)
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received')
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
mavproxy.send('switch 1\n') # auto mode
mavproxy.send('switch 1\n') # auto mode
wait_mode(mav, 'AUTO')
if not wait_waypoint(mav, 1, 7, max_dist=60):
return False
@ -426,24 +437,24 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non
return True
def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly ArduPlane in SIL
def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
"""Fly ArduPlane in SITL.
you can pass viewerip as an IP address to optionally send fg and
mavproxy packets too for local viewing of the flight in real time
'''
"""
global homeloc
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
if use_map:
options += ' --map'
sil = util.start_SIL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10,
valgrind=valgrind, gdb=gdb,
defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'))
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
sitl = util.start_SITL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10,
valgrind=valgrind, gdb=gdb,
defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'))
mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
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')
expect_list_clear()
expect_list_extend([sil, mavproxy])
expect_list_extend([sitl, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
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)
raise
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")
failed = True
fail_list.append("log_download")
except pexpect.TIMEOUT, e:
except pexpect.TIMEOUT as e:
print("Failed with timeout")
failed = True
fail_list.append("timeout")
mav.close()
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):
os.chmod(valgrind_log, 0644)
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
# APM automatic test suite
# Andrew Tridgell, October 2011
"""
APM automatic test suite
Andrew Tridgell, October 2011
"""
import pexpect, os, sys, shutil, atexit
import optparse, fnmatch, time, glob, traceback, signal
import atexit
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 util
import apmrover2
import arducopter
import arduplane
import quadplane
from pysim import util
os.environ['PYTHONUNBUFFERED'] = '1'
os.putenv('TMPDIR', util.reltopdir('tmp'))
def get_default_params(atype, binary):
'''get default parameters'''
"""Get default parameters."""
# use rover simulator so SITL is not starved of input
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:
frame = "rover"
else:
frame = "+"
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)
mavproxy = util.start_MAVProxy_SIL(atype)
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=10)
mavproxy = util.start_MAVProxy_SITL(atype)
print("Dumping defaults")
idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
if idx == 0:
# we need to restart it after eeprom erase
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
sil = util.start_SIL(binary, model=frame, home=home, speedup=10)
mavproxy = util.start_MAVProxy_SIL(atype)
util.pexpect_close(sitl)
sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
mavproxy = util.start_MAVProxy_SITL(atype)
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
parmfile = mavproxy.match.group(1)
dest = util.reltopdir('../buildlogs/%s-defaults.parm' % atype)
shutil.copy(parmfile, dest)
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
util.pexpect_close(sitl)
print("Saved defaults for %s to %s" % (atype, dest))
return True
def build_all():
'''run the build_all.sh script'''
"""Run the build_all.sh script."""
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")
return False
return True
def build_binaries():
'''run the build_binaries.sh script'''
"""Run the build_binaries.sh script."""
print("Running build_binaries.sh")
import shutil
# copy the script as it changes git branch, which can change the script while running
orig=util.reltopdir('Tools/scripts/build_binaries.sh')
copy=util.reltopdir('./build_binaries.sh')
orig = util.reltopdir('Tools/scripts/build_binaries.sh')
copy = util.reltopdir('./build_binaries.sh')
shutil.copyfile(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")
return False
return True
def build_devrelease():
'''run the build_devrelease.sh script'''
"""Run the build_devrelease.sh script."""
print("Running build_devrelease.sh")
import shutil
# copy the script as it changes git branch, which can change the script while running
orig=util.reltopdir('Tools/scripts/build_devrelease.sh')
copy=util.reltopdir('./build_devrelease.sh')
orig = util.reltopdir('Tools/scripts/build_devrelease.sh')
copy = util.reltopdir('./build_devrelease.sh')
shutil.copyfile(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")
return False
return True
def build_examples():
'''build examples'''
"""Build examples."""
for target in 'px4-v2', 'navio':
print("Running build.examples for %s" % target)
try:
@ -93,17 +110,18 @@ def build_examples():
return True
def build_parameters():
'''run the param_parse.py script'''
"""Run the param_parse.py script."""
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")
return False
return True
def convert_gpx():
'''convert any tlog files to GPX and KML'''
"""Convert any tlog files to GPX and KML."""
import glob
mavlog = glob.glob(util.reltopdir("../buildlogs/*.tlog"))
for m in mavlog:
@ -112,18 +130,19 @@ def convert_gpx():
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('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
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")
util.mkdir_p(util.reltopdir('../buildlogs'))
return True
def alarm_handler(signum, frame):
'''handle test timeout'''
"""Handle test timeout."""
global results, opts
try:
results.add('TIMEOUT', '<span class="failed-text">FAILED</span>', opts.timeout)
@ -135,6 +154,7 @@ def alarm_handler(signum, frame):
pass
sys.exit(1)
############## main program #############
parser = optparse.OptionParser("autotest")
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()
import arducopter, arduplane, apmrover2, quadplane
steps = [
'prerequisites',
'build.All',
'build.Binaries',
# 'build.DevRelease',
# 'build.DevRelease',
'build.Examples',
'build.Parameters',
@ -192,13 +211,15 @@ if opts.list:
print(step)
sys.exit(0)
def skip_step(step):
'''see if a step should be skipped'''
"""See if a step should be skipped."""
for skip in skipsteps:
if fnmatch.fnmatch(step.lower(), skip.lower()):
return True
return False
def binary_path(step, debug=False):
if step.find("ArduCopter") != -1:
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))
if not os.path.exists(binary):
if os.path.exists(binary + ".exe"):
binary_path += ".exe"
binary += ".exe"
else:
raise ValueError("Binary (%s) does not exist" % (binary,))
return binary
def run_step(step):
'''run one step'''
"""Run one step."""
# remove old logs
util.run_cmd('/bin/rm -f logs/*.BIN logs/LASTLOG.TXT')
@ -240,19 +262,19 @@ def run_step(step):
return test_prerequisites()
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':
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':
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':
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':
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)
@ -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)
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':
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':
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':
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':
return build_all()
@ -300,68 +322,71 @@ def run_step(step):
raise RuntimeError("Unknown step %s" % step)
class TestResult(object):
'''test result class'''
"""Test result class."""
def __init__(self, name, result, elapsed):
self.name = name
self.result = result
self.elapsed = "%.1f" % elapsed
class TestFile(object):
'''test result file'''
"""Test result file."""
def __init__(self, name, fname):
self.name = name
self.fname = fname
class TestResults(object):
'''test results class'''
"""Test results class."""
def __init__(self):
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.files = []
self.images = []
def add(self, name, result, elapsed):
'''add a result'''
"""Add a result."""
self.tests.append(TestResult(name, result, elapsed))
def addfile(self, name, fname):
'''add a result file'''
"""Add a result file."""
self.files.append(TestFile(name, fname))
def addimage(self, name, fname):
'''add a result image'''
"""Add a result image."""
self.images.append(TestFile(name, fname))
def addglob(self, name, pattern):
'''add a set of files'''
"""Add a set of files."""
import glob
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
self.addfile(name, os.path.basename(f))
def addglobimage(self, name, pattern):
'''add a set of images'''
"""Add a set of images."""
import glob
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
self.addimage(name, os.path.basename(f))
def write_webresults(results):
'''write webpage results'''
def write_webresults(results_to_write):
"""Write webpage results."""
from pymavlink.generator import mavtemplate
t = mavtemplate.MAVTemplate()
for h in glob.glob(util.reltopdir('Tools/autotest/web/*.html')):
html = util.loadfile(h)
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()
for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')):
shutil.copy(f, util.reltopdir('../buildlogs/%s' % os.path.basename(f)))
def write_fullresults():
'''write out full results set'''
"""Write out full results set."""
global results
results.addglob("Google Earth track", '*.kmz')
results.addfile('Full Logs', 'autotest-output.txt')
@ -406,8 +431,9 @@ def write_fullresults():
results = TestResults()
def check_logs(step):
'''check for log files from a step'''
"""Check for log files from a step."""
print("check step: ", step)
if step.startswith('fly.'):
vehicle = step[4:]
@ -427,10 +453,11 @@ def check_logs(step):
newname = util.reltopdir("../buildlogs/%s.core" % vehicle)
print("Renaming %s to %s" % (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):
'''run a list of steps'''
"""Run a list of steps."""
global results
passed = True
@ -449,7 +476,7 @@ def run_tests(steps):
failed.append(step)
results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1)
continue
except Exception, msg:
except Exception as msg:
passed = False
failed.append(step)
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 pysim import util
# a list of pexpect objects to read while waiting for
# messages. This keeps the output to stdout flowing
expect_list = []
def expect_list_clear():
'''clear the expect list'''
"""clear the expect list."""
global expect_list
for p in expect_list[:]:
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
expect_list.extend(list)
expect_list.extend(list_to_add)
def idle_hook(mav):
'''called when waiting for a mavlink message'''
"""Called when waiting for a mavlink message."""
global expect_list
for p in expect_list:
util.pexpect_drain(p)
def message_hook(mav, msg):
'''called as each mavlink msg is received'''
"""Called as each mavlink msg is received."""
idle_hook(mav)
def expect_callback(e):
'''called when waiting for a expect pattern'''
"""Called when waiting for a expect pattern."""
global expect_list
for p in expect_list:
if p == e:
continue
util.pexpect_drain(p)
def get_distance(loc1, loc2):
'''get ground distance between two locations'''
dlat = loc2.lat - loc1.lat
dlong = loc2.lng - loc1.lng
"""Get ground distance between two locations."""
dlat = loc2.lat - loc1.lat
dlong = loc2.lng - loc1.lng
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
def get_bearing(loc1, loc2):
'''get bearing from loc1 to loc2'''
"""Get bearing from loc1 to loc2."""
off_x = loc2.lng - loc1.lng
off_y = loc2.lat - loc1.lat
bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
if bearing < 0:
bearing += 360.00
return bearing;
return bearing
def wait_seconds(mav, seconds_to_wait):
tstart = get_sim_time(mav)
@ -55,29 +67,33 @@ def wait_seconds(mav, seconds_to_wait):
while tstart + seconds_to_wait > tnow:
tnow = get_sim_time(mav)
def get_sim_time(mav):
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
return m.time_boot_ms * 1.0e-3
def wait_altitude(mav, alt_min, alt_max, timeout=30):
"""Wait for a given altitude range."""
climb_rate = 0
previous_alt = 0
'''wait for a given altitude range'''
tstart = get_sim_time(mav)
print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
while get_sim_time(mav) < tstart + timeout:
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
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:
print("Altitude OK")
return True
print("Failed to attain altitude range")
return False
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)
print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
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):
'''wait for a given roll in degrees'''
"""Wait for a given roll in degrees."""
tstart = get_sim_time(mav)
print("Waiting for roll of %d at %s" % (roll, time.ctime()))
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)
return False
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)
print("Waiting for pitch of %u at %s" % (pitch, time.ctime()))
while get_sim_time(mav) < tstart + timeout:
@ -120,9 +137,8 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
return False
def wait_heading(mav, heading, accuracy=5, timeout=30):
'''wait for a given heading'''
"""Wait for a given heading."""
tstart = get_sim_time(mav)
print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
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):
'''wait for flight of a given distance'''
"""Wait for flight of a given distance."""
tstart = get_sim_time(mav)
start = mav.location()
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):
'''wait for arrival at a location'''
"""Wait for arrival at a location."""
tstart = get_sim_time(mav)
if target_altitude is None:
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")
return False
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)
# this message arrives after we set the current WP
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
# the wp_dist check is a hack until we can sort out the right seqnum
# 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):
print("Reached final waypoint %u" % seq)
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))
return False
def save_wp(mavproxy, mav):
mavproxy.send('rc 7 1000\n')
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)
wait_seconds(mav, 1)
def wait_mode(mav, mode, timeout=None):
print("Waiting for mode %s" % mode)
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
print("Got mode %s" % mode)
return mav.flightmode
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.load(filename)
num_wp = wploader.count()
return num_wp
def sim_location(mav):
'''return current simulator location'''
"""Return current simulator location."""
from pymavlink import mavutil
m = mav.recv_match(type='SIMSTATE', blocking=True)
return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
def log_download(mavproxy, mav, filename, timeout=360):
'''download latest log'''
"""Download latest log."""
mavproxy.send("log list\n")
mavproxy.expect("numLogs")
mav.wait_heartbeat()
@ -258,4 +280,3 @@ def log_download(mavproxy, mav, filename, timeout=360):
mav.wait_heartbeat()
mav.wait_heartbeat()
return True

25
Tools/autotest/dump_logs.py
View File

@ -1,12 +1,15 @@
#!/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, fnmatch, time, glob, traceback, signal
import optparse
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 #############
parser = optparse.OptionParser(sys.argv[0])
@ -17,13 +20,14 @@ opts, args = parser.parse_args()
os.environ['PYTHONUNBUFFERED'] = '1'
def dump_logs(atype):
'''dump DataFlash logs'''
"""Dump DataFlash logs."""
logfile = '%s.log' % atype
print("Dumping logs for %s to %s" % (atype, logfile))
sil = util.start_SIL(atype)
sitl = util.start_SITL(atype)
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')
print("navigating menus")
mavproxy.expect(']')
@ -48,11 +52,10 @@ def dump_logs(atype):
mavproxy.expect("logs enabled:", timeout=120)
mavproxy.expect("Log]")
util.pexpect_close(mavproxy)
util.pexpect_close(sil)
util.pexpect_close(sitl)
log.close()
print("Saved log for %s to %s" % (atype, logfile))
return True
vehicle = os.path.basename(os.getcwd())
dump_logs(vehicle)

42
Tools/autotest/fakepos.py
View File

@ -1,10 +1,15 @@
#!/usr/bin/env python
import socket, struct, time, errno
from math import *
import errno
import socket
import struct
import sys
import time
from math import cos, fabs, radians, sin, sqrt
class udp_out(object):
'''a UDP output socket'''
"""A UDP output socket."""
def __init__(self, device):
a = device.split(':')
if len(a) != 2:
@ -15,11 +20,11 @@ class udp_out(object):
self.port.setblocking(0)
self.last_address = None
def recv(self,n=None):
def recv(self, n=None):
try:
data, self.last_address = self.port.recvfrom(300)
except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
return ""
raise
return data
@ -34,12 +39,15 @@ class udp_out(object):
def ft2m(x):
return x * 0.3048
def m2ft(x):
return x / 0.3048
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
@ -81,19 +89,19 @@ while True:
xAccel = sin(radians(pitchDeg))
yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg))
zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg))
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
xAccel *= scale;
yAccel *= scale;
zAccel *= scale;
scale = 9.81 / sqrt((xAccel*xAccel) + (yAccel*yAccel) + (zAccel*zAccel))
xAccel *= scale
yAccel *= scale
zAccel *= scale
buf = struct.pack('<17dI',
latitude, longitude, altitude, heading,
speedN, speedE, speedD,
xAccel, yAccel, zAccel,
rollRate, pitchRate, yawRate,
rollDeg, pitchDeg, yawDeg,
airspeed, magic)
udp.write(buf)
struc_buf = struct.pack('<17dI',
latitude, longitude, altitude, heading,
speedN, speedE, speedD,
xAccel, yAccel, zAccel,
rollRate, pitchRate, yawRate,
rollDeg, pitchDeg, yawDeg,
airspeed, magic)
udp.write(struc_buf)
time.sleep(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
# run a jsbsim model as a child process
"""
Run a jsbsim model as a child process.
"""
import sys, os, pexpect, socket
import math, time, select, struct, signal, errno
import atexit
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 util, atexit, fdpexpect
import pexpect
from pymavlink import fgFDM
from .. pysim import util
class control_state(object):
def __init__(self):
self.aileron = 0
@ -21,18 +33,20 @@ sitl_state = control_state()
def interpret_address(addrstr):
'''interpret a IP:port string'''
"""Interpret a IP:port string."""
a = addrstr.split(':')
a[1] = int(a[1])
return tuple(a)
def jsb_set(variable, value):
'''set a JSBSim variable'''
"""Set a JSBSim variable."""
global jsb_console
jsb_console.send('set %s %s\r\n' % (variable, value))
def setup_template(home):
'''setup aircraft/Rascal/reset.xml'''
"""Setup aircraft/Rascal/reset.xml ."""
global opts
v = home.split(',')
if len(v) != 4:
@ -45,50 +59,50 @@ def setup_template(home):
sitl_state.ground_height = altitude
template = os.path.join('aircraft', 'Rascal', 'reset_template.xml')
reset = os.path.join('aircraft', 'Rascal', 'reset.xml')
xml = open(template).read() % { 'LATITUDE' : str(latitude),
'LONGITUDE' : str(longitude),
'HEADING' : str(heading) }
xml = open(template).read() % {'LATITUDE': str(latitude),
'LONGITUDE': str(longitude),
'HEADING': str(heading)}
open(reset, mode='w').write(xml)
print("Wrote %s" % reset)
baseport = int(opts.simout.split(':')[1])
template = os.path.join('jsb_sim', 'fgout_template.xml')
out = os.path.join('jsb_sim', 'fgout.xml')
xml = open(template).read() % { 'FGOUTPORT' : str(baseport+3) }
out = os.path.join('jsb_sim', 'fgout.xml')
xml = open(template).read() % {'FGOUTPORT': str(baseport+3)}
open(out, mode='w').write(xml)
print("Wrote %s" % out)
template = os.path.join('jsb_sim', 'rascal_test_template.xml')
out = os.path.join('jsb_sim', 'rascal_test.xml')
xml = open(template).read() % { 'JSBCONSOLEPORT' : str(baseport+4) }
out = os.path.join('jsb_sim', 'rascal_test.xml')
xml = open(template).read() % {'JSBCONSOLEPORT': str(baseport+4)}
open(out, mode='w').write(xml)
print("Wrote %s" % out)
def process_sitl_input(buf):
'''process control changes from SITL sim'''
"""Process control changes from SITL sim."""
control = list(struct.unpack('<14H', buf))
pwm = control[:11]
(speed, direction, turbulance) = control[11:]
global wind
wind.speed = speed*0.01
wind.direction = direction*0.01
wind.speed = speed*0.01
wind.direction = direction*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
throttle = (pwm[2]-1000)/1000.0
if opts.revthr:
throttle = 1.0 - throttle
rudder = (pwm[3]-1500)/500.0
rudder = (pwm[3]-1500)/500.0
if opts.elevon:
# fake an elevon plane
ch1 = aileron
ch2 = elevator
aileron = (ch2-ch1)/2.0
aileron = (ch2-ch1)/2.0
# the minus does away with the need for RC2_REV=-1
elevator = -(ch2+ch1)/2.0
@ -98,7 +112,7 @@ def process_sitl_input(buf):
ch2 = rudder
# this matches VTAIL_OUTPUT==2
elevator = (ch2-ch1)/2.0
rudder = (ch2+ch1)/2.0
rudder = (ch2+ch1)/2.0
buf = ''
if aileron != sitl_state.aileron:
@ -117,14 +131,16 @@ def process_sitl_input(buf):
global jsb_console
jsb_console.send(buf)
def update_wind(wind):
'''update wind simulation'''
"""Update wind simulation."""
(speed, direction) = wind.current()
jsb_set('atmosphere/psiw-rad', math.radians(direction))
jsb_set('atmosphere/wind-mag-fps', speed/0.3048)
def process_jsb_input(buf, simtime):
'''process FG FDM input from JSBSim'''
"""Process FG FDM input from JSBSim."""
global fdm, fg_out, sim_out
fdm.parse(buf)
if fg_out:
@ -134,7 +150,7 @@ def process_jsb_input(buf, simtime):
fdm.set('rpm', sitl_state.throttle*1000)
fg_out.send(fdm.pack())
except socket.error as e:
if e.errno not in [ errno.ECONNREFUSED ]:
if e.errno not in [errno.ECONNREFUSED]:
raise
timestamp = int(simtime*1.0e6)
@ -162,13 +178,11 @@ def process_jsb_input(buf, simtime):
try:
sim_out.send(simbuf)
except socket.error as e:
if e.errno not in [ errno.ECONNREFUSED ]:
if e.errno not in [errno.ECONNREFUSED]:
raise
##################
# main program
################## main program ##################
from optparse import OptionParser
parser = OptionParser("runsim.py [options]")
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()
for m in [ 'home', 'script' ]:
for m in ['home', 'script']:
if not opts.__dict__[m]:
print("Missing required option '%s'" % m)
parser.print_help()
@ -233,7 +247,7 @@ jsb_in.setblocking(0)
# socket addresses
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
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('resume\n')
jsb.expect(["trim computation time","Trim Results"])
jsb.expect(["trim computation time", "Trim Results"])
time.sleep(1.5)
jsb_console.send('step\n')
jsb_console.logfile = None
print("Simulator ready to fly")
def main_loop():
'''run main loop'''
"""Run main loop."""
tnow = time.time()
last_report = tnow
last_sim_input = tnow
@ -348,8 +363,9 @@ def main_loop():
last_wall_time = now
def exit_handler():
'''exit the sim'''
"""Exit the sim."""
print("running exit handler")
signal.signal(signal.SIGINT, 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
"""
The standard interface emitters must implement
"""
import re
from param import *
# The standard interface emitters must implement
class Emit:
def __init__(self):
pass
prog_values_field = re.compile(r"\s*(-?\w+:\w+)+,*")
def close(self):
def close(self):
pass
def start_libraries(self):
pass
pass
def emit(self, g, f):
pass
pass
def set_annotate_with_vehicle(self, value):
self.annotate_with_vehicle = value

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

@ -1,17 +1,20 @@
#!/usr/bin/env python
"""
Emit docs in a form acceptable to the Ardupilot wordpress docs site
"""
import re
from param import *
from param import known_param_fields
from emit import Emit
import cgi
# Emit docs in a form acceptable to the APM wordpress docs site
class HtmlEmit(Emit):
def __init__(self):
Emit.__init__(self)
html_fname = 'Parameters.html'
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
DO NOT EDIT
@ -26,7 +29,7 @@ DO NOT EDIT
<!-- add auto-generated table of contents with "Table of Contents Plus" plugin -->
[toc exclude="Complete Parameter List"]
'''
"""
self.t = ''
def escape(self, s):
@ -40,25 +43,25 @@ DO NOT EDIT
self.f.write(self.preamble)
self.f.write(self.t)
self.f.close()
def start_libraries(self):
pass
def emit(self, g, f):
def emit(self, g, f):
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:
if not hasattr(param, 'DisplayName') or not hasattr(param, 'Description'):
continue
d = param.__dict__
tag = '%s (%s)' % (param.DisplayName, param.name)
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 += "\n\n<p>%s</p>\n" % cgi.escape(param.Description)
t += "<ul>\n"
for field in param.__dict__.keys():
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]):
@ -72,4 +75,3 @@ DO NOT EDIT
t += "<li>%s: %s</li>\n" % (field, cgi.escape(param.__dict__[field]))
t += "</ul>\n"
self.t += t

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

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

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

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

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

@ -1,27 +1,29 @@
#!/usr/bin/env python
import re
from param import *
from param import known_param_fields
from emit import Emit
import cgi
# Emit docs in a RST format
class RSTEmit(Emit):
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.
'''
"""
def toolname(self):
return "Tools/autotest/param_metadata/param_parse.py"
def __init__(self):
Emit.__init__(self)
output_fname = 'Parameters.rst'
self.f = open(output_fname, mode='w')
self.spacer = re.compile("^", re.MULTILINE)
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}
.. DO NOT EDIT
@ -34,7 +36,7 @@ Complete Parameter List
{blurb}
'''.format(blurb=self.escape(self.blurb()),
""".format(blurb=self.escape(self.blurb()),
toolname=self.escape(self.toolname()))
self.t = ''
@ -47,7 +49,6 @@ Complete Parameter List
self.f.write(self.t)
self.f.close()
def start_libraries(self):
pass
@ -55,34 +56,34 @@ Complete Parameter List
ret = ""
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:
row_line.extend([""] * (height-len(row_line)))
row_line.extend([""] * (height - len(row_line)))
if rowheading is not None:
rowheading_lines = rowheading.split("\n")
rowheading_lines.extend([""] * (height-len(rowheading_lines)))
rowheading_lines.extend([""] * (height - len(rowheading_lines)))
out_lines = []
for i in range(0,height):
for i in range(0, height):
out_line = ""
if rowheading is not None:
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 = "#"
j=0
j = 0
for item in row_lines:
widthnum = j
if rowheading is not None:
widthnum += 1
line = item[i]
out_line += joiner + " " + line + " "*(widths[widthnum]-len(line)-1)
out_line += joiner + " " + line + " " * (widths[widthnum] - len(line) - 1)
joiner = "|"
j += 1
out_line += "|"
out_lines.append(out_line)
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[:]
if headings is not None:
check_width_rows.append(headings)
@ -109,7 +110,7 @@ Complete Parameter List
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
all_rowheadings = []
@ -118,7 +119,7 @@ Complete Parameter List
all_rowheadings.append("")
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]
values_to_check = []
if rowheadings is not None:
@ -130,15 +131,15 @@ Complete Parameter List
if height > heights[rownum]:
heights[rownum] = height
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]:
widths[colnum] = width
colnum += 1
return (widths,heights)
return (widths, heights)
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
bar = ""
@ -159,7 +160,7 @@ Complete Parameter List
rowheading = ""
ret += self.tablify_row(rowheading, headings, widths, heights[0]) + "\n"
ret += heading_bar + "\n"
for i in range(0,len(rows)):
for i in range(0, len(rows)):
rowheading = None
height = i
if rowheadings is not None:
@ -171,13 +172,11 @@ Complete Parameter List
return ret
def render_prog_values_field(self, render_info, param, field):
values = (param.__dict__[field]).split(',')
rows = []
for value in values:
v = [ x.strip() for x in value.split(':') ]
v = [x.strip() for x in value.split(':')]
rows.append(v)
return self.tablify(rows, headings=render_info["headings"])
@ -187,20 +186,20 @@ Complete Parameter List
field_table_info = {
"Values": {
"headings": ['Value','Meaning']
"headings": ['Value', 'Meaning'],
},
"Bitmask": {
"headings": ['Bit', 'Meaning']
}
"headings": ['Bit', 'Meaning'],
},
}
ret = '''
ret = """
.. _{reference}:
{tag}
{underline}
'''.format(tag=tag,underline="-" * len(tag),
""".format(tag=tag, underline="-" * len(tag),
reference=reference)
for param in g.params:
@ -211,7 +210,7 @@ Complete Parameter List
name = param.name
else:
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()
reference = param.name
# remove e.g. "ArduPlane:" from start of parameter name:
@ -220,15 +219,15 @@ Complete Parameter List
else:
reference = reference.split(":")[-1]
ret += '''
ret += """
.. _{reference}:
{tag}
{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\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]):
row.append(self.render_prog_values_field(field_table_info[field], param, field))
elif field == "Range":
(min,max) = (param.__dict__[field]).split(' ')
row.append("%s - %s" % (min,max,))
(param_min, param_max) = (param.__dict__[field]).split(' ')
row.append("%s - %s" % (param_min, param_max,))
else:
row.append(cgi.escape(param.__dict__[field]))
if len(row):
ret += "\n\n" + self.tablify([row], headings=headings) + "\n\n"
self.t += ret + "\n"
def table_test():
e = RSTEmit()
print("Test 1")
print e.tablify([["A","B"],["C","D"]])
print(e.tablify([["A", "B"], ["C", "D"]]))
print("Test 2")
print e.tablify([["A","B"],["CD\nE","FG"]])
print e.tablify([["A", "B"], ["CD\nE", "FG"]])
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 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 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__':
table_test()

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

@ -1,13 +1,15 @@
#!/usr/bin/env python
import re
from param import *
from emit import Emit
from param import known_param_fields
# Emit docs in a form acceptable to the APM wiki site
class WikiEmit(Emit):
def __init__(self):
Emit.__init__(self)
wiki_fname = 'Parameters.wiki'
self.f = open(wiki_fname, mode='w')
preamble = '''#summary Dynamically generated list of documented parameters
@ -17,56 +19,52 @@ class WikiEmit(Emit):
= Vehicles =
'''
self.f.write(preamble)
def close(self):
self.f.close
def close(self):
self.f.close()
def camelcase_escape(self, word):
if re.match(r"([A-Z][a-z]+[A-Z][a-z]*)", word.strip()):
return "!"+word
return "!" + word
else:
return word
def wikichars_escape(self, text):
for c in "*,{,},[,],_,=,#,^,~,!,@,$,|,<,>,&,|,\,/".split(','):
text = re.sub("\\"+c, '`'+c+'`', text)
text = re.sub("\\" + c, '`' + c + '`', text)
return text
def emit_comment(self, s):
self.f.write("\n\n=" + s + "=\n\n")
def start_libraries(self):
self.emit_comment("Libraries")
def emit(self, g, f):
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
def emit(self, g, f):
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
for param in g.params:
if hasattr(param, 'DisplayName'):
t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName),self.camelcase_escape(param.name))
if hasattr(param, 'DisplayName'):
t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName), self.camelcase_escape(param.name))
else:
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)
else:
t += "\n\n_TODO: description_\n"
for field in param.__dict__.keys():
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]):
t+= " * Values \n"
t += " * Values \n"
values = (param.__dict__[field]).split(',')
t+="|| *Value* || *Meaning* ||\n"
t += "|| *Value* || *Meaning* ||\n"
for value in values:
v = value.split(':')
t+="|| "+v[0]+" || "+self.camelcase_escape(v[1])+" ||\n"
t += "|| " + v[0] + " || " + self.camelcase_escape(v[1]) + " ||\n"
else:
t += " * %s: %s\n" % (self.camelcase_escape(field), self.wikichars_escape(param.__dict__[field]))
#print t
# print 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 param import *
from emit import Emit
from param import known_param_fields
# Emit APM documentation in an machine readable XML format
class XmlEmit(Emit):
def __init__(self):
Emit.__init__(self)
wiki_fname = 'apm.pdef.xml'
self.f = open(wiki_fname, mode='w')
preamble = '''<?xml version="1.0" encoding="utf-8"?>
@ -17,56 +18,53 @@ class XmlEmit(Emit):
<vehicles>
'''
self.f.write(preamble)
def close(self):
def close(self):
self.f.write('</libraries>')
self.f.write('''</paramfile>\n''')
self.f.close
self.f.close()
def emit_comment(self, s):
self.f.write("<!-- " + s + " -->")
def start_libraries(self):
self.f.write('</vehicles>')
self.f.write('<libraries>')
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:
# Begin our parameter node
if hasattr(param, 'DisplayName'):
t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName),quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM)
if hasattr(param, 'DisplayName'):
t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName), quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM)
else:
t += '<param name=%s' % quoteattr(param.name)
if hasattr(param, 'Description'):
t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs
if hasattr(param, 'Description'):
t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs
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"
# Add values as chidren of this node
for field in param.__dict__.keys():
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]):
t+= "<values>\n"
t += "<values>\n"
values = (param.__dict__[field]).split(',')
for value in values:
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"
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 += '''</parameters>\n'''
#print t
# print 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 time
import util
from rotmat import Vector3, Matrix3
class Aircraft(object):
'''a basic aircraft class'''
"""A basic aircraft class."""
def __init__(self):
self.home_latitude = 0
self.home_longitude = 0
@ -11,20 +15,20 @@ class Aircraft(object):
self.ground_level = 0
self.frame_height = 0.0
self.latitude = self.home_latitude
self.latitude = self.home_latitude
self.longitude = self.home_longitude
self.altitude = self.home_altitude
self.altitude = self.home_altitude
self.dcm = Matrix3()
# 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.position = Vector3(0, 0, 0) # m 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.mass = 0.0
self.update_frequency = 50 # in Hz
self.gravity = 9.80665 # m/s/s
self.update_frequency = 50 # in Hz
self.gravity = 9.80665 # m/s/s
self.accelerometer = Vector3(0, 0, -self.gravity)
self.wind = util.Wind('0,0,0')
@ -35,13 +39,13 @@ class Aircraft(object):
self.accel_noise = 0.3
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:
position = self.position
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
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))
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,
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
self.accelerometer = self.accel_body.copy()
def set_yaw_degrees(self, yaw_degrees):
'''rotate to the given yaw'''
"""Rotate to the given yaw."""
(roll, pitch, yaw) = self.dcm.to_euler()
yaw = math.radians(yaw_degrees)
self.dcm.from_euler(roll, pitch, yaw)
def time_advance(self, deltat):
'''advance time by deltat in seconds'''
"""Advance time by deltat in seconds."""
self.time_now += deltat
def setup_frame_time(self, rate, speedup):
'''setup frame_time calculation'''
"""Setup frame_time calculation."""
self.rate = rate
self.speedup = speedup
self.frame_time = 1.0/rate
@ -75,14 +79,14 @@ class Aircraft(object):
self.achieved_rate = rate
def adjust_frame_time(self, rate):
'''adjust frame_time calculation'''
"""Adjust frame_time calculation."""
self.rate = rate
self.frame_time = 1.0/rate
self.scaled_frame_time = self.frame_time/self.speedup
def sync_frame_time(self):
'''try to synchronise simulation time with wall clock time, taking
into account desired speedup'''
"""Try to synchronise simulation time with wall clock time, taking
into account desired speedup."""
now = time.time()
if now < self.last_wall_time + self.scaled_frame_time:
time.sleep(self.last_wall_time+self.scaled_frame_time - now)
@ -99,13 +103,10 @@ class Aircraft(object):
self.last_wall_time = now
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),
random.gauss(0, 1),
random.gauss(0, 1)) * throttle * self.gyro_noise
self.accel_body += Vector3(random.gauss(0, 1),
random.gauss(0, 1),
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 $
"""
from pexpect import *
import os
from pexpect import ExceptionPexpect, spawn
__all__ = ['fdspawn']
class fdspawn (spawn):
class fdspawn(spawn):
"""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
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
fileno() method (returning an int). All Python file-like objects
support fileno(). """
### 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: Add better handling of trying to use fdspawn in place of spawn
# 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()
if type(fd) != type(0):
raise ExceptionPexpect ('The fd argument is not an int. If this is a command string then maybe you want to use pexpect.spawn.')
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.")
try: # make sure fd is a valid file descriptor
try: # make sure fd is a valid file descriptor
os.fstat(fd)
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.command = None
@ -43,23 +45,23 @@ class fdspawn (spawn):
self.closed = False
self.name = '<file descriptor %d>' % fd
def __del__ (self):
def __del__(self):
return
def close (self):
def close(self):
if self.child_fd == -1:
return
if self.own_fd:
self.close (self)
self.close(self)
else:
self.flush()
os.close(self.child_fd)
self.child_fd = -1
self.closed = True
def isalive (self):
def isalive(self):
"""This checks if the file descriptor is still valid. If os.fstat()
does not raise an exception then we assume it is alive. """
@ -72,11 +74,10 @@ class fdspawn (spawn):
except:
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

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

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

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

@ -1,23 +1,23 @@
#!/usr/bin/env python
'''
"""
Python interface to euroc ROS multirotor simulator
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
import util, time, math
from math import degrees, radians
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):
'''convert quaternion to DCM'''
"""Convert quaternion to DCM."""
q3q3 = q3 * q3
q3q4 = q3 * q4
q2q2 = q2 * q2
@ -33,7 +33,7 @@ def quat_to_dcm(q1, q2, q3, q4):
m.a.y = 2.0*(q2q3 - q1q4)
m.a.z = 2.0*(q2q4 + q1q3)
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.c.x = 2.0*(q2q4 - q1q3)
m.c.y = 2.0*(q3q4 + q1q2)
@ -42,7 +42,7 @@ def quat_to_dcm(q1, q2, q3, q4):
class IrisRos(Aircraft):
'''a IRIS MultiCopter from ROS'''
"""A IRIS MultiCopter from ROS."""
def __init__(self):
Aircraft.__init__(self)
self.max_rpm = 1200
@ -51,11 +51,11 @@ class IrisRos(Aircraft):
self.have_new_pos = False
topics = {
"/clock" : (self.clock_cb, rosgraph_msgs.Clock),
"/iris/imu" : (self.imu_cb, sensor_msgs.Imu),
"/clock" : (self.clock_cb, rosgraph_msgs.Clock),
"/iris/imu" : (self.imu_cb, sensor_msgs.Imu),
"/iris/vehicle_local_position" : (self.pos_cb, px4.vehicle_local_position),
}
rospy.init_node('ArduPilot', anonymous=True)
for topic in topics.keys():
(callback, msgtype) = topics[topic]
@ -65,9 +65,9 @@ class IrisRos(Aircraft):
mav_msgs.CommandMotorSpeed,
queue_size=1)
self.last_time = 0
# spin() simply keeps python from exiting until this node is stopped
#rospy.spin()
# rospy.spin()
def clock_cb(self, msg):
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.z)
self.have_new_imu = True
def pos_cb(self, msg):
self.velocity = Vector3(msg.vx, msg.vy, msg.vz)
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
#
# vector3 and rotation matrix classes
# This follows the conventions in the ArduPilot code,
# 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
"""
vector3 and rotation matrix classes
This follows the conventions in the ArduPilot code,
and is essentially a python version of the AP_Math library
'''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:
'''a vector'''
"""A vector."""
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.y = float(y)
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.y = float(x[1])
self.z = float(x[2])
elif x != None:
elif x is not None:
raise ValueError('bad initialiser')
else:
self.x = float(0)
@ -70,8 +71,8 @@ class Vector3:
def __mul__(self, v):
if isinstance(v, Vector3):
'''dot product'''
return self.x*v.x + self.y*v.y + self.z*v.z
"""dot product"""
return self.x * v.x + self.y * v.y + self.z * v.z
return Vector3(self.x * v,
self.y * v,
self.z * v)
@ -84,10 +85,10 @@ class Vector3:
self.z / v)
def __mod__(self, v):
'''cross product'''
return Vector3(self.y*v.z - self.z*v.y,
self.z*v.x - self.x*v.z,
self.x*v.y - self.y*v.x)
"""Cross product."""
return Vector3(self.y * v.z - self.z * v.y,
self.z * v.x - self.x * v.z,
self.x * v.y - self.y * v.x)
def __copy__(self):
return Vector3(self.x, self.y, self.z)
@ -95,13 +96,13 @@ class Vector3:
copy = __copy__
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):
self.x = self.y = self.z = 0
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())
def normalized(self):
@ -113,8 +114,10 @@ class Vector3:
self.y = v.y
self.z = v.z
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):
if a is not None and b is not None and c is not None:
self.a = a.copy()
@ -130,18 +133,17 @@ class Matrix3:
self.c.x, self.c.y, self.c.z)
def identity(self):
self.a = Vector3(1,0,0)
self.b = Vector3(0,1,0)
self.c = Vector3(0,0,1)
self.a = Vector3(1, 0, 0)
self.b = Vector3(0, 1, 0)
self.c = Vector3(0, 0, 1)
def transposed(self):
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.z, self.b.z, self.c.z))
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)
sp = sin(pitch)
sr = sin(roll)
@ -159,9 +161,8 @@ class Matrix3:
self.c.y = sr * cp
self.c.z = cr * cp
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:
pitch = pi
elif self.c.x <= -1.0:
@ -169,14 +170,13 @@ class Matrix3:
else:
pitch = -asin(self.c.x)
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)
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
'''
"""
T21 = self.a.y
T22 = self.b.y
T23 = self.c.y
@ -188,7 +188,7 @@ class Matrix3:
return (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)
s3 = sin(pitch)
s2 = sin(roll)
@ -199,11 +199,11 @@ class Matrix3:
self.a.x = c1 * c3 - s1 * s2 * s3
self.b.y = c1 * c2
self.c.z = c3 * c2
self.a.y = -c2*s1
self.a.z = s3*c1 + c3*s2*s1
self.b.x = c3*s1 + s3*s2*c1
self.b.z = s1*s3 - s2*c1*c3
self.c.x = -s3*c2
self.a.y = -c2 * s1
self.a.z = s3 * c1 + c3 * s2 * s1
self.b.x = c3 * s1 + s3 * s2 * c1
self.b.z = s1 * s3 - s2 * c1 * c3
self.c.x = -s3 * c2
self.c.y = s2
def __add__(self, m):
@ -249,7 +249,7 @@ class Matrix3:
copy = __copy__
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()
a = self.a
b = self.b
@ -268,7 +268,7 @@ class Matrix3:
self.c += temp_matrix.c
def normalize(self):
'''re-normalise a rotation matrix'''
"""Re-normalise a rotation matrix."""
error = self.a * self.b
t0 = self.a - (self.b * (0.5 * error))
t1 = self.b - (self.a * (0.5 * error))
@ -278,11 +278,12 @@ class Matrix3:
self.c = t2 * (1.0 / t2.length())
def trace(self):
'''the trace of the matrix'''
"""The trace of the matrix."""
return self.a.x + self.b.y + self.c.z
def test_euler():
'''check that from_euler() and to_euler() are consistent'''
"""Check that from_euler() and to_euler() are consistent."""
m = Matrix3()
from math import radians, degrees
for r in range(-179, 179, 3):
@ -290,38 +291,41 @@ def test_euler():
for y in range(-179, 179, 3):
m.from_euler(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler()
v1 = Vector3(r,p,y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2
if diff.length() > 1.0e-12:
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
m = Matrix3()
m.from_euler312(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler312()
v1 = Vector3(r,p,y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2
if diff.length() > 1.0e-12:
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
m = Matrix3()
m.from_euler312(radians(r), radians(p), radians(y))
(r2, p2, y2) = m.to_euler()
v1 = Vector3(r,p,y)
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
v1 = Vector3(r, p, y)
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
diff = v1 - v2
if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length())
def test_euler312():
'''check that from_euler312() and to_euler312() are consistent'''
"""Check that from_euler312() and to_euler312() are consistent."""
m = Matrix3()
for x in range(-89, 89, 3):
@ -331,10 +335,12 @@ def test_euler312():
for r in range(-89, 89, 3):
for p 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__":
import doctest
doctest.testmod()
test_euler()
test_euler312()

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

@ -1,13 +1,16 @@
#!/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
wind = util.Wind('7,90,0.1')
t0 = time.time()
velocity = Vector3(0,0,0)
velocity = Vector3(0, 0, 0)
t = 0
deltat = 0.01

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

@ -1,82 +1,100 @@
import math
from math import sqrt, acos, cos, pi, sin, atan2
import os, sys, time, random
from rotmat import Vector3, Matrix3
from subprocess import call, check_call,Popen, PIPE
import os
import random
import re
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):
'''meters to feet'''
"""Meters to feet."""
return float(x) / 0.3048
def ft2m(x):
'''feet to meters'''
"""Feet to meters."""
return float(x) * 0.3048
def kt2mps(x):
return x * 0.514444444
def mps2kt(x):
return x / 0.514444444
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__))
assert(os.path.basename(d)=='pysim')
assert(os.path.basename(d) == 'pysim')
d = os.path.dirname(d)
assert(os.path.basename(d)=='autotest')
assert(os.path.basename(d) == 'autotest')
d = os.path.dirname(d)
assert(os.path.basename(d)=='Tools')
assert(os.path.basename(d) == 'Tools')
d = os.path.dirname(d)
return d
def reltopdir(path):
'''return a path relative to topdir()'''
"""Return a path relative to topdir()."""
return os.path.normpath(os.path.join(topdir(), path))
def run_cmd(cmd, dir=".", show=True, output=False, checkfail=True):
'''run a shell command'''
def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
"""Run a shell command."""
shell = False
if not isinstance(cmd, list):
cmd = [cmd]
shell = True
if show:
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), dir,))
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
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:
return check_call(cmd, shell=shell, cwd=dir)
return check_call(cmd, shell=shell, cwd=directory)
else:
return call(cmd, shell=shell, cwd=dir)
return call(cmd, shell=shell, cwd=directory)
def rmfile(path):
'''remove a file if it exists'''
"""Remove a file if it exists."""
try:
os.unlink(path)
except Exception:
pass
def deltree(path):
'''delete a tree of files'''
"""Delete a tree of files."""
run_cmd('rm -rf %s' % path)
def relwaf():
return "./modules/waf/waf-light"
def waf_configure(board, j=None, debug=False):
cmd_configure = [relwaf(), "configure", "--board", board]
if debug:
cmd_configure.append('--debug')
if j is not None:
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():
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
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]
if j is not None:
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
def build_examples(board, j=None, debug=False, clean=False):
# first configure
waf_configure(board, j=j, debug=debug)
@ -101,20 +120,22 @@ def build_examples(board, j=None, debug=False, clean=False):
# then build
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
# list of pexpect children to close on exit
close_list = []
def pexpect_autoclose(p):
'''mark for autoclosing'''
"""Mark for autoclosing."""
global close_list
close_list.append(p)
def pexpect_close(p):
'''close a pexpect child'''
"""Close a pexpect child."""
global close_list
try:
@ -128,44 +149,47 @@ def pexpect_close(p):
if p in close_list:
close_list.remove(p)
def pexpect_close_all():
'''close all pexpect children'''
"""Close all pexpect children."""
global close_list
for p in close_list[:]:
pexpect_close(p)
def pexpect_drain(p):
'''drain any pending input'''
"""Drain any pending input."""
import pexpect
try:
p.read_nonblocking(1000, timeout=0)
except pexpect.TIMEOUT:
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):
'''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_.+-])")
text.replace('/','-')
filename = r.sub(lambda m : "%" + str(hex(ord(str(m.group(1))))).upper(), text)
text.replace('/', '-')
filename = r.sub(lambda m: "%" + str(hex(ord(str(m.group(1))))).upper(), text)
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):
self.binary = binary
self.model = model
cmd=[]
cmd = []
if valgrind and os.path.exists('/usr/bin/valgrind'):
cmd.extend(['valgrind', '-q', '--log-file=%s' % self.valgrind_log_filepath()])
if gdb:
f = open("/tmp/x.gdb", "w")
f.write("r\n");
f.write("r\n")
f.close()
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))
first = cmd[0]
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
pexpect_autoclose(self)
# give time for parameters to properly setup
@ -193,24 +217,25 @@ class SIL(pexpect.spawn):
if gdb:
# if we run GDB we do so in an xterm. "Waiting for
# 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)
# TODO: have a SITL-compiled ardupilot able to have its
# console on an output fd.
else:
self.expect('Waiting for connection',timeout=300)
self.expect('Waiting for connection', timeout=300)
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',
options=None, logfile=sys.stdout):
'''launch mavproxy connected to a SIL instance'''
def start_SITL(binary, **kwargs):
"""Launch a SITL 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
global close_list
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):
'''setup a callback that is called once a second while waiting for
patterns'''
"""Setup a callback that is called once a second while waiting for
patterns."""
import pexpect
def _expect_callback(pattern, timeout=e.timeout):
tstart = time.time()
while time.time() < tstart + timeout:
@ -248,27 +274,30 @@ def expect_setup_callback(e, callback):
e.expect_saved = e.expect
e.expect = _expect_callback
def mkdir_p(dir):
'''like mkdir -p'''
if not dir:
def mkdir_p(directory):
"""Like mkdir -p ."""
if not directory:
return
if dir.endswith("/"):
mkdir_p(dir[:-1])
if directory.endswith("/"):
mkdir_p(directory[:-1])
return
if os.path.isdir(dir):
if os.path.isdir(directory):
return
mkdir_p(os.path.dirname(dir))
os.mkdir(dir)
mkdir_p(os.path.dirname(directory))
os.mkdir(directory)
def loadfile(fname):
'''load a file as a string'''
"""Load a file as a string."""
f = open(fname, mode='r')
r = f.read()
f.close()
return r
def lock_file(fname):
'''lock a file'''
"""Lock a file."""
import fcntl
f = open(fname, mode='w')
try:
@ -277,8 +306,9 @@ def lock_file(fname):
return None
return f
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:
try:
parent_pid = os.getppid()
@ -294,13 +324,13 @@ def check_parent(parent_pid=None):
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
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
(phi, theta, psi) = dcm.to_euler()
@ -308,19 +338,20 @@ def EarthRatesToBodyRates(dcm, earth_rates):
thetaDot = earth_rates.y
psiDot = earth_rates.z
p = phiDot - psiDot*sin(theta)
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
p = phiDot - psiDot * sin(theta)
q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta)
r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot
return Vector3(p, q, r)
def BodyRatesToEarthRates(dcm, gyro):
'''convert the angular velocities from body frame to
"""Convert the angular velocities from body frame to
earth frame.
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
p = gyro.x
@ -329,37 +360,38 @@ def BodyRatesToEarthRates(dcm, gyro):
(phi, theta, psi) = dcm.to_euler()
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
thetaDot = q*cos(phi) - r*sin(phi)
phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi))
thetaDot = q * cos(phi) - r * sin(phi)
if fabs(cos(theta)) < 1.0e-20:
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)
radius_of_earth = 6378100.0 # in meters
radius_of_earth = 6378100.0 # in meters
def gps_newpos(lat, lon, bearing, distance):
'''extrapolate latitude/longitude given a heading and distance
thanks to http://www.movable-type.co.uk/scripts/latlong.html
'''
"""Extrapolate latitude/longitude given a heading and distance
thanks to http://www.movable-type.co.uk/scripts/latlong.html .
"""
from math import sin, asin, cos, atan2, radians, degrees
lat1 = radians(lat)
lon1 = radians(lon)
brng = radians(bearing)
dr = distance/radius_of_earth
lat2 = asin(sin(lat1)*cos(dr) +
cos(lat1)*sin(dr)*cos(brng))
lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1),
cos(dr)-sin(lat1)*sin(lat2))
dr = distance / radius_of_earth
lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1) * sin(dr) * cos(brng))
lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
cos(dr) - sin(lat1) * sin(lat2))
return (degrees(lat2), degrees(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
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)
lat2 = math.radians(lat2)
lon1 = math.radians(lon1)
@ -367,35 +399,36 @@ def gps_distance(lat1, lon1, lat2, lon2):
dLat = lat2 - lat1
dLon = lon2 - lon1
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))
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))
return radius_of_earth * c
def gps_bearing(lat1, lon1, lat2, lon2):
'''return bearing between two points in degrees, in range 0-360
thanks to http://www.movable-type.co.uk/scripts/latlong.html'''
"""Return bearing between two points in degrees, in range 0-360
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
lat1 = math.radians(lat1)
lat2 = math.radians(lat2)
lon1 = math.radians(lon1)
lon2 = math.radians(lon2)
dLat = lat2 - lat1
dLon = lon2 - lon1
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))
if bearing < 0:
bearing += 360.0
return bearing
class Wind(object):
'''a wind generation object'''
"""A wind generation object."""
def __init__(self, windstring, cross_section=0.1):
a = windstring.split(',')
if len(a) != 3:
raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring)
self.speed = float(a[0]) # m/s
self.direction = float(a[1]) # direction the wind is going in
self.turbulance= float(a[2]) # turbulance factor (standard deviation)
self.speed = float(a[0]) # m/s
self.direction = float(a[1]) # direction the wind is going in
self.turbulance = float(a[2]) # turbulance factor (standard deviation)
# 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
@ -412,26 +445,24 @@ class Wind(object):
self.turbulance_mul = 1.0
def current(self, deltat=None):
'''return current wind speed and direction as a tuple
speed is in m/s, direction in degrees
'''
"""Return current wind speed and direction as a tuple
speed is in m/s, direction in degrees."""
if deltat is None:
tnow = time.time()
deltat = tnow - self.tlast
self.tlast = tnow
# update turbulance random walk
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 = math.sqrt(deltat) * (1.0 - random.gauss(1.0, self.turbulance))
w_delta -= (self.turbulance_mul - 1.0) * (deltat / self.turbulance_time_constant)
self.turbulance_mul += w_delta
speed = self.speed * math.fabs(self.turbulance_mul)
return (speed, self.direction)
# Calculate drag.
def drag(self, velocity, deltat=None, testing=None):
'''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'''
"""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 ."""
from math import radians
# (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
# the dot product and dividing by the magnitudes.
d = w.length() * obj_speed
if d == 0:
if d == 0:
alpha = 0
else:
else:
alpha = acos((w * velocity) / d)
# 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))
# Combine them to get the acceleration vector.
return Vector3( acc(relWindVec.x, drag_force(self, relWindVec.x))
, acc(relWindVec.y, drag_force(self, relWindVec.y))
, 0 )
return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), 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):
"""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)
x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta
rel_speed = sqrt(x)
@ -483,37 +513,41 @@ def apparent_wind(wind_sp, obj_speed, alpha):
return (rel_speed, beta)
# See http://en.wikipedia.org/wiki/Drag_equation
#
# Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient
# (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
# (to the body), a : area acted on (this is captured by the cross_section
# parameter).
#
# So then we have
# F(a) = 0.2 * 1/2 * v^2 * cross_section = 0.1 * v^2 * cross_section
def drag_force(wind, sp):
def drag_force(wind, sp):
"""See http://en.wikipedia.org/wiki/Drag_equation
Drag equation is F(a) = cl * p/2 * v^2 * a, where cl : drag coefficient
(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
(to the body), a : area acted on (this is captured by the cross_section
parameter).
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
# 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):
""" 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:
return mag
else:
return (val / abs(val)) * (0 - mag)
# Converts a magnitude and angle (radians) to a vector in the xy plane.
def toVec(magnitude, angle):
"""Converts a magnitude and angle (radians) to a vector in the xy plane."""
v = Vector3(magnitude, 0, 0)
m = Matrix3()
m.from_euler(0, 0, angle)
return m.transposed() * v
def constrain(value, minv, maxv):
'''constrain a value to a range'''
"""Constrain a value to a range."""
if value < minv:
value = minv
if value > maxv:
@ -523,4 +557,3 @@ def constrain(value, minv, maxv):
if __name__ == "__main__":
import doctest
doctest.testmod()

50
Tools/autotest/quadplane.py
View File

@ -1,23 +1,27 @@
# fly ArduPlane QuadPlane in SITL
import util, pexpect, sys, time, math, shutil, os
from common import *
import os
import pexpect
import shutil
from pymavlink import mavutil
import random
from common import *
from pysim import util
# 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'
MISSION='ArduPlane-Missions/Dalby-OBC2016.txt'
FENCE='ArduPlane-Missions/Dalby-OBC2016-fence.txt'
WIND="0,180,0.2" # speed,direction,variance
HOME_LOCATION = '-27.274439,151.290064,343,8.7'
MISSION = 'ArduPlane-Missions/Dalby-OBC2016.txt'
FENCE = 'ArduPlane-Missions/Dalby-OBC2016-fence.txt'
WIND = "0,180,0.2" # speed,direction,variance
homeloc = None
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)
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('Flight plan received')
@ -40,23 +44,23 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
return True
def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
'''fly QuadPlane in SIL
def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
"""Fly QuadPlane in SITL.
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
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
if viewerip:
options += " --out=%s:14550" % viewerip
if map:
if use_map:
options += ' --map'
sil = util.start_SIL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb)
mavproxy = util.start_MAVProxy_SIL('QuadPlane', options=options)
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)
mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options)
mavproxy.expect('Telemetry log: (\S+)')
logfile = mavproxy.match.group(1)
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')
expect_list_clear()
expect_list_extend([sil, mavproxy])
expect_list_extend([sitl, mavproxy])
print("Started simulator")
# get a mavlink connection going
try:
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)
raise
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.expect('ARMED')
if not fly_mission(mavproxy, mav,
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016.txt"),
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")):
print("Failed mission")
failed = True
except pexpect.TIMEOUT, e:
except pexpect.TIMEOUT as e:
print("Failed with timeout")
failed = True
mav.close()
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):
os.chmod(valgrind_log, 0644)
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"""
progress("Killing tasks")
try:
victim_names = set([
victim_names = {
'JSBSim',
'lt-JSBSim',
'ArduPlane.elf',
@ -153,9 +153,9 @@ def kill_tasks():
'MAVProxy.exe',
'runsim.py',
'AntennaTracker.elf',
])
}
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
exe_name = os.path.basename(_options_for_frame[frame]["waf_target"])
victim_names.add(exe_name)