From 9e1ffcae5d498650a4ea45ff337a06d2c9b3fd0c Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Sun, 31 Jul 2016 12:22:06 +0200 Subject: [PATCH] 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 --- Tools/PrintVersion.py | 11 +- Tools/autotest/__init__.py | 1 + Tools/autotest/apmrover2.py | 62 ++-- Tools/autotest/arducopter.py | 284 ++++++++++-------- Tools/autotest/arduplane.py | 93 +++--- Tools/autotest/autotest.py | 157 ++++++---- Tools/autotest/common.py | 79 +++-- Tools/autotest/dump_logs.py | 25 +- Tools/autotest/fakepos.py | 42 +-- Tools/autotest/jsb_sim/__init__.py | 0 Tools/autotest/jsb_sim/runsim.py | 90 +++--- Tools/autotest/param_metadata/__init__.py | 0 Tools/autotest/param_metadata/emit.py | 18 +- Tools/autotest/param_metadata/htmlemit.py | 30 +- Tools/autotest/param_metadata/param.py | 17 +- Tools/autotest/param_metadata/param_parse.py | 86 +++--- Tools/autotest/param_metadata/rstemit.py | 81 ++--- Tools/autotest/param_metadata/wikiemit.py | 54 ++-- Tools/autotest/param_metadata/xmlemit.py | 54 ++-- Tools/autotest/pysim/__init__.py | 1 + Tools/autotest/pysim/aircraft.py | 47 +-- Tools/autotest/pysim/fdpexpect.py | 37 +-- Tools/autotest/pysim/fg_display.py | 29 +- Tools/autotest/pysim/iris_ros.py | 40 +-- Tools/autotest/pysim/rotmat.py | 144 ++++----- Tools/autotest/pysim/testwind.py | 9 +- Tools/autotest/pysim/util.py | 299 ++++++++++--------- Tools/autotest/quadplane.py | 50 ++-- Tools/autotest/sim_vehicle.py | 6 +- 29 files changed, 1016 insertions(+), 830 deletions(-) create mode 100644 Tools/autotest/__init__.py create mode 100644 Tools/autotest/jsb_sim/__init__.py create mode 100644 Tools/autotest/param_metadata/__init__.py create mode 100644 Tools/autotest/pysim/__init__.py diff --git a/Tools/PrintVersion.py b/Tools/PrintVersion.py index 4318870399..17b3ea1338 100644 --- a/Tools/PrintVersion.py +++ b/Tools/PrintVersion.py @@ -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()) diff --git a/Tools/autotest/__init__.py b/Tools/autotest/__init__.py new file mode 100644 index 0000000000..80448f7721 --- /dev/null +++ b/Tools/autotest/__init__.py @@ -0,0 +1 @@ +"""Autotests tools suite""" diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index d0d4b6c9b9..6fdd08e66a 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -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")) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index dffe1642db..1e79800d8f 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -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")) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 069fa27bcf..d9eab8cec2 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -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")) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 529cb4ec55..2d81b15d96 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -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', 'FAILED', 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, 'FAILED', 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)) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 35a4b449f2..93c747d899 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -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 - diff --git a/Tools/autotest/dump_logs.py b/Tools/autotest/dump_logs.py index a12fd8a552..44c75d99a2 100755 --- a/Tools/autotest/dump_logs.py +++ b/Tools/autotest/dump_logs.py @@ -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) - diff --git a/Tools/autotest/fakepos.py b/Tools/autotest/fakepos.py index a0801ea30a..9e62661d9b 100755 --- a/Tools/autotest/fakepos.py +++ b/Tools/autotest/fakepos.py @@ -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 diff --git a/Tools/autotest/jsb_sim/__init__.py b/Tools/autotest/jsb_sim/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/autotest/jsb_sim/runsim.py b/Tools/autotest/jsb_sim/runsim.py index 830c1aa05a..cf4f12ece0 100755 --- a/Tools/autotest/jsb_sim/runsim.py +++ b/Tools/autotest/jsb_sim/runsim.py @@ -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) diff --git a/Tools/autotest/param_metadata/__init__.py b/Tools/autotest/param_metadata/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/autotest/param_metadata/emit.py b/Tools/autotest/param_metadata/emit.py index ba92656047..55279413dd 100644 --- a/Tools/autotest/param_metadata/emit.py +++ b/Tools/autotest/param_metadata/emit.py @@ -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 - diff --git a/Tools/autotest/param_metadata/htmlemit.py b/Tools/autotest/param_metadata/htmlemit.py index dd88b1add3..9cb5d09429 100644 --- a/Tools/autotest/param_metadata/htmlemit.py +++ b/Tools/autotest/param_metadata/htmlemit.py @@ -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 = ''' [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

%s

\n' % tag - + t = '\n\n

%s

\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

%s

' % tag - if d.get('User',None) == 'Advanced': + if d.get('User', None) == 'Advanced': t += 'Note: This parameter is for advanced users
' t += "\n\n

%s

\n" % cgi.escape(param.Description) t += "\n" self.t += t - diff --git a/Tools/autotest/param_metadata/param.py b/Tools/autotest/param_metadata/param.py index f36f100a1d..3080c80076 100644 --- a/Tools/autotest/param_metadata/param.py +++ b/Tools/autotest/param_metadata/param.py @@ -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', ] diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index 3aed151cfb..315c7d4638 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -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()) diff --git a/Tools/autotest/param_metadata/rstemit.py b/Tools/autotest/param_metadata/rstemit.py index 99c8fcd707..403aa9081a 100644 --- a/Tools/autotest/param_metadata/rstemit.py +++ b/Tools/autotest/param_metadata/rstemit.py @@ -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() diff --git a/Tools/autotest/param_metadata/wikiemit.py b/Tools/autotest/param_metadata/wikiemit.py index 72279efb6b..61da69834d 100644 --- a/Tools/autotest/param_metadata/wikiemit.py +++ b/Tools/autotest/param_metadata/wikiemit.py @@ -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) - - - diff --git a/Tools/autotest/param_metadata/xmlemit.py b/Tools/autotest/param_metadata/xmlemit.py index 873cc58e7e..f14c7381a7 100644 --- a/Tools/autotest/param_metadata/xmlemit.py +++ b/Tools/autotest/param_metadata/xmlemit.py @@ -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 = ''' @@ -17,56 +18,53 @@ class XmlEmit(Emit): ''' self.f.write(preamble) - - def close(self): + + def close(self): self.f.write('') self.f.write('''\n''') - self.f.close - + self.f.close() + def emit_comment(self, s): self.f.write("") def start_libraries(self): self.f.write('') self.f.write('') - + def emit(self, g, f): - t = '''\n''' % quoteattr(g.name) # i.e. ArduPlane - + t = '''\n''' % quoteattr(g.name) # i.e. ArduPlane + for param in g.params: # Begin our parameter node - if hasattr(param, 'DisplayName'): - t += '%s\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label - + t += '''%s\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label + t += "\n" else: - t += '''%s\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10 - + t += '''%s\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10 + t += '''\n''' t += '''\n''' - - #print t + + # print t self.f.write(t) - - - diff --git a/Tools/autotest/pysim/__init__.py b/Tools/autotest/pysim/__init__.py new file mode 100644 index 0000000000..fc470dd2ce --- /dev/null +++ b/Tools/autotest/pysim/__init__.py @@ -0,0 +1 @@ +"""pysim tools""" \ No newline at end of file diff --git a/Tools/autotest/pysim/aircraft.py b/Tools/autotest/pysim/aircraft.py index 39effb3583..47a1c51f0b 100644 --- a/Tools/autotest/pysim/aircraft.py +++ b/Tools/autotest/pysim/aircraft.py @@ -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 - - - diff --git a/Tools/autotest/pysim/fdpexpect.py b/Tools/autotest/pysim/fdpexpect.py index 2f583e6122..c1f5059077 100644 --- a/Tools/autotest/pysim/fdpexpect.py +++ b/Tools/autotest/pysim/fdpexpect.py @@ -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 = '' % 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 - diff --git a/Tools/autotest/pysim/fg_display.py b/Tools/autotest/pysim/fg_display.py index 032e9c7fa5..9bfb0a5069 100755 --- a/Tools/autotest/pysim/fg_display.py +++ b/Tools/autotest/pysim/fg_display.py @@ -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'), diff --git a/Tools/autotest/pysim/iris_ros.py b/Tools/autotest/pysim/iris_ros.py index 2fdc7cc54e..d81fd56162 100644 --- a/Tools/autotest/pysim/iris_ros.py +++ b/Tools/autotest/pysim/iris_ros.py @@ -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) diff --git a/Tools/autotest/pysim/rotmat.py b/Tools/autotest/pysim/rotmat.py index 6ea3a996f3..0e042208da 100644 --- a/Tools/autotest/pysim/rotmat.py +++ b/Tools/autotest/pysim/rotmat.py @@ -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() diff --git a/Tools/autotest/pysim/testwind.py b/Tools/autotest/pysim/testwind.py index 05a69b1ad8..a4b0ba98a6 100755 --- a/Tools/autotest/pysim/testwind.py +++ b/Tools/autotest/pysim/testwind.py @@ -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 diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index e784fba6cf..794416d1dc 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -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() - diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 8cf52ffa81..e088aa11b9 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -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")) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index e6aba0109a..309f3031a4 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -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)