mirror of https://github.com/ArduPilot/ardupilot
Tools: update python coding style
Tools: update PrintVersion.py coding style autotest: update python coding style pysim: update python coding style jsb_sim: update Python coding style param_metadata: update Python coding style
This commit is contained in:
parent
38b3d3ff3a
commit
9e1ffcae5d
|
@ -1,14 +1,15 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
'''
|
"""
|
||||||
Extract version information for the various vehicle types, print it
|
Extract version information for the various vehicle types, print it
|
||||||
'''
|
"""
|
||||||
|
|
||||||
import sys
|
|
||||||
import re
|
|
||||||
import os
|
import os
|
||||||
|
import re
|
||||||
|
import sys
|
||||||
|
|
||||||
from optparse import OptionParser
|
from optparse import OptionParser
|
||||||
|
|
||||||
parser = OptionParser("print_version.py [options] ArduCopter|ArduPlane|APMrover2|AntennaTracker")
|
parser = OptionParser("print_version.py [options] ArduCopter|ArduPlane|APMrover2|AntennaTracker")
|
||||||
|
|
||||||
(opts, args) = parser.parse_args()
|
(opts, args) = parser.parse_args()
|
||||||
|
@ -25,7 +26,7 @@ if len(args) > 0:
|
||||||
if vehicle not in includefiles:
|
if vehicle not in includefiles:
|
||||||
print("Unknown vehicle (%s) (be in a vehicle directory or supply a vehicle type as an argument)" % (vehicle,))
|
print("Unknown vehicle (%s) (be in a vehicle directory or supply a vehicle type as an argument)" % (vehicle,))
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
includefilepath="%s/%s" % (vehicle, includefiles[vehicle])
|
includefilepath = "%s/%s" % (vehicle, includefiles[vehicle])
|
||||||
else:
|
else:
|
||||||
# assume we are in e.g. APM/APMrover2/
|
# assume we are in e.g. APM/APMrover2/
|
||||||
vehicle = os.path.basename(os.getcwd())
|
vehicle = os.path.basename(os.getcwd())
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
"""Autotests tools suite"""
|
|
@ -1,18 +1,23 @@
|
||||||
# drive APMrover2 in SITL
|
# drive APMrover2 in SITL
|
||||||
|
|
||||||
import util, pexpect, sys, time, math, shutil, os
|
import os
|
||||||
from common import *
|
import shutil
|
||||||
|
|
||||||
|
import pexpect
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
import random
|
|
||||||
|
from common import *
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
|
|
||||||
#HOME=mavutil.location(-35.362938,149.165085,584,270)
|
# HOME=mavutil.location(-35.362938,149.165085,584,270)
|
||||||
HOME=mavutil.location(40.071374969556928,-105.22978898137808,1583.702759,246)
|
HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
|
||||||
homeloc = None
|
homeloc = None
|
||||||
|
|
||||||
|
|
||||||
def arm_rover(mavproxy, mav):
|
def arm_rover(mavproxy, mav):
|
||||||
# wait for EKF and GPS checks to pass
|
# wait for EKF and GPS checks to pass
|
||||||
wait_seconds(mav, 30)
|
wait_seconds(mav, 30)
|
||||||
|
@ -23,15 +28,16 @@ def arm_rover(mavproxy, mav):
|
||||||
print("ROVER ARMED")
|
print("ROVER ARMED")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def drive_left_circuit(mavproxy, mav):
|
def drive_left_circuit(mavproxy, mav):
|
||||||
'''drive a left circuit, 50m on a side'''
|
"""Drive a left circuit, 50m on a side."""
|
||||||
mavproxy.send('switch 6\n')
|
mavproxy.send('switch 6\n')
|
||||||
wait_mode(mav, 'MANUAL')
|
wait_mode(mav, 'MANUAL')
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
|
|
||||||
print("Driving left circuit")
|
print("Driving left circuit")
|
||||||
# do 4 turns
|
# do 4 turns
|
||||||
for i in range(0,4):
|
for i in range(0, 4):
|
||||||
# hard left
|
# hard left
|
||||||
print("Starting turn %u" % i)
|
print("Starting turn %u" % i)
|
||||||
mavproxy.send('rc 1 1000\n')
|
mavproxy.send('rc 1 1000\n')
|
||||||
|
@ -45,8 +51,9 @@ def drive_left_circuit(mavproxy, mav):
|
||||||
print("Circuit complete")
|
print("Circuit complete")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def drive_RTL(mavproxy, mav):
|
def drive_RTL(mavproxy, mav):
|
||||||
'''drive to home'''
|
"""Drive to home."""
|
||||||
print("Driving home in RTL")
|
print("Driving home in RTL")
|
||||||
mavproxy.send('switch 3\n')
|
mavproxy.send('switch 3\n')
|
||||||
if not wait_location(mav, homeloc, accuracy=22, timeout=90):
|
if not wait_location(mav, homeloc, accuracy=22, timeout=90):
|
||||||
|
@ -54,22 +61,23 @@ def drive_RTL(mavproxy, mav):
|
||||||
print("RTL Complete")
|
print("RTL Complete")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def setup_rc(mavproxy):
|
def setup_rc(mavproxy):
|
||||||
'''setup RC override control'''
|
"""Setup RC override control."""
|
||||||
for chan in [1,2,3,4,5,6,7]:
|
for chan in [1, 2, 3, 4, 5, 6, 7]:
|
||||||
mavproxy.send('rc %u 1500\n' % chan)
|
mavproxy.send('rc %u 1500\n' % chan)
|
||||||
mavproxy.send('rc 8 1800\n')
|
mavproxy.send('rc 8 1800\n')
|
||||||
|
|
||||||
|
|
||||||
def drive_mission(mavproxy, mav, filename):
|
def drive_mission(mavproxy, mav, filename):
|
||||||
'''drive a mission from a file'''
|
"""Drive a mission from a file."""
|
||||||
global homeloc
|
global homeloc
|
||||||
print("Driving mission %s" % filename)
|
print("Driving mission %s" % filename)
|
||||||
mavproxy.send('wp load %s\n' % filename)
|
mavproxy.send('wp load %s\n' % filename)
|
||||||
mavproxy.expect('Flight plan received')
|
mavproxy.expect('Flight plan received')
|
||||||
mavproxy.send('wp list\n')
|
mavproxy.send('wp list\n')
|
||||||
mavproxy.expect('Requesting [0-9]+ waypoints')
|
mavproxy.expect('Requesting [0-9]+ waypoints')
|
||||||
mavproxy.send('switch 4\n') # auto mode
|
mavproxy.send('switch 4\n') # auto mode
|
||||||
mavproxy.send('rc 3 1500\n')
|
mavproxy.send('rc 3 1500\n')
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
if not wait_waypoint(mav, 1, 4, max_dist=5):
|
if not wait_waypoint(mav, 1, 4, max_dist=5):
|
||||||
|
@ -79,23 +87,23 @@ def drive_mission(mavproxy, mav, filename):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
def drive_APMrover2(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||||
'''drive APMrover2 in SIL
|
"""Drive APMrover2 in SITL.
|
||||||
|
|
||||||
you can pass viewerip as an IP address to optionally send fg and
|
you can pass viewerip as an IP address to optionally send fg and
|
||||||
mavproxy packets too for local viewing of the mission in real time
|
mavproxy packets too for local viewing of the mission in real time
|
||||||
'''
|
"""
|
||||||
global homeloc
|
global homeloc
|
||||||
|
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += " --out=%s:14550" % viewerip
|
options += " --out=%s:14550" % viewerip
|
||||||
if map:
|
if use_map:
|
||||||
options += ' --map'
|
options += ' --map'
|
||||||
|
|
||||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||||
sil = util.start_SIL(binary, wipe=True, model='rover', home=home, speedup=10)
|
sitl = util.start_SITL(binary, wipe=True, model='rover', home=home, speedup=10)
|
||||||
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
|
mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
|
||||||
|
|
||||||
print("WAITING FOR PARAMETERS")
|
print("WAITING FOR PARAMETERS")
|
||||||
mavproxy.expect('Received [0-9]+ parameters')
|
mavproxy.expect('Received [0-9]+ parameters')
|
||||||
|
@ -109,10 +117,10 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
|
||||||
|
|
||||||
# restart with new parms
|
# restart with new parms
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
sil = util.start_SIL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
|
sitl = util.start_SITL(binary, model='rover', home=home, speedup=10, valgrind=valgrind, gdb=gdb)
|
||||||
mavproxy = util.start_MAVProxy_SIL('APMrover2', options=options)
|
mavproxy = util.start_MAVProxy_SITL('APMrover2', options=options)
|
||||||
mavproxy.expect('Telemetry log: (\S+)')
|
mavproxy.expect('Telemetry log: (\S+)')
|
||||||
logfile = mavproxy.match.group(1)
|
logfile = mavproxy.match.group(1)
|
||||||
print("LOGFILE %s" % logfile)
|
print("LOGFILE %s" % logfile)
|
||||||
|
@ -131,14 +139,14 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
|
||||||
util.expect_setup_callback(mavproxy, expect_callback)
|
util.expect_setup_callback(mavproxy, expect_callback)
|
||||||
|
|
||||||
expect_list_clear()
|
expect_list_clear()
|
||||||
expect_list_extend([sil, mavproxy])
|
expect_list_extend([sitl, mavproxy])
|
||||||
|
|
||||||
print("Started simulator")
|
print("Started simulator")
|
||||||
|
|
||||||
# get a mavlink connection going
|
# get a mavlink connection going
|
||||||
try:
|
try:
|
||||||
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
||||||
raise
|
raise
|
||||||
mav.message_hooks.append(message_hook)
|
mav.message_hooks.append(message_hook)
|
||||||
|
@ -170,15 +178,15 @@ def drive_APMrover2(binary, viewerip=None, map=False, valgrind=False, gdb=False)
|
||||||
# if not drive_RTL(mavproxy, mav):
|
# if not drive_RTL(mavproxy, mav):
|
||||||
# print("Failed RTL")
|
# print("Failed RTL")
|
||||||
# failed = True
|
# failed = True
|
||||||
except pexpect.TIMEOUT, e:
|
except pexpect.TIMEOUT as e:
|
||||||
print("Failed with timeout")
|
print("Failed with timeout")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
mav.close()
|
mav.close()
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
valgrind_log = sil.valgrind_log_filepath()
|
valgrind_log = sitl.valgrind_log_filepath()
|
||||||
if os.path.exists(valgrind_log):
|
if os.path.exists(valgrind_log):
|
||||||
os.chmod(valgrind_log, 0644)
|
os.chmod(valgrind_log, 0644)
|
||||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log"))
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/APMrover2-valgrind.log"))
|
||||||
|
|
|
@ -7,30 +7,38 @@
|
||||||
# switch 5 = Loiter
|
# switch 5 = Loiter
|
||||||
# switch 6 = Stabilize
|
# switch 6 = Stabilize
|
||||||
|
|
||||||
import util, pexpect, sys, time, math, shutil, os
|
import math
|
||||||
from common import *
|
import os
|
||||||
|
import shutil
|
||||||
|
import time
|
||||||
|
|
||||||
|
import pexpect
|
||||||
from pymavlink import mavutil, mavwp
|
from pymavlink import mavutil, mavwp
|
||||||
import random
|
|
||||||
|
from common import *
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
FRAME='+'
|
FRAME = '+'
|
||||||
HOME=mavutil.location(-35.362938,149.165085,584,270)
|
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
|
||||||
AVCHOME=mavutil.location(40.072842,-105.230575,1586,0)
|
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
|
||||||
|
|
||||||
homeloc = None
|
homeloc = None
|
||||||
num_wp = 0
|
num_wp = 0
|
||||||
speedup_default = 10
|
speedup_default = 10
|
||||||
|
|
||||||
|
|
||||||
def hover(mavproxy, mav, hover_throttle=1500):
|
def hover(mavproxy, mav, hover_throttle=1500):
|
||||||
mavproxy.send('rc 3 %u\n' % hover_throttle)
|
mavproxy.send('rc 3 %u\n' % hover_throttle)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def arm_motors(mavproxy, mav):
|
def arm_motors(mavproxy, mav):
|
||||||
'''arm motors'''
|
"""Arm motors."""
|
||||||
print("Arming motors")
|
print("Arming motors")
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
mavproxy.send('rc 4 2000\n')
|
mavproxy.send('rc 4 2000\n')
|
||||||
|
@ -40,10 +48,11 @@ def arm_motors(mavproxy, mav):
|
||||||
print("MOTORS ARMED OK")
|
print("MOTORS ARMED OK")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def disarm_motors(mavproxy, mav):
|
def disarm_motors(mavproxy, mav):
|
||||||
'''disarm motors'''
|
"""Disarm motors."""
|
||||||
print("Disarming motors")
|
print("Disarming motors")
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
mavproxy.send('rc 4 1000\n')
|
mavproxy.send('rc 4 1000\n')
|
||||||
|
@ -54,9 +63,9 @@ def disarm_motors(mavproxy, mav):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
|
def takeoff(mavproxy, mav, alt_min=30, takeoff_throttle=1700):
|
||||||
'''takeoff get to 30m altitude'''
|
"""Takeoff get to 30m altitude."""
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
mavproxy.send('rc 3 %u\n' % takeoff_throttle)
|
mavproxy.send('rc 3 %u\n' % takeoff_throttle)
|
||||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
|
@ -66,10 +75,11 @@ def takeoff(mavproxy, mav, alt_min = 30, takeoff_throttle=1700):
|
||||||
print("TAKEOFF COMPLETE")
|
print("TAKEOFF COMPLETE")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
# loiter - fly south west, then hold loiter within 5m position and altitude
|
# loiter - fly south west, then hold loiter within 5m position and altitude
|
||||||
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
||||||
'''hold loiter position'''
|
"""Hold loiter position."""
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# first aim south east
|
# first aim south east
|
||||||
|
@ -79,7 +89,7 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
||||||
return False
|
return False
|
||||||
mavproxy.send('rc 4 1500\n')
|
mavproxy.send('rc 4 1500\n')
|
||||||
|
|
||||||
#fly south east 50m
|
# fly south east 50m
|
||||||
mavproxy.send('rc 2 1100\n')
|
mavproxy.send('rc 2 1100\n')
|
||||||
if not wait_distance(mav, 50):
|
if not wait_distance(mav, 50):
|
||||||
return False
|
return False
|
||||||
|
@ -114,8 +124,9 @@ def loiter(mavproxy, mav, holdtime=10, maxaltchange=5, maxdistchange=5):
|
||||||
print("Loiter FAILED")
|
print("Loiter FAILED")
|
||||||
return success
|
return success
|
||||||
|
|
||||||
|
|
||||||
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
|
def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=1080):
|
||||||
'''change altitude'''
|
"""Change altitude."""
|
||||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
if(m.alt < alt_min):
|
if(m.alt < alt_min):
|
||||||
print("Rise to alt:%u from %u" % (alt_min, m.alt))
|
print("Rise to alt:%u from %u" % (alt_min, m.alt))
|
||||||
|
@ -124,13 +135,14 @@ def change_alt(mavproxy, mav, alt_min, climb_throttle=1920, descend_throttle=108
|
||||||
else:
|
else:
|
||||||
print("Lower to alt:%u from %u" % (alt_min, m.alt))
|
print("Lower to alt:%u from %u" % (alt_min, m.alt))
|
||||||
mavproxy.send('rc 3 %u\n' % descend_throttle)
|
mavproxy.send('rc 3 %u\n' % descend_throttle)
|
||||||
wait_altitude(mav, (alt_min -5), alt_min)
|
wait_altitude(mav, (alt_min - 5), alt_min)
|
||||||
hover(mavproxy, mav)
|
hover(mavproxy, mav)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
# fly a square in stabilize mode
|
# fly a square in stabilize mode
|
||||||
def fly_square(mavproxy, mav, side=50, timeout=300):
|
def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
'''fly a square, flying N then E'''
|
"""Fly a square, flying N then E ."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
success = True
|
success = True
|
||||||
|
|
||||||
|
@ -166,7 +178,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
print("Going north %u meters" % side)
|
print("Going north %u meters" % side)
|
||||||
mavproxy.send('rc 2 1300\n')
|
mavproxy.send('rc 2 1300\n')
|
||||||
if not wait_distance(mav, side):
|
if not wait_distance(mav, side):
|
||||||
print("Failed to reach distance of %u") % side
|
print("Failed to reach distance of %u" % side)
|
||||||
success = False
|
success = False
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
|
@ -178,7 +190,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
print("Going east %u meters" % side)
|
print("Going east %u meters" % side)
|
||||||
mavproxy.send('rc 1 1700\n')
|
mavproxy.send('rc 1 1700\n')
|
||||||
if not wait_distance(mav, side):
|
if not wait_distance(mav, side):
|
||||||
print("Failed to reach distance of %u") % side
|
print("Failed to reach distance of %u" % side)
|
||||||
success = False
|
success = False
|
||||||
mavproxy.send('rc 1 1500\n')
|
mavproxy.send('rc 1 1500\n')
|
||||||
|
|
||||||
|
@ -190,7 +202,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
print("Going south %u meters" % side)
|
print("Going south %u meters" % side)
|
||||||
mavproxy.send('rc 2 1700\n')
|
mavproxy.send('rc 2 1700\n')
|
||||||
if not wait_distance(mav, side):
|
if not wait_distance(mav, side):
|
||||||
print("Failed to reach distance of %u") % side
|
print("Failed to reach distance of %u" % side)
|
||||||
success = False
|
success = False
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
|
@ -202,7 +214,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
print("Going west %u meters" % side)
|
print("Going west %u meters" % side)
|
||||||
mavproxy.send('rc 1 1300\n')
|
mavproxy.send('rc 1 1300\n')
|
||||||
if not wait_distance(mav, side):
|
if not wait_distance(mav, side):
|
||||||
print("Failed to reach distance of %u") % side
|
print("Failed to reach distance of %u" % side)
|
||||||
success = False
|
success = False
|
||||||
mavproxy.send('rc 1 1500\n')
|
mavproxy.send('rc 1 1500\n')
|
||||||
|
|
||||||
|
@ -212,7 +224,7 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
|
|
||||||
# descend to 10m
|
# descend to 10m
|
||||||
print("Descend to 10m in Loiter")
|
print("Descend to 10m in Loiter")
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
mavproxy.send('rc 3 1300\n')
|
mavproxy.send('rc 3 1300\n')
|
||||||
time_left = timeout - (get_sim_time(mav) - tstart)
|
time_left = timeout - (get_sim_time(mav) - tstart)
|
||||||
|
@ -226,8 +238,9 @@ def fly_square(mavproxy, mav, side=50, timeout=300):
|
||||||
|
|
||||||
return success
|
return success
|
||||||
|
|
||||||
|
|
||||||
def fly_RTL(mavproxy, mav, side=60, timeout=250):
|
def fly_RTL(mavproxy, mav, side=60, timeout=250):
|
||||||
'''Return, land'''
|
"""Return, land."""
|
||||||
print("# Enter RTL")
|
print("# Enter RTL")
|
||||||
mavproxy.send('switch 3\n')
|
mavproxy.send('switch 3\n')
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
|
@ -240,8 +253,9 @@ def fly_RTL(mavproxy, mav, side=60, timeout=250):
|
||||||
return True
|
return True
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||||
'''Fly east, Failsafe, return, land'''
|
"""Fly east, Failsafe, return, land."""
|
||||||
|
|
||||||
# switch to loiter mode temporarily to stop us from rising
|
# switch to loiter mode temporarily to stop us from rising
|
||||||
mavproxy.send('switch 5\n')
|
mavproxy.send('switch 5\n')
|
||||||
|
@ -285,9 +299,9 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||||
# reduce throttle
|
# reduce throttle
|
||||||
mavproxy.send('rc 3 1100\n')
|
mavproxy.send('rc 3 1100\n')
|
||||||
# switch back to stabilize
|
# switch back to stabilize
|
||||||
mavproxy.send('switch 2\n') # land mode
|
mavproxy.send('switch 2\n') # land mode
|
||||||
wait_mode(mav, 'LAND')
|
wait_mode(mav, 'LAND')
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
print("Reached failsafe home OK")
|
print("Reached failsafe home OK")
|
||||||
return True
|
return True
|
||||||
|
@ -295,12 +309,13 @@ def fly_throttle_failsafe(mavproxy, mav, side=60, timeout=180):
|
||||||
# reduce throttle
|
# reduce throttle
|
||||||
mavproxy.send('rc 3 1100\n')
|
mavproxy.send('rc 3 1100\n')
|
||||||
# switch back to stabilize mode
|
# switch back to stabilize mode
|
||||||
mavproxy.send('switch 2\n') # land mode
|
mavproxy.send('switch 2\n') # land mode
|
||||||
wait_mode(mav, 'LAND')
|
wait_mode(mav, 'LAND')
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def fly_battery_failsafe(mavproxy, mav, timeout=30):
|
def fly_battery_failsafe(mavproxy, mav, timeout=30):
|
||||||
# assume failure
|
# assume failure
|
||||||
success = False
|
success = False
|
||||||
|
@ -332,10 +347,11 @@ def fly_battery_failsafe(mavproxy, mav, timeout=30):
|
||||||
|
|
||||||
return success
|
return success
|
||||||
|
|
||||||
|
|
||||||
# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
|
# fly_stability_patch - fly south, then hold loiter within 5m position and altitude and reduce 1 motor to 60% efficiency
|
||||||
def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
|
def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchange=10):
|
||||||
'''hold loiter position'''
|
"""Hold loiter position."""
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# first south
|
# first south
|
||||||
|
@ -345,7 +361,7 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
|
||||||
return False
|
return False
|
||||||
mavproxy.send('rc 4 1500\n')
|
mavproxy.send('rc 4 1500\n')
|
||||||
|
|
||||||
#fly west 80m
|
# fly west 80m
|
||||||
mavproxy.send('rc 2 1100\n')
|
mavproxy.send('rc 2 1100\n')
|
||||||
if not wait_distance(mav, 80):
|
if not wait_distance(mav, 80):
|
||||||
return False
|
return False
|
||||||
|
@ -390,10 +406,11 @@ def fly_stability_patch(mavproxy, mav, holdtime=30, maxaltchange=5, maxdistchang
|
||||||
|
|
||||||
return success
|
return success
|
||||||
|
|
||||||
|
|
||||||
# fly_fence_test - fly east until you hit the horizontal circular fence
|
# fly_fence_test - fly east until you hit the horizontal circular fence
|
||||||
def fly_fence_test(mavproxy, mav, timeout=180):
|
def fly_fence_test(mavproxy, mav, timeout=180):
|
||||||
'''hold loiter position'''
|
"""Hold loiter position."""
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# enable fence, disable avoidance
|
# enable fence, disable avoidance
|
||||||
|
@ -421,7 +438,7 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||||
home_distance = get_distance(HOME, pos)
|
home_distance = get_distance(HOME, pos)
|
||||||
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
|
print("Alt: %u HomeDistance: %.0f" % (m.alt, home_distance))
|
||||||
# recenter pitch sticks once we reach home so we don't fly off again
|
# recenter pitch sticks once we reach home so we don't fly off again
|
||||||
if pitching_forward and home_distance < 10 :
|
if pitching_forward and home_distance < 10:
|
||||||
pitching_forward = False
|
pitching_forward = False
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
# disable fence
|
# disable fence
|
||||||
|
@ -430,9 +447,9 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||||
# reduce throttle
|
# reduce throttle
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
# switch mode to stabilize
|
# switch mode to stabilize
|
||||||
mavproxy.send('switch 2\n') # land mode
|
mavproxy.send('switch 2\n') # land mode
|
||||||
wait_mode(mav, 'LAND')
|
wait_mode(mav, 'LAND')
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
print("Reached home OK")
|
print("Reached home OK")
|
||||||
return True
|
return True
|
||||||
|
@ -444,15 +461,16 @@ def fly_fence_test(mavproxy, mav, timeout=180):
|
||||||
# reduce throttle
|
# reduce throttle
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
# switch mode to stabilize
|
# switch mode to stabilize
|
||||||
mavproxy.send('switch 2\n') # land mode
|
mavproxy.send('switch 2\n') # land mode
|
||||||
wait_mode(mav, 'LAND')
|
wait_mode(mav, 'LAND')
|
||||||
mavproxy.send('switch 6\n') # stabilize mode
|
mavproxy.send('switch 6\n') # stabilize mode
|
||||||
wait_mode(mav, 'STABILIZE')
|
wait_mode(mav, 'STABILIZE')
|
||||||
print("Fence test failed to reach home - timed out after %u seconds" % timeout)
|
print("Fence test failed to reach home - timed out after %u seconds" % timeout)
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def show_gps_and_sim_positions(mavproxy, on_off):
|
def show_gps_and_sim_positions(mavproxy, on_off):
|
||||||
if on_off == True:
|
if on_off is True:
|
||||||
# turn on simulator display of gps and actual position
|
# turn on simulator display of gps and actual position
|
||||||
mavproxy.send('map set showgpspos 1\n')
|
mavproxy.send('map set showgpspos 1\n')
|
||||||
mavproxy.send('map set showsimpos 1\n')
|
mavproxy.send('map set showsimpos 1\n')
|
||||||
|
@ -461,24 +479,25 @@ def show_gps_and_sim_positions(mavproxy, on_off):
|
||||||
mavproxy.send('map set showgpspos 0\n')
|
mavproxy.send('map set showgpspos 0\n')
|
||||||
mavproxy.send('map set showsimpos 0\n')
|
mavproxy.send('map set showsimpos 0\n')
|
||||||
|
|
||||||
# fly_gps_glitch_loiter_test - fly south east in loiter and test reaction to gps glitch
|
|
||||||
def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
|
def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_distance=20):
|
||||||
'''hold loiter position'''
|
"""fly_gps_glitch_loiter_test.
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
|
||||||
|
Fly south east in loiter and test reaction to gps glitch."""
|
||||||
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# turn on simulator display of gps and actual position
|
# turn on simulator display of gps and actual position
|
||||||
if (use_map):
|
if (use_map):
|
||||||
show_gps_and_sim_positions(mavproxy, True)
|
show_gps_and_sim_positions(mavproxy, True)
|
||||||
|
|
||||||
|
|
||||||
# set-up gps glitch array
|
# set-up gps glitch array
|
||||||
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221]
|
glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
|
||||||
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304]
|
glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
|
||||||
glitch_num = len(glitch_lat)
|
glitch_num = len(glitch_lat)
|
||||||
print("GPS Glitches:")
|
print("GPS Glitches:")
|
||||||
for i in range(1,glitch_num):
|
for i in range(1, glitch_num):
|
||||||
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
|
print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
|
||||||
|
|
||||||
# turn south east
|
# turn south east
|
||||||
print("turn south east")
|
print("turn south east")
|
||||||
|
@ -510,7 +529,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
|
||||||
success = True
|
success = True
|
||||||
|
|
||||||
# initialise current glitch
|
# initialise current glitch
|
||||||
glitch_current = 0;
|
glitch_current = 0
|
||||||
print("Apply first glitch")
|
print("Apply first glitch")
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
||||||
|
@ -529,7 +548,7 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
mavproxy.send('param set SIM_GPS_GLITCH_Y 0\n')
|
||||||
else:
|
else:
|
||||||
print("Applying glitch %u" % glitch_current)
|
print("Applying glitch %u" % glitch_current)
|
||||||
#move onto the next glitch
|
# move onto the next glitch
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
||||||
|
|
||||||
|
@ -557,16 +576,17 @@ def fly_gps_glitch_loiter_test(mavproxy, mav, use_map=False, timeout=30, max_dis
|
||||||
print("GPS glitch test FAILED!")
|
print("GPS glitch test FAILED!")
|
||||||
return success
|
return success
|
||||||
|
|
||||||
|
|
||||||
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
|
# fly_gps_glitch_auto_test - fly mission and test reaction to gps glitch
|
||||||
def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||||
|
|
||||||
# set-up gps glitch array
|
# set-up gps glitch array
|
||||||
glitch_lat = [0.0002996,0.0006958,0.0009431,0.0009991,0.0009444,0.0007716,0.0006221]
|
glitch_lat = [0.0002996, 0.0006958, 0.0009431, 0.0009991, 0.0009444, 0.0007716, 0.0006221]
|
||||||
glitch_lon = [0.0000717,0.0000912,0.0002761,0.0002626,0.0002807,0.0002049,0.0001304]
|
glitch_lon = [0.0000717, 0.0000912, 0.0002761, 0.0002626, 0.0002807, 0.0002049, 0.0001304]
|
||||||
glitch_num = len(glitch_lat)
|
glitch_num = len(glitch_lat)
|
||||||
print("GPS Glitches:")
|
print("GPS Glitches:")
|
||||||
for i in range(1,glitch_num):
|
for i in range(1, glitch_num):
|
||||||
print("glitch %d %.7f %.7f" % (i,glitch_lat[i],glitch_lon[i]))
|
print("glitch %d %.7f %.7f" % (i, glitch_lat[i], glitch_lon[i]))
|
||||||
|
|
||||||
# Fly mission #1
|
# Fly mission #1
|
||||||
print("# Load copter_glitch_mission")
|
print("# Load copter_glitch_mission")
|
||||||
|
@ -585,7 +605,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||||
mavproxy.send('wp set 1\n')
|
mavproxy.send('wp set 1\n')
|
||||||
|
|
||||||
# switch into AUTO mode and raise throttle
|
# switch into AUTO mode and raise throttle
|
||||||
mavproxy.send('switch 4\n') # auto mode
|
mavproxy.send('switch 4\n') # auto mode
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
mavproxy.send('rc 3 1500\n')
|
mavproxy.send('rc 3 1500\n')
|
||||||
|
|
||||||
|
@ -600,7 +620,7 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||||
tnow = tstart
|
tnow = tstart
|
||||||
|
|
||||||
# initialise current glitch
|
# initialise current glitch
|
||||||
glitch_current = 0;
|
glitch_current = 0
|
||||||
print("Apply first glitch")
|
print("Apply first glitch")
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_X %.7f\n' % glitch_lat[glitch_current])
|
||||||
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
mavproxy.send('param set SIM_GPS_GLITCH_Y %.7f\n' % glitch_lon[glitch_current])
|
||||||
|
@ -647,17 +667,18 @@ def fly_gps_glitch_auto_test(mavproxy, mav, use_map=False, timeout=120):
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
#fly_simple - assumes the simple bearing is initialised to be directly north
|
|
||||||
|
# fly_simple - assumes the simple bearing is initialised to be directly north
|
||||||
# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
|
# flies a box with 100m west, 15 seconds north, 50 seconds east, 15 seconds south
|
||||||
def fly_simple(mavproxy, mav, side=50, timeout=120):
|
def fly_simple(mavproxy, mav, side=50, timeout=120):
|
||||||
|
|
||||||
failed = False
|
failed = False
|
||||||
|
|
||||||
# hold position in loiter
|
# hold position in loiter
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
#set SIMPLE mode for all flight modes
|
# set SIMPLE mode for all flight modes
|
||||||
mavproxy.send('param set SIMPLE 63\n')
|
mavproxy.send('param set SIMPLE 63\n')
|
||||||
|
|
||||||
# switch to stabilize mode
|
# switch to stabilize mode
|
||||||
|
@ -679,7 +700,7 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
|
||||||
while get_sim_time(mav) < (tstart + 8):
|
while get_sim_time(mav) < (tstart + 8):
|
||||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
delta = (get_sim_time(mav) - tstart)
|
delta = (get_sim_time(mav) - tstart)
|
||||||
#print("%u" % delta)
|
# print("%u" % delta)
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
# fly north 25 meters
|
# fly north 25 meters
|
||||||
|
@ -696,23 +717,24 @@ def fly_simple(mavproxy, mav, side=50, timeout=120):
|
||||||
while get_sim_time(mav) < (tstart + 8):
|
while get_sim_time(mav) < (tstart + 8):
|
||||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
delta = (get_sim_time(mav) - tstart)
|
delta = (get_sim_time(mav) - tstart)
|
||||||
#print("%u" % delta)
|
# print("%u" % delta)
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
#restore to default
|
# restore to default
|
||||||
mavproxy.send('param set SIMPLE 0\n')
|
mavproxy.send('param set SIMPLE 0\n')
|
||||||
|
|
||||||
#hover in place
|
# hover in place
|
||||||
hover(mavproxy, mav)
|
hover(mavproxy, mav)
|
||||||
return not failed
|
return not failed
|
||||||
|
|
||||||
#fly_super_simple - flies a circle around home for 45 seconds
|
|
||||||
|
# fly_super_simple - flies a circle around home for 45 seconds
|
||||||
def fly_super_simple(mavproxy, mav, timeout=45):
|
def fly_super_simple(mavproxy, mav, timeout=45):
|
||||||
|
|
||||||
failed = False
|
failed = False
|
||||||
|
|
||||||
# hold position in loiter
|
# hold position in loiter
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# fly forward 20m
|
# fly forward 20m
|
||||||
|
@ -722,7 +744,7 @@ def fly_super_simple(mavproxy, mav, timeout=45):
|
||||||
failed = True
|
failed = True
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
#set SUPER SIMPLE mode for all flight modes
|
# set SUPER SIMPLE mode for all flight modes
|
||||||
mavproxy.send('param set SUPER_SIMPLE 63\n')
|
mavproxy.send('param set SUPER_SIMPLE 63\n')
|
||||||
|
|
||||||
# switch to stabilize mode
|
# switch to stabilize mode
|
||||||
|
@ -745,18 +767,19 @@ def fly_super_simple(mavproxy, mav, timeout=45):
|
||||||
mavproxy.send('rc 1 1500\n')
|
mavproxy.send('rc 1 1500\n')
|
||||||
mavproxy.send('rc 4 1500\n')
|
mavproxy.send('rc 4 1500\n')
|
||||||
|
|
||||||
#restore simple mode parameters to default
|
# restore simple mode parameters to default
|
||||||
mavproxy.send('param set SUPER_SIMPLE 0\n')
|
mavproxy.send('param set SUPER_SIMPLE 0\n')
|
||||||
|
|
||||||
#hover in place
|
# hover in place
|
||||||
hover(mavproxy, mav)
|
hover(mavproxy, mav)
|
||||||
return not failed
|
return not failed
|
||||||
|
|
||||||
#fly_circle - flies a circle with 20m radius
|
|
||||||
|
# fly_circle - flies a circle with 20m radius
|
||||||
def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
||||||
|
|
||||||
# hold position in loiter
|
# hold position in loiter
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
|
||||||
# face west
|
# face west
|
||||||
|
@ -766,7 +789,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
||||||
return False
|
return False
|
||||||
mavproxy.send('rc 4 1500\n')
|
mavproxy.send('rc 4 1500\n')
|
||||||
|
|
||||||
#set CIRCLE radius
|
# set CIRCLE radius
|
||||||
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
|
mavproxy.send('param set CIRCLE_RADIUS 3000\n')
|
||||||
|
|
||||||
# fly forward (east) at least 100m
|
# fly forward (east) at least 100m
|
||||||
|
@ -778,7 +801,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
||||||
mavproxy.send('rc 2 1500\n')
|
mavproxy.send('rc 2 1500\n')
|
||||||
|
|
||||||
# set CIRCLE mode
|
# set CIRCLE mode
|
||||||
mavproxy.send('switch 1\n') # circle mode
|
mavproxy.send('switch 1\n') # circle mode
|
||||||
wait_mode(mav, 'CIRCLE')
|
wait_mode(mav, 'CIRCLE')
|
||||||
|
|
||||||
# wait
|
# wait
|
||||||
|
@ -794,6 +817,7 @@ def fly_circle(mavproxy, mav, maxaltchange=10, holdtime=36):
|
||||||
print("CIRCLE OK for %u seconds" % holdtime)
|
print("CIRCLE OK for %u seconds" % holdtime)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
# fly_auto_test - fly mission which tests a significant number of commands
|
# fly_auto_test - fly mission which tests a significant number of commands
|
||||||
def fly_auto_test(mavproxy, mav):
|
def fly_auto_test(mavproxy, mav):
|
||||||
|
|
||||||
|
@ -810,7 +834,7 @@ def fly_auto_test(mavproxy, mav):
|
||||||
mavproxy.send('wp set 1\n')
|
mavproxy.send('wp set 1\n')
|
||||||
|
|
||||||
# switch into AUTO mode and raise throttle
|
# switch into AUTO mode and raise throttle
|
||||||
mavproxy.send('switch 4\n') # auto mode
|
mavproxy.send('switch 4\n') # auto mode
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
mavproxy.send('rc 3 1500\n')
|
mavproxy.send('rc 3 1500\n')
|
||||||
|
|
||||||
|
@ -818,7 +842,7 @@ def fly_auto_test(mavproxy, mav):
|
||||||
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
||||||
|
|
||||||
# land if mission failed
|
# land if mission failed
|
||||||
if ret == False:
|
if ret is False:
|
||||||
land(mavproxy, mav)
|
land(mavproxy, mav)
|
||||||
|
|
||||||
# set throttle to minimum
|
# set throttle to minimum
|
||||||
|
@ -832,6 +856,7 @@ def fly_auto_test(mavproxy, mav):
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
||||||
# fly_avc_test - fly AVC mission
|
# fly_avc_test - fly AVC mission
|
||||||
def fly_avc_test(mavproxy, mav):
|
def fly_avc_test(mavproxy, mav):
|
||||||
|
|
||||||
|
@ -848,7 +873,7 @@ def fly_avc_test(mavproxy, mav):
|
||||||
mavproxy.send('wp set 1\n')
|
mavproxy.send('wp set 1\n')
|
||||||
|
|
||||||
# switch into AUTO mode and raise throttle
|
# switch into AUTO mode and raise throttle
|
||||||
mavproxy.send('switch 4\n') # auto mode
|
mavproxy.send('switch 4\n') # auto mode
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
mavproxy.send('rc 3 1500\n')
|
mavproxy.send('rc 3 1500\n')
|
||||||
|
|
||||||
|
@ -866,23 +891,25 @@ def fly_avc_test(mavproxy, mav):
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
||||||
def land(mavproxy, mav, timeout=60):
|
def land(mavproxy, mav, timeout=60):
|
||||||
'''land the quad'''
|
"""Land the quad."""
|
||||||
print("STARTING LANDING")
|
print("STARTING LANDING")
|
||||||
mavproxy.send('switch 2\n') # land mode
|
mavproxy.send('switch 2\n') # land mode
|
||||||
wait_mode(mav, 'LAND')
|
wait_mode(mav, 'LAND')
|
||||||
print("Entered Landing Mode")
|
print("Entered Landing Mode")
|
||||||
ret = wait_altitude(mav, -5, 1)
|
ret = wait_altitude(mav, -5, 1)
|
||||||
print("LANDING: ok= %s" % ret)
|
print("LANDING: ok= %s" % ret)
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
|
|
||||||
'''fly a mission from a file'''
|
def fly_mission(mavproxy, mav, height_accuracy=-1.0, target_altitude=None):
|
||||||
|
"""Fly a mission from a file."""
|
||||||
global homeloc
|
global homeloc
|
||||||
global num_wp
|
global num_wp
|
||||||
print("test: Fly a mission from 1 to %u" % num_wp)
|
print("test: Fly a mission from 1 to %u" % num_wp)
|
||||||
mavproxy.send('wp set 1\n')
|
mavproxy.send('wp set 1\n')
|
||||||
mavproxy.send('switch 4\n') # auto mode
|
mavproxy.send('switch 4\n') # auto mode
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
ret = wait_waypoint(mav, 0, num_wp-1, timeout=500)
|
||||||
expect_msg = "Reached command #%u" % (num_wp-1)
|
expect_msg = "Reached command #%u" % (num_wp-1)
|
||||||
|
@ -890,12 +917,13 @@ def fly_mission(mavproxy, mav, height_accuracy=-1, target_altitude=None):
|
||||||
mavproxy.expect(expect_msg)
|
mavproxy.expect(expect_msg)
|
||||||
print("test: MISSION COMPLETE: passed=%s" % ret)
|
print("test: MISSION COMPLETE: passed=%s" % ret)
|
||||||
# wait here until ready
|
# wait here until ready
|
||||||
mavproxy.send('switch 5\n') # loiter mode
|
mavproxy.send('switch 5\n') # loiter mode
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
||||||
def load_mission_from_file(mavproxy, mav, filename):
|
def load_mission_from_file(mavproxy, mav, filename):
|
||||||
'''Load a mission from a file to flight controller'''
|
"""Load a mission from a file to flight controller."""
|
||||||
global num_wp
|
global num_wp
|
||||||
mavproxy.send('wp load %s\n' % filename)
|
mavproxy.send('wp load %s\n' % filename)
|
||||||
mavproxy.expect('Flight plan received')
|
mavproxy.expect('Flight plan received')
|
||||||
|
@ -908,6 +936,7 @@ def load_mission_from_file(mavproxy, mav, filename):
|
||||||
num_wp = wploader.count()
|
num_wp = wploader.count()
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def save_mission_to_file(mavproxy, mav, filename):
|
def save_mission_to_file(mavproxy, mav, filename):
|
||||||
global num_wp
|
global num_wp
|
||||||
mavproxy.send('wp save %s\n' % filename)
|
mavproxy.send('wp save %s\n' % filename)
|
||||||
|
@ -916,24 +945,26 @@ def save_mission_to_file(mavproxy, mav, filename):
|
||||||
print("num_wp: %d" % num_wp)
|
print("num_wp: %d" % num_wp)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def setup_rc(mavproxy):
|
def setup_rc(mavproxy):
|
||||||
'''setup RC override control'''
|
"""Setup RC override control."""
|
||||||
for chan in range(1,9):
|
for chan in range(1, 9):
|
||||||
mavproxy.send('rc %u 1500\n' % chan)
|
mavproxy.send('rc %u 1500\n' % chan)
|
||||||
# zero throttle
|
# zero throttle
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
|
|
||||||
|
|
||||||
def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||||
'''fly ArduCopter in SIL
|
"""Fly ArduCopter in SITL.
|
||||||
|
|
||||||
you can pass viewerip as an IP address to optionally send fg and
|
you can pass viewerip as an IP address to optionally send fg and
|
||||||
mavproxy packets too for local viewing of the flight in real time
|
mavproxy packets too for local viewing of the flight in real time
|
||||||
'''
|
"""
|
||||||
global homeloc
|
global homeloc
|
||||||
|
|
||||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||||
sil = util.start_SIL(binary, wipe=True, model='+', home=home, speedup=speedup_default)
|
sitl = util.start_SITL(binary, wipe=True, model='+', home=home, speedup=speedup_default)
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter')
|
||||||
mavproxy.expect('Received [0-9]+ parameters')
|
mavproxy.expect('Received [0-9]+ parameters')
|
||||||
|
|
||||||
# setup test parameters
|
# setup test parameters
|
||||||
|
@ -945,29 +976,29 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||||
|
|
||||||
# reboot with new parameters
|
# reboot with new parameters
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
sil = util.start_SIL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
|
sitl = util.start_SITL(binary, model='+', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=5'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += ' --out=%s:14550' % viewerip
|
options += ' --out=%s:14550' % viewerip
|
||||||
if use_map:
|
if use_map:
|
||||||
options += ' --map'
|
options += ' --map'
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
|
||||||
mavproxy.expect('Telemetry log: (\S+)')
|
mavproxy.expect('Telemetry log: (\S+)')
|
||||||
logfile = mavproxy.match.group(1)
|
logfile = mavproxy.match.group(1)
|
||||||
print("LOGFILE %s" % logfile)
|
print("LOGFILE %s" % logfile)
|
||||||
|
|
||||||
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
|
buildlog = util.reltopdir("../buildlogs/ArduCopter-test.tlog")
|
||||||
print("buildlog=%s" % buildlog)
|
print("buildlog=%s" % buildlog)
|
||||||
copyTLog = False
|
copy_tlog = False
|
||||||
if os.path.exists(buildlog):
|
if os.path.exists(buildlog):
|
||||||
os.unlink(buildlog)
|
os.unlink(buildlog)
|
||||||
try:
|
try:
|
||||||
os.link(logfile, buildlog)
|
os.link(logfile, buildlog)
|
||||||
except Exception:
|
except Exception:
|
||||||
print( "WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location" )
|
print("WARN: Failed to create symlink: " + logfile + " => " + buildlog + ", Will copy tlog manually to target location")
|
||||||
copyTLog = True
|
copy_tlog = True
|
||||||
|
|
||||||
# the received parameters can come before or after the ready to fly message
|
# the received parameters can come before or after the ready to fly message
|
||||||
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY'])
|
||||||
|
@ -976,18 +1007,17 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||||
util.expect_setup_callback(mavproxy, expect_callback)
|
util.expect_setup_callback(mavproxy, expect_callback)
|
||||||
|
|
||||||
expect_list_clear()
|
expect_list_clear()
|
||||||
expect_list_extend([sil, mavproxy])
|
expect_list_extend([sitl, mavproxy])
|
||||||
|
|
||||||
# get a mavlink connection going
|
# get a mavlink connection going
|
||||||
try:
|
try:
|
||||||
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
||||||
raise
|
raise
|
||||||
mav.message_hooks.append(message_hook)
|
mav.message_hooks.append(message_hook)
|
||||||
mav.idle_hooks.append(idle_hook)
|
mav.idle_hooks.append(idle_hook)
|
||||||
|
|
||||||
|
|
||||||
failed = False
|
failed = False
|
||||||
failed_test_msg = "None"
|
failed_test_msg = "None"
|
||||||
|
|
||||||
|
@ -1030,7 +1060,7 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||||
|
|
||||||
# fly the stored mission
|
# fly the stored mission
|
||||||
print("# Fly CH7 saved mission")
|
print("# Fly CH7 saved mission")
|
||||||
if not fly_mission(mavproxy, mav,height_accuracy = 0.5, target_altitude=10):
|
if not fly_mission(mavproxy, mav, height_accuracy=0.5, target_altitude=10):
|
||||||
failed_test_msg = "fly ch7_mission failed"
|
failed_test_msg = "fly ch7_mission failed"
|
||||||
print(failed_test_msg)
|
print(failed_test_msg)
|
||||||
failed = True
|
failed = True
|
||||||
|
@ -1250,22 +1280,22 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||||
print(failed_test_msg)
|
print(failed_test_msg)
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
except pexpect.TIMEOUT, failed_test_msg:
|
except pexpect.TIMEOUT as failed_test_msg:
|
||||||
failed_test_msg = "Timeout"
|
failed_test_msg = "Timeout"
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
mav.close()
|
mav.close()
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
valgrind_log = sil.valgrind_log_filepath()
|
valgrind_log = sitl.valgrind_log_filepath()
|
||||||
if os.path.exists(valgrind_log):
|
if os.path.exists(valgrind_log):
|
||||||
os.chmod(valgrind_log, 0644)
|
os.chmod(valgrind_log, 0644)
|
||||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduCopter-valgrind.log"))
|
||||||
|
|
||||||
# [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link
|
# [2014/05/07] FC Because I'm doing a cross machine build (source is on host, build is on guest VM) I cannot hard link
|
||||||
# This flag tells me that I need to copy the data out
|
# This flag tells me that I need to copy the data out
|
||||||
if copyTLog:
|
if copy_tlog:
|
||||||
shutil.copy(logfile, buildlog)
|
shutil.copy(logfile, buildlog)
|
||||||
|
|
||||||
if failed:
|
if failed:
|
||||||
|
@ -1274,14 +1304,13 @@ def fly_ArduCopter(binary, viewerip=None, use_map=False, valgrind=False, gdb=Fal
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
def fly_CopterAVC(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||||
'''fly ArduCopter in SIL for AVC2013 mission
|
"""Fly ArduCopter in SITL for AVC2013 mission."""
|
||||||
'''
|
|
||||||
global homeloc
|
global homeloc
|
||||||
|
|
||||||
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
|
home = "%f,%f,%u,%u" % (AVCHOME.lat, AVCHOME.lng, AVCHOME.alt, AVCHOME.heading)
|
||||||
sil = util.start_SIL(binary, wipe=True, model='heli', home=home, speedup=speedup_default)
|
sitl = util.start_SITL(binary, wipe=True, model='heli', home=home, speedup=speedup_default)
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options='--sitl=127.0.0.1:5501 --out=127.0.0.1:19550')
|
||||||
mavproxy.expect('Received [0-9]+ parameters')
|
mavproxy.expect('Received [0-9]+ parameters')
|
||||||
|
|
||||||
# setup test parameters
|
# setup test parameters
|
||||||
|
@ -1293,15 +1322,15 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
|
|
||||||
# reboot with new parameters
|
# reboot with new parameters
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
sil = util.start_SIL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
|
sitl = util.start_SITL(binary, model='heli', home=home, speedup=speedup_default, valgrind=valgrind, gdb=gdb)
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=5'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += ' --out=%s:14550' % viewerip
|
options += ' --out=%s:14550' % viewerip
|
||||||
if map:
|
if use_map:
|
||||||
options += ' --map'
|
options += ' --map'
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options)
|
mavproxy = util.start_MAVProxy_SITL('ArduCopter', options=options)
|
||||||
mavproxy.expect('Telemetry log: (\S+)')
|
mavproxy.expect('Telemetry log: (\S+)')
|
||||||
logfile = mavproxy.match.group(1)
|
logfile = mavproxy.match.group(1)
|
||||||
print("LOGFILE %s" % logfile)
|
print("LOGFILE %s" % logfile)
|
||||||
|
@ -1322,22 +1351,21 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
util.expect_setup_callback(mavproxy, expect_callback)
|
util.expect_setup_callback(mavproxy, expect_callback)
|
||||||
|
|
||||||
expect_list_clear()
|
expect_list_clear()
|
||||||
expect_list_extend([sil, mavproxy])
|
expect_list_extend([sitl, mavproxy])
|
||||||
|
|
||||||
if map:
|
if use_map:
|
||||||
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
|
mavproxy.send('map icon 40.072467969730496 -105.2314389590174\n')
|
||||||
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
|
mavproxy.send('map icon 40.072600990533829 -105.23146100342274\n')
|
||||||
|
|
||||||
# get a mavlink connection going
|
# get a mavlink connection going
|
||||||
try:
|
try:
|
||||||
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
||||||
raise
|
raise
|
||||||
mav.message_hooks.append(message_hook)
|
mav.message_hooks.append(message_hook)
|
||||||
mav.idle_hooks.append(idle_hook)
|
mav.idle_hooks.append(idle_hook)
|
||||||
|
|
||||||
|
|
||||||
failed = False
|
failed = False
|
||||||
failed_test_msg = "None"
|
failed_test_msg = "None"
|
||||||
|
|
||||||
|
@ -1373,21 +1401,21 @@ def fly_CopterAVC(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
print("Lowering rotor speed")
|
print("Lowering rotor speed")
|
||||||
mavproxy.send('rc 8 1000\n')
|
mavproxy.send('rc 8 1000\n')
|
||||||
|
|
||||||
#mission includes disarm at end so should be ok to download logs now
|
# mission includes disarm at end so should be ok to download logs now
|
||||||
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
|
if not log_download(mavproxy, mav, util.reltopdir("../buildlogs/CopterAVC-log.bin")):
|
||||||
failed_test_msg = "log_download failed"
|
failed_test_msg = "log_download failed"
|
||||||
print(failed_test_msg)
|
print(failed_test_msg)
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
except pexpect.TIMEOUT, failed_test_msg:
|
except pexpect.TIMEOUT as failed_test_msg:
|
||||||
failed_test_msg = "Timeout"
|
failed_test_msg = "Timeout"
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
mav.close()
|
mav.close()
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
valgrind_log = sil.valgrind_log_filepath()
|
valgrind_log = sitl.valgrind_log_filepath()
|
||||||
if os.path.exists(valgrind_log):
|
if os.path.exists(valgrind_log):
|
||||||
os.chmod(valgrind_log, 0644)
|
os.chmod(valgrind_log, 0644)
|
||||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log"))
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/Helicopter-valgrind.log"))
|
||||||
|
|
|
@ -1,21 +1,27 @@
|
||||||
# fly ArduPlane in SIL
|
# Fly ArduPlane in SITL
|
||||||
|
|
||||||
import util, pexpect, sys, time, math, shutil, os
|
import math
|
||||||
from common import *
|
import os
|
||||||
|
import shutil
|
||||||
|
|
||||||
|
import pexpect
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
import random
|
|
||||||
|
from common import *
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
|
|
||||||
HOME_LOCATION='-35.362938,149.165085,585,354'
|
HOME_LOCATION = '-35.362938,149.165085,585,354'
|
||||||
WIND="0,180,0.2" # speed,direction,variance
|
WIND = "0,180,0.2" # speed,direction,variance
|
||||||
|
|
||||||
homeloc = None
|
homeloc = None
|
||||||
|
|
||||||
|
|
||||||
def takeoff(mavproxy, mav):
|
def takeoff(mavproxy, mav):
|
||||||
'''takeoff get to 30m altitude'''
|
"""Takeoff get to 30m altitude."""
|
||||||
|
|
||||||
# wait for EKF and GPS checks to pass
|
# wait for EKF and GPS checks to pass
|
||||||
wait_seconds(mav, 30)
|
wait_seconds(mav, 30)
|
||||||
|
@ -55,8 +61,9 @@ def takeoff(mavproxy, mav):
|
||||||
print("TAKEOFF COMPLETE")
|
print("TAKEOFF COMPLETE")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_left_circuit(mavproxy, mav):
|
def fly_left_circuit(mavproxy, mav):
|
||||||
'''fly a left circuit, 200m on a side'''
|
"""Fly a left circuit, 200m on a side."""
|
||||||
mavproxy.send('switch 4\n')
|
mavproxy.send('switch 4\n')
|
||||||
wait_mode(mav, 'FBWA')
|
wait_mode(mav, 'FBWA')
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
|
@ -65,7 +72,7 @@ def fly_left_circuit(mavproxy, mav):
|
||||||
|
|
||||||
print("Flying left circuit")
|
print("Flying left circuit")
|
||||||
# do 4 turns
|
# do 4 turns
|
||||||
for i in range(0,4):
|
for i in range(0, 4):
|
||||||
# hard left
|
# hard left
|
||||||
print("Starting turn %u" % i)
|
print("Starting turn %u" % i)
|
||||||
mavproxy.send('rc 1 1000\n')
|
mavproxy.send('rc 1 1000\n')
|
||||||
|
@ -78,8 +85,9 @@ def fly_left_circuit(mavproxy, mav):
|
||||||
print("Circuit complete")
|
print("Circuit complete")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_RTL(mavproxy, mav):
|
def fly_RTL(mavproxy, mav):
|
||||||
'''fly to home'''
|
"""Fly to home."""
|
||||||
print("Flying home in RTL")
|
print("Flying home in RTL")
|
||||||
mavproxy.send('switch 2\n')
|
mavproxy.send('switch 2\n')
|
||||||
wait_mode(mav, 'RTL')
|
wait_mode(mav, 'RTL')
|
||||||
|
@ -90,8 +98,9 @@ def fly_RTL(mavproxy, mav):
|
||||||
print("RTL Complete")
|
print("RTL Complete")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_LOITER(mavproxy, mav, num_circles=4):
|
def fly_LOITER(mavproxy, mav, num_circles=4):
|
||||||
'''loiter where we are'''
|
"""Loiter where we are."""
|
||||||
print("Testing LOITER for %u turns" % num_circles)
|
print("Testing LOITER for %u turns" % num_circles)
|
||||||
mavproxy.send('loiter\n')
|
mavproxy.send('loiter\n')
|
||||||
wait_mode(mav, 'LOITER')
|
wait_mode(mav, 'LOITER')
|
||||||
|
@ -122,8 +131,9 @@ def fly_LOITER(mavproxy, mav, num_circles=4):
|
||||||
print("Completed Loiter OK")
|
print("Completed Loiter OK")
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_CIRCLE(mavproxy, mav, num_circles=1):
|
def fly_CIRCLE(mavproxy, mav, num_circles=1):
|
||||||
'''circle where we are'''
|
"""Circle where we are."""
|
||||||
print("Testing CIRCLE for %u turns" % num_circles)
|
print("Testing CIRCLE for %u turns" % num_circles)
|
||||||
mavproxy.send('mode CIRCLE\n')
|
mavproxy.send('mode CIRCLE\n')
|
||||||
wait_mode(mav, 'CIRCLE')
|
wait_mode(mav, 'CIRCLE')
|
||||||
|
@ -156,7 +166,7 @@ def fly_CIRCLE(mavproxy, mav, num_circles=1):
|
||||||
|
|
||||||
|
|
||||||
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
||||||
'''wait for level flight'''
|
"""Wait for level flight."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for level flight")
|
print("Waiting for level flight")
|
||||||
mavproxy.send('rc 1 1500\n')
|
mavproxy.send('rc 1 1500\n')
|
||||||
|
@ -175,7 +185,7 @@ def wait_level_flight(mavproxy, mav, accuracy=5, timeout=30):
|
||||||
|
|
||||||
|
|
||||||
def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
||||||
'''get to a given altitude'''
|
"""Get to a given altitude."""
|
||||||
mavproxy.send('mode FBWA\n')
|
mavproxy.send('mode FBWA\n')
|
||||||
wait_mode(mav, 'FBWA')
|
wait_mode(mav, 'FBWA')
|
||||||
alt_error = mav.messages['VFR_HUD'].alt - altitude
|
alt_error = mav.messages['VFR_HUD'].alt - altitude
|
||||||
|
@ -191,7 +201,7 @@ def change_altitude(mavproxy, mav, altitude, accuracy=30):
|
||||||
|
|
||||||
|
|
||||||
def axial_left_roll(mavproxy, mav, count=1):
|
def axial_left_roll(mavproxy, mav, count=1):
|
||||||
'''fly a left axial roll'''
|
"""Fly a left axial roll."""
|
||||||
# full throttle!
|
# full throttle!
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||||
|
@ -224,7 +234,7 @@ def axial_left_roll(mavproxy, mav, count=1):
|
||||||
|
|
||||||
|
|
||||||
def inside_loop(mavproxy, mav, count=1):
|
def inside_loop(mavproxy, mav, count=1):
|
||||||
'''fly a inside loop'''
|
"""Fly a inside loop."""
|
||||||
# full throttle!
|
# full throttle!
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
if not change_altitude(mavproxy, mav, homeloc.alt+300):
|
||||||
|
@ -252,7 +262,7 @@ def inside_loop(mavproxy, mav, count=1):
|
||||||
|
|
||||||
|
|
||||||
def test_stabilize(mavproxy, mav, count=1):
|
def test_stabilize(mavproxy, mav, count=1):
|
||||||
'''fly stabilize mode'''
|
"""Fly stabilize mode."""
|
||||||
# full throttle!
|
# full throttle!
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
mavproxy.send('rc 2 1300\n')
|
mavproxy.send('rc 2 1300\n')
|
||||||
|
@ -285,8 +295,9 @@ def test_stabilize(mavproxy, mav, count=1):
|
||||||
mavproxy.send('rc 3 1700\n')
|
mavproxy.send('rc 3 1700\n')
|
||||||
return wait_level_flight(mavproxy, mav)
|
return wait_level_flight(mavproxy, mav)
|
||||||
|
|
||||||
|
|
||||||
def test_acro(mavproxy, mav, count=1):
|
def test_acro(mavproxy, mav, count=1):
|
||||||
'''fly ACRO mode'''
|
"""Fly ACRO mode."""
|
||||||
# full throttle!
|
# full throttle!
|
||||||
mavproxy.send('rc 3 2000\n')
|
mavproxy.send('rc 3 2000\n')
|
||||||
mavproxy.send('rc 2 1300\n')
|
mavproxy.send('rc 2 1300\n')
|
||||||
|
@ -339,7 +350,7 @@ def test_acro(mavproxy, mav, count=1):
|
||||||
|
|
||||||
|
|
||||||
def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||||
'''fly FBWB or CRUISE mode'''
|
"""Fly FBWB or CRUISE mode."""
|
||||||
mavproxy.send("mode %s\n" % mode)
|
mavproxy.send("mode %s\n" % mode)
|
||||||
wait_mode(mav, mode)
|
wait_mode(mav, mode)
|
||||||
mavproxy.send('rc 3 1700\n')
|
mavproxy.send('rc 3 1700\n')
|
||||||
|
@ -357,7 +368,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||||
|
|
||||||
print("Flying right circuit")
|
print("Flying right circuit")
|
||||||
# do 4 turns
|
# do 4 turns
|
||||||
for i in range(0,4):
|
for i in range(0, 4):
|
||||||
# hard left
|
# hard left
|
||||||
print("Starting turn %u" % i)
|
print("Starting turn %u" % i)
|
||||||
mavproxy.send('rc 1 1800\n')
|
mavproxy.send('rc 1 1800\n')
|
||||||
|
@ -372,7 +383,7 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||||
|
|
||||||
print("Flying rudder left circuit")
|
print("Flying rudder left circuit")
|
||||||
# do 4 turns
|
# do 4 turns
|
||||||
for i in range(0,4):
|
for i in range(0, 4):
|
||||||
# hard left
|
# hard left
|
||||||
print("Starting turn %u" % i)
|
print("Starting turn %u" % i)
|
||||||
mavproxy.send('rc 4 1900\n')
|
mavproxy.send('rc 4 1900\n')
|
||||||
|
@ -401,22 +412,22 @@ def test_FBWB(mavproxy, mav, count=1, mode='FBWB'):
|
||||||
|
|
||||||
|
|
||||||
def setup_rc(mavproxy):
|
def setup_rc(mavproxy):
|
||||||
'''setup RC override control'''
|
"""Setup RC override control."""
|
||||||
for chan in [1,2,4,5,6,7]:
|
for chan in [1, 2, 4, 5, 6, 7]:
|
||||||
mavproxy.send('rc %u 1500\n' % chan)
|
mavproxy.send('rc %u 1500\n' % chan)
|
||||||
mavproxy.send('rc 3 1000\n')
|
mavproxy.send('rc 3 1000\n')
|
||||||
mavproxy.send('rc 8 1800\n')
|
mavproxy.send('rc 8 1800\n')
|
||||||
|
|
||||||
|
|
||||||
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
|
def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=None):
|
||||||
'''fly a mission from a file'''
|
"""Fly a mission from a file."""
|
||||||
global homeloc
|
global homeloc
|
||||||
print("Flying mission %s" % filename)
|
print("Flying mission %s" % filename)
|
||||||
mavproxy.send('wp load %s\n' % filename)
|
mavproxy.send('wp load %s\n' % filename)
|
||||||
mavproxy.expect('Flight plan received')
|
mavproxy.expect('Flight plan received')
|
||||||
mavproxy.send('wp list\n')
|
mavproxy.send('wp list\n')
|
||||||
mavproxy.expect('Requesting [0-9]+ waypoints')
|
mavproxy.expect('Requesting [0-9]+ waypoints')
|
||||||
mavproxy.send('switch 1\n') # auto mode
|
mavproxy.send('switch 1\n') # auto mode
|
||||||
wait_mode(mav, 'AUTO')
|
wait_mode(mav, 'AUTO')
|
||||||
if not wait_waypoint(mav, 1, 7, max_dist=60):
|
if not wait_waypoint(mav, 1, 7, max_dist=60):
|
||||||
return False
|
return False
|
||||||
|
@ -426,24 +437,24 @@ def fly_mission(mavproxy, mav, filename, height_accuracy=-1, target_altitude=Non
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
def fly_ArduPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||||
'''fly ArduPlane in SIL
|
"""Fly ArduPlane in SITL.
|
||||||
|
|
||||||
you can pass viewerip as an IP address to optionally send fg and
|
you can pass viewerip as an IP address to optionally send fg and
|
||||||
mavproxy packets too for local viewing of the flight in real time
|
mavproxy packets too for local viewing of the flight in real time
|
||||||
'''
|
"""
|
||||||
global homeloc
|
global homeloc
|
||||||
|
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += " --out=%s:14550" % viewerip
|
options += " --out=%s:14550" % viewerip
|
||||||
if map:
|
if use_map:
|
||||||
options += ' --map'
|
options += ' --map'
|
||||||
|
|
||||||
sil = util.start_SIL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10,
|
sitl = util.start_SITL(binary, model='plane-elevrev', home=HOME_LOCATION, speedup=10,
|
||||||
valgrind=valgrind, gdb=gdb,
|
valgrind=valgrind, gdb=gdb,
|
||||||
defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'))
|
defaults_file=os.path.join(testdir, 'default_params/plane-jsbsim.parm'))
|
||||||
mavproxy = util.start_MAVProxy_SIL('ArduPlane', options=options)
|
mavproxy = util.start_MAVProxy_SITL('ArduPlane', options=options)
|
||||||
mavproxy.expect('Telemetry log: (\S+)')
|
mavproxy.expect('Telemetry log: (\S+)')
|
||||||
logfile = mavproxy.match.group(1)
|
logfile = mavproxy.match.group(1)
|
||||||
print("LOGFILE %s" % logfile)
|
print("LOGFILE %s" % logfile)
|
||||||
|
@ -462,14 +473,14 @@ def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
mavproxy.expect('Received [0-9]+ parameters')
|
mavproxy.expect('Received [0-9]+ parameters')
|
||||||
|
|
||||||
expect_list_clear()
|
expect_list_clear()
|
||||||
expect_list_extend([sil, mavproxy])
|
expect_list_extend([sitl, mavproxy])
|
||||||
|
|
||||||
print("Started simulator")
|
print("Started simulator")
|
||||||
|
|
||||||
# get a mavlink connection going
|
# get a mavlink connection going
|
||||||
try:
|
try:
|
||||||
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
||||||
raise
|
raise
|
||||||
mav.message_hooks.append(message_hook)
|
mav.message_hooks.append(message_hook)
|
||||||
|
@ -543,16 +554,16 @@ def fly_ArduPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
print("Failed log download")
|
print("Failed log download")
|
||||||
failed = True
|
failed = True
|
||||||
fail_list.append("log_download")
|
fail_list.append("log_download")
|
||||||
except pexpect.TIMEOUT, e:
|
except pexpect.TIMEOUT as e:
|
||||||
print("Failed with timeout")
|
print("Failed with timeout")
|
||||||
failed = True
|
failed = True
|
||||||
fail_list.append("timeout")
|
fail_list.append("timeout")
|
||||||
|
|
||||||
mav.close()
|
mav.close()
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
valgrind_log = sil.valgrind_log_filepath()
|
valgrind_log = sitl.valgrind_log_filepath()
|
||||||
if os.path.exists(valgrind_log):
|
if os.path.exists(valgrind_log):
|
||||||
os.chmod(valgrind_log, 0644)
|
os.chmod(valgrind_log, 0644)
|
||||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log"))
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/ArduPlane-valgrind.log"))
|
||||||
|
|
|
@ -1,87 +1,104 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# APM automatic test suite
|
"""
|
||||||
# Andrew Tridgell, October 2011
|
APM automatic test suite
|
||||||
|
Andrew Tridgell, October 2011
|
||||||
|
"""
|
||||||
|
|
||||||
import pexpect, os, sys, shutil, atexit
|
import atexit
|
||||||
import optparse, fnmatch, time, glob, traceback, signal
|
import fnmatch
|
||||||
|
import glob
|
||||||
|
import optparse
|
||||||
|
import os
|
||||||
|
import shutil
|
||||||
|
import signal
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
import traceback
|
||||||
|
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim'))
|
import apmrover2
|
||||||
|
import arducopter
|
||||||
import util
|
import arduplane
|
||||||
|
import quadplane
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
os.environ['PYTHONUNBUFFERED'] = '1'
|
os.environ['PYTHONUNBUFFERED'] = '1'
|
||||||
|
|
||||||
os.putenv('TMPDIR', util.reltopdir('tmp'))
|
os.putenv('TMPDIR', util.reltopdir('tmp'))
|
||||||
|
|
||||||
|
|
||||||
def get_default_params(atype, binary):
|
def get_default_params(atype, binary):
|
||||||
'''get default parameters'''
|
"""Get default parameters."""
|
||||||
|
|
||||||
# use rover simulator so SITL is not starved of input
|
# use rover simulator so SITL is not starved of input
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
HOME=mavutil.location(40.071374969556928,-105.22978898137808,1583.702759,246)
|
HOME = mavutil.location(40.071374969556928, -105.22978898137808, 1583.702759, 246)
|
||||||
if binary.find("plane") != -1 or binary.find("rover") != -1:
|
if binary.find("plane") != -1 or binary.find("rover") != -1:
|
||||||
frame = "rover"
|
frame = "rover"
|
||||||
else:
|
else:
|
||||||
frame = "+"
|
frame = "+"
|
||||||
|
|
||||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||||
sil = util.start_SIL(binary, wipe=True, model=frame, home=home, speedup=10)
|
sitl = util.start_SITL(binary, wipe=True, model=frame, home=home, speedup=10)
|
||||||
mavproxy = util.start_MAVProxy_SIL(atype)
|
mavproxy = util.start_MAVProxy_SITL(atype)
|
||||||
print("Dumping defaults")
|
print("Dumping defaults")
|
||||||
idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
|
idx = mavproxy.expect(['Please Run Setup', 'Saved [0-9]+ parameters to (\S+)'])
|
||||||
if idx == 0:
|
if idx == 0:
|
||||||
# we need to restart it after eeprom erase
|
# we need to restart it after eeprom erase
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
sil = util.start_SIL(binary, model=frame, home=home, speedup=10)
|
sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
|
||||||
mavproxy = util.start_MAVProxy_SIL(atype)
|
mavproxy = util.start_MAVProxy_SITL(atype)
|
||||||
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
|
idx = mavproxy.expect('Saved [0-9]+ parameters to (\S+)')
|
||||||
parmfile = mavproxy.match.group(1)
|
parmfile = mavproxy.match.group(1)
|
||||||
dest = util.reltopdir('../buildlogs/%s-defaults.parm' % atype)
|
dest = util.reltopdir('../buildlogs/%s-defaults.parm' % atype)
|
||||||
shutil.copy(parmfile, dest)
|
shutil.copy(parmfile, dest)
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
print("Saved defaults for %s to %s" % (atype, dest))
|
print("Saved defaults for %s to %s" % (atype, dest))
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_all():
|
def build_all():
|
||||||
'''run the build_all.sh script'''
|
"""Run the build_all.sh script."""
|
||||||
print("Running build_all.sh")
|
print("Running build_all.sh")
|
||||||
if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')) != 0:
|
if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), directory=util.reltopdir('.')) != 0:
|
||||||
print("Failed build_all.sh")
|
print("Failed build_all.sh")
|
||||||
return False
|
return False
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_binaries():
|
def build_binaries():
|
||||||
'''run the build_binaries.sh script'''
|
"""Run the build_binaries.sh script."""
|
||||||
print("Running build_binaries.sh")
|
print("Running build_binaries.sh")
|
||||||
import shutil
|
import shutil
|
||||||
# copy the script as it changes git branch, which can change the script while running
|
# copy the script as it changes git branch, which can change the script while running
|
||||||
orig=util.reltopdir('Tools/scripts/build_binaries.sh')
|
orig = util.reltopdir('Tools/scripts/build_binaries.sh')
|
||||||
copy=util.reltopdir('./build_binaries.sh')
|
copy = util.reltopdir('./build_binaries.sh')
|
||||||
shutil.copyfile(orig, copy)
|
shutil.copyfile(orig, copy)
|
||||||
shutil.copymode(orig, copy)
|
shutil.copymode(orig, copy)
|
||||||
if util.run_cmd(copy, dir=util.reltopdir('.')) != 0:
|
if util.run_cmd(copy, directory=util.reltopdir('.')) != 0:
|
||||||
print("Failed build_binaries.sh")
|
print("Failed build_binaries.sh")
|
||||||
return False
|
return False
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_devrelease():
|
def build_devrelease():
|
||||||
'''run the build_devrelease.sh script'''
|
"""Run the build_devrelease.sh script."""
|
||||||
print("Running build_devrelease.sh")
|
print("Running build_devrelease.sh")
|
||||||
import shutil
|
import shutil
|
||||||
# copy the script as it changes git branch, which can change the script while running
|
# copy the script as it changes git branch, which can change the script while running
|
||||||
orig=util.reltopdir('Tools/scripts/build_devrelease.sh')
|
orig = util.reltopdir('Tools/scripts/build_devrelease.sh')
|
||||||
copy=util.reltopdir('./build_devrelease.sh')
|
copy = util.reltopdir('./build_devrelease.sh')
|
||||||
shutil.copyfile(orig, copy)
|
shutil.copyfile(orig, copy)
|
||||||
shutil.copymode(orig, copy)
|
shutil.copymode(orig, copy)
|
||||||
if util.run_cmd(copy, dir=util.reltopdir('.')) != 0:
|
if util.run_cmd(copy, directory=util.reltopdir('.')) != 0:
|
||||||
print("Failed build_devrelease.sh")
|
print("Failed build_devrelease.sh")
|
||||||
return False
|
return False
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_examples():
|
def build_examples():
|
||||||
'''build examples'''
|
"""Build examples."""
|
||||||
for target in 'px4-v2', 'navio':
|
for target in 'px4-v2', 'navio':
|
||||||
print("Running build.examples for %s" % target)
|
print("Running build.examples for %s" % target)
|
||||||
try:
|
try:
|
||||||
|
@ -93,17 +110,18 @@ def build_examples():
|
||||||
|
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_parameters():
|
def build_parameters():
|
||||||
'''run the param_parse.py script'''
|
"""Run the param_parse.py script."""
|
||||||
print("Running param_parse.py")
|
print("Running param_parse.py")
|
||||||
if util.run_cmd(util.reltopdir('Tools/autotest/param_metadata/param_parse.py'), dir=util.reltopdir('.')) != 0:
|
if util.run_cmd(util.reltopdir('Tools/autotest/param_metadata/param_parse.py'), directory=util.reltopdir('.')) != 0:
|
||||||
print("Failed param_parse.py")
|
print("Failed param_parse.py")
|
||||||
return False
|
return False
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def convert_gpx():
|
def convert_gpx():
|
||||||
'''convert any tlog files to GPX and KML'''
|
"""Convert any tlog files to GPX and KML."""
|
||||||
import glob
|
import glob
|
||||||
mavlog = glob.glob(util.reltopdir("../buildlogs/*.tlog"))
|
mavlog = glob.glob(util.reltopdir("../buildlogs/*.tlog"))
|
||||||
for m in mavlog:
|
for m in mavlog:
|
||||||
|
@ -112,18 +130,19 @@ def convert_gpx():
|
||||||
kml = m + '.kml'
|
kml = m + '.kml'
|
||||||
util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1,extrude=1 -F %s' % (gpx, kml), checkfail=False)
|
util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1,extrude=1 -F %s' % (gpx, kml), checkfail=False)
|
||||||
util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False)
|
util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False)
|
||||||
util.run_cmd("mavflightview.py --imagefile=%s.png %s" % (m,m))
|
util.run_cmd("mavflightview.py --imagefile=%s.png %s" % (m, m))
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def test_prerequisites():
|
def test_prerequisites():
|
||||||
'''check we have the right directories and tools to run tests'''
|
"""Check we have the right directories and tools to run tests."""
|
||||||
print("Testing prerequisites")
|
print("Testing prerequisites")
|
||||||
util.mkdir_p(util.reltopdir('../buildlogs'))
|
util.mkdir_p(util.reltopdir('../buildlogs'))
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def alarm_handler(signum, frame):
|
def alarm_handler(signum, frame):
|
||||||
'''handle test timeout'''
|
"""Handle test timeout."""
|
||||||
global results, opts
|
global results, opts
|
||||||
try:
|
try:
|
||||||
results.add('TIMEOUT', '<span class="failed-text">FAILED</span>', opts.timeout)
|
results.add('TIMEOUT', '<span class="failed-text">FAILED</span>', opts.timeout)
|
||||||
|
@ -135,6 +154,7 @@ def alarm_handler(signum, frame):
|
||||||
pass
|
pass
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
|
|
||||||
############## main program #############
|
############## main program #############
|
||||||
parser = optparse.OptionParser("autotest")
|
parser = optparse.OptionParser("autotest")
|
||||||
parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)')
|
parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)')
|
||||||
|
@ -150,13 +170,12 @@ parser.add_option("-j", default=None, type='int', help='build CPUs')
|
||||||
|
|
||||||
opts, args = parser.parse_args()
|
opts, args = parser.parse_args()
|
||||||
|
|
||||||
import arducopter, arduplane, apmrover2, quadplane
|
|
||||||
|
|
||||||
steps = [
|
steps = [
|
||||||
'prerequisites',
|
'prerequisites',
|
||||||
'build.All',
|
'build.All',
|
||||||
'build.Binaries',
|
'build.Binaries',
|
||||||
# 'build.DevRelease',
|
# 'build.DevRelease',
|
||||||
'build.Examples',
|
'build.Examples',
|
||||||
'build.Parameters',
|
'build.Parameters',
|
||||||
|
|
||||||
|
@ -192,13 +211,15 @@ if opts.list:
|
||||||
print(step)
|
print(step)
|
||||||
sys.exit(0)
|
sys.exit(0)
|
||||||
|
|
||||||
|
|
||||||
def skip_step(step):
|
def skip_step(step):
|
||||||
'''see if a step should be skipped'''
|
"""See if a step should be skipped."""
|
||||||
for skip in skipsteps:
|
for skip in skipsteps:
|
||||||
if fnmatch.fnmatch(step.lower(), skip.lower()):
|
if fnmatch.fnmatch(step.lower(), skip.lower()):
|
||||||
return True
|
return True
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def binary_path(step, debug=False):
|
def binary_path(step, debug=False):
|
||||||
if step.find("ArduCopter") != -1:
|
if step.find("ArduCopter") != -1:
|
||||||
binary_name = "arducopter-quad"
|
binary_name = "arducopter-quad"
|
||||||
|
@ -224,14 +245,15 @@ def binary_path(step, debug=False):
|
||||||
binary = util.reltopdir(os.path.join('build', binary_basedir, 'bin', binary_name))
|
binary = util.reltopdir(os.path.join('build', binary_basedir, 'bin', binary_name))
|
||||||
if not os.path.exists(binary):
|
if not os.path.exists(binary):
|
||||||
if os.path.exists(binary + ".exe"):
|
if os.path.exists(binary + ".exe"):
|
||||||
binary_path += ".exe"
|
binary += ".exe"
|
||||||
else:
|
else:
|
||||||
raise ValueError("Binary (%s) does not exist" % (binary,))
|
raise ValueError("Binary (%s) does not exist" % (binary,))
|
||||||
|
|
||||||
return binary
|
return binary
|
||||||
|
|
||||||
|
|
||||||
def run_step(step):
|
def run_step(step):
|
||||||
'''run one step'''
|
"""Run one step."""
|
||||||
|
|
||||||
# remove old logs
|
# remove old logs
|
||||||
util.run_cmd('/bin/rm -f logs/*.BIN logs/LASTLOG.TXT')
|
util.run_cmd('/bin/rm -f logs/*.BIN logs/LASTLOG.TXT')
|
||||||
|
@ -240,19 +262,19 @@ def run_step(step):
|
||||||
return test_prerequisites()
|
return test_prerequisites()
|
||||||
|
|
||||||
if step == 'build.ArduPlane':
|
if step == 'build.ArduPlane':
|
||||||
return util.build_SIL('bin/arduplane', j=opts.j, debug=opts.debug)
|
return util.build_SITL('bin/arduplane', j=opts.j, debug=opts.debug)
|
||||||
|
|
||||||
if step == 'build.APMrover2':
|
if step == 'build.APMrover2':
|
||||||
return util.build_SIL('bin/ardurover', j=opts.j, debug=opts.debug)
|
return util.build_SITL('bin/ardurover', j=opts.j, debug=opts.debug)
|
||||||
|
|
||||||
if step == 'build.ArduCopter':
|
if step == 'build.ArduCopter':
|
||||||
return util.build_SIL('bin/arducopter-quad', j=opts.j, debug=opts.debug)
|
return util.build_SITL('bin/arducopter-quad', j=opts.j, debug=opts.debug)
|
||||||
|
|
||||||
if step == 'build.AntennaTracker':
|
if step == 'build.AntennaTracker':
|
||||||
return util.build_SIL('bin/antennatracker', j=opts.j, debug=opts.debug)
|
return util.build_SITL('bin/antennatracker', j=opts.j, debug=opts.debug)
|
||||||
|
|
||||||
if step == 'build.Helicopter':
|
if step == 'build.Helicopter':
|
||||||
return util.build_SIL('bin/arducopter-heli', j=opts.j, debug=opts.debug)
|
return util.build_SITL('bin/arducopter-heli', j=opts.j, debug=opts.debug)
|
||||||
|
|
||||||
binary = binary_path(step, debug=opts.debug)
|
binary = binary_path(step, debug=opts.debug)
|
||||||
|
|
||||||
|
@ -269,16 +291,16 @@ def run_step(step):
|
||||||
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
return arducopter.fly_ArduCopter(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||||
|
|
||||||
if step == 'fly.CopterAVC':
|
if step == 'fly.CopterAVC':
|
||||||
return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
return arducopter.fly_CopterAVC(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||||
|
|
||||||
if step == 'fly.ArduPlane':
|
if step == 'fly.ArduPlane':
|
||||||
return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
return arduplane.fly_ArduPlane(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||||
|
|
||||||
if step == 'fly.QuadPlane':
|
if step == 'fly.QuadPlane':
|
||||||
return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
return quadplane.fly_QuadPlane(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||||
|
|
||||||
if step == 'drive.APMrover2':
|
if step == 'drive.APMrover2':
|
||||||
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
return apmrover2.drive_APMrover2(binary, viewerip=opts.viewerip, use_map=opts.map, valgrind=opts.valgrind, gdb=opts.gdb)
|
||||||
|
|
||||||
if step == 'build.All':
|
if step == 'build.All':
|
||||||
return build_all()
|
return build_all()
|
||||||
|
@ -300,68 +322,71 @@ def run_step(step):
|
||||||
|
|
||||||
raise RuntimeError("Unknown step %s" % step)
|
raise RuntimeError("Unknown step %s" % step)
|
||||||
|
|
||||||
|
|
||||||
class TestResult(object):
|
class TestResult(object):
|
||||||
'''test result class'''
|
"""Test result class."""
|
||||||
def __init__(self, name, result, elapsed):
|
def __init__(self, name, result, elapsed):
|
||||||
self.name = name
|
self.name = name
|
||||||
self.result = result
|
self.result = result
|
||||||
self.elapsed = "%.1f" % elapsed
|
self.elapsed = "%.1f" % elapsed
|
||||||
|
|
||||||
|
|
||||||
class TestFile(object):
|
class TestFile(object):
|
||||||
'''test result file'''
|
"""Test result file."""
|
||||||
def __init__(self, name, fname):
|
def __init__(self, name, fname):
|
||||||
self.name = name
|
self.name = name
|
||||||
self.fname = fname
|
self.fname = fname
|
||||||
|
|
||||||
|
|
||||||
class TestResults(object):
|
class TestResults(object):
|
||||||
'''test results class'''
|
"""Test results class."""
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.date = time.asctime()
|
self.date = time.asctime()
|
||||||
self.githash = util.run_cmd('git rev-parse HEAD', output=True, dir=util.reltopdir('.')).strip()
|
self.githash = util.run_cmd('git rev-parse HEAD', output=True, directory=util.reltopdir('.')).strip()
|
||||||
self.tests = []
|
self.tests = []
|
||||||
self.files = []
|
self.files = []
|
||||||
self.images = []
|
self.images = []
|
||||||
|
|
||||||
def add(self, name, result, elapsed):
|
def add(self, name, result, elapsed):
|
||||||
'''add a result'''
|
"""Add a result."""
|
||||||
self.tests.append(TestResult(name, result, elapsed))
|
self.tests.append(TestResult(name, result, elapsed))
|
||||||
|
|
||||||
def addfile(self, name, fname):
|
def addfile(self, name, fname):
|
||||||
'''add a result file'''
|
"""Add a result file."""
|
||||||
self.files.append(TestFile(name, fname))
|
self.files.append(TestFile(name, fname))
|
||||||
|
|
||||||
def addimage(self, name, fname):
|
def addimage(self, name, fname):
|
||||||
'''add a result image'''
|
"""Add a result image."""
|
||||||
self.images.append(TestFile(name, fname))
|
self.images.append(TestFile(name, fname))
|
||||||
|
|
||||||
def addglob(self, name, pattern):
|
def addglob(self, name, pattern):
|
||||||
'''add a set of files'''
|
"""Add a set of files."""
|
||||||
import glob
|
import glob
|
||||||
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
|
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
|
||||||
self.addfile(name, os.path.basename(f))
|
self.addfile(name, os.path.basename(f))
|
||||||
|
|
||||||
def addglobimage(self, name, pattern):
|
def addglobimage(self, name, pattern):
|
||||||
'''add a set of images'''
|
"""Add a set of images."""
|
||||||
import glob
|
import glob
|
||||||
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
|
for f in glob.glob(util.reltopdir('../buildlogs/%s' % pattern)):
|
||||||
self.addimage(name, os.path.basename(f))
|
self.addimage(name, os.path.basename(f))
|
||||||
|
|
||||||
|
|
||||||
|
def write_webresults(results_to_write):
|
||||||
def write_webresults(results):
|
"""Write webpage results."""
|
||||||
'''write webpage results'''
|
|
||||||
from pymavlink.generator import mavtemplate
|
from pymavlink.generator import mavtemplate
|
||||||
t = mavtemplate.MAVTemplate()
|
t = mavtemplate.MAVTemplate()
|
||||||
for h in glob.glob(util.reltopdir('Tools/autotest/web/*.html')):
|
for h in glob.glob(util.reltopdir('Tools/autotest/web/*.html')):
|
||||||
html = util.loadfile(h)
|
html = util.loadfile(h)
|
||||||
f = open(util.reltopdir("../buildlogs/%s" % os.path.basename(h)), mode='w')
|
f = open(util.reltopdir("../buildlogs/%s" % os.path.basename(h)), mode='w')
|
||||||
t.write(f, html, results)
|
t.write(f, html, results_to_write)
|
||||||
f.close()
|
f.close()
|
||||||
for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')):
|
for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')):
|
||||||
shutil.copy(f, util.reltopdir('../buildlogs/%s' % os.path.basename(f)))
|
shutil.copy(f, util.reltopdir('../buildlogs/%s' % os.path.basename(f)))
|
||||||
|
|
||||||
|
|
||||||
def write_fullresults():
|
def write_fullresults():
|
||||||
'''write out full results set'''
|
"""Write out full results set."""
|
||||||
global results
|
global results
|
||||||
results.addglob("Google Earth track", '*.kmz')
|
results.addglob("Google Earth track", '*.kmz')
|
||||||
results.addfile('Full Logs', 'autotest-output.txt')
|
results.addfile('Full Logs', 'autotest-output.txt')
|
||||||
|
@ -406,8 +431,9 @@ def write_fullresults():
|
||||||
|
|
||||||
results = TestResults()
|
results = TestResults()
|
||||||
|
|
||||||
|
|
||||||
def check_logs(step):
|
def check_logs(step):
|
||||||
'''check for log files from a step'''
|
"""Check for log files from a step."""
|
||||||
print("check step: ", step)
|
print("check step: ", step)
|
||||||
if step.startswith('fly.'):
|
if step.startswith('fly.'):
|
||||||
vehicle = step[4:]
|
vehicle = step[4:]
|
||||||
|
@ -427,10 +453,11 @@ def check_logs(step):
|
||||||
newname = util.reltopdir("../buildlogs/%s.core" % vehicle)
|
newname = util.reltopdir("../buildlogs/%s.core" % vehicle)
|
||||||
print("Renaming %s to %s" % (corefile, newname))
|
print("Renaming %s to %s" % (corefile, newname))
|
||||||
os.rename(corefile, newname)
|
os.rename(corefile, newname)
|
||||||
util.run_cmd('/bin/cp A*/A*.elf ../buildlogs', dir=util.reltopdir('.'))
|
util.run_cmd('/bin/cp A*/A*.elf ../buildlogs', directory=util.reltopdir('.'))
|
||||||
|
|
||||||
|
|
||||||
def run_tests(steps):
|
def run_tests(steps):
|
||||||
'''run a list of steps'''
|
"""Run a list of steps."""
|
||||||
global results
|
global results
|
||||||
|
|
||||||
passed = True
|
passed = True
|
||||||
|
@ -449,7 +476,7 @@ def run_tests(steps):
|
||||||
failed.append(step)
|
failed.append(step)
|
||||||
results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1)
|
results.add(step, '<span class="failed-text">FAILED</span>', time.time() - t1)
|
||||||
continue
|
continue
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
passed = False
|
passed = False
|
||||||
failed.append(step)
|
failed.append(step)
|
||||||
print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg))
|
print(">>>> FAILED STEP: %s at %s (%s)" % (step, time.asctime(), msg))
|
||||||
|
|
|
@ -1,53 +1,65 @@
|
||||||
import util, pexpect, time, math
|
import math
|
||||||
|
import time
|
||||||
|
|
||||||
from pymavlink import mavwp
|
from pymavlink import mavwp
|
||||||
|
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
# a list of pexpect objects to read while waiting for
|
# a list of pexpect objects to read while waiting for
|
||||||
# messages. This keeps the output to stdout flowing
|
# messages. This keeps the output to stdout flowing
|
||||||
expect_list = []
|
expect_list = []
|
||||||
|
|
||||||
|
|
||||||
def expect_list_clear():
|
def expect_list_clear():
|
||||||
'''clear the expect list'''
|
"""clear the expect list."""
|
||||||
global expect_list
|
global expect_list
|
||||||
for p in expect_list[:]:
|
for p in expect_list[:]:
|
||||||
expect_list.remove(p)
|
expect_list.remove(p)
|
||||||
|
|
||||||
def expect_list_extend(list):
|
|
||||||
'''extend the expect list'''
|
def expect_list_extend(list_to_add):
|
||||||
|
"""Extend the expect list."""
|
||||||
global expect_list
|
global expect_list
|
||||||
expect_list.extend(list)
|
expect_list.extend(list_to_add)
|
||||||
|
|
||||||
|
|
||||||
def idle_hook(mav):
|
def idle_hook(mav):
|
||||||
'''called when waiting for a mavlink message'''
|
"""Called when waiting for a mavlink message."""
|
||||||
global expect_list
|
global expect_list
|
||||||
for p in expect_list:
|
for p in expect_list:
|
||||||
util.pexpect_drain(p)
|
util.pexpect_drain(p)
|
||||||
|
|
||||||
|
|
||||||
def message_hook(mav, msg):
|
def message_hook(mav, msg):
|
||||||
'''called as each mavlink msg is received'''
|
"""Called as each mavlink msg is received."""
|
||||||
idle_hook(mav)
|
idle_hook(mav)
|
||||||
|
|
||||||
|
|
||||||
def expect_callback(e):
|
def expect_callback(e):
|
||||||
'''called when waiting for a expect pattern'''
|
"""Called when waiting for a expect pattern."""
|
||||||
global expect_list
|
global expect_list
|
||||||
for p in expect_list:
|
for p in expect_list:
|
||||||
if p == e:
|
if p == e:
|
||||||
continue
|
continue
|
||||||
util.pexpect_drain(p)
|
util.pexpect_drain(p)
|
||||||
|
|
||||||
|
|
||||||
def get_distance(loc1, loc2):
|
def get_distance(loc1, loc2):
|
||||||
'''get ground distance between two locations'''
|
"""Get ground distance between two locations."""
|
||||||
dlat = loc2.lat - loc1.lat
|
dlat = loc2.lat - loc1.lat
|
||||||
dlong = loc2.lng - loc1.lng
|
dlong = loc2.lng - loc1.lng
|
||||||
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
|
return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
|
||||||
|
|
||||||
|
|
||||||
def get_bearing(loc1, loc2):
|
def get_bearing(loc1, loc2):
|
||||||
'''get bearing from loc1 to loc2'''
|
"""Get bearing from loc1 to loc2."""
|
||||||
off_x = loc2.lng - loc1.lng
|
off_x = loc2.lng - loc1.lng
|
||||||
off_y = loc2.lat - loc1.lat
|
off_y = loc2.lat - loc1.lat
|
||||||
bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
|
bearing = 90.00 + math.atan2(-off_y, off_x) * 57.2957795
|
||||||
if bearing < 0:
|
if bearing < 0:
|
||||||
bearing += 360.00
|
bearing += 360.00
|
||||||
return bearing;
|
return bearing
|
||||||
|
|
||||||
|
|
||||||
def wait_seconds(mav, seconds_to_wait):
|
def wait_seconds(mav, seconds_to_wait):
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
|
@ -55,29 +67,33 @@ def wait_seconds(mav, seconds_to_wait):
|
||||||
while tstart + seconds_to_wait > tnow:
|
while tstart + seconds_to_wait > tnow:
|
||||||
tnow = get_sim_time(mav)
|
tnow = get_sim_time(mav)
|
||||||
|
|
||||||
|
|
||||||
def get_sim_time(mav):
|
def get_sim_time(mav):
|
||||||
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
|
m = mav.recv_match(type='SYSTEM_TIME', blocking=True)
|
||||||
return m.time_boot_ms * 1.0e-3
|
return m.time_boot_ms * 1.0e-3
|
||||||
|
|
||||||
|
|
||||||
def wait_altitude(mav, alt_min, alt_max, timeout=30):
|
def wait_altitude(mav, alt_min, alt_max, timeout=30):
|
||||||
|
"""Wait for a given altitude range."""
|
||||||
climb_rate = 0
|
climb_rate = 0
|
||||||
previous_alt = 0
|
previous_alt = 0
|
||||||
'''wait for a given altitude range'''
|
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
|
print("Waiting for altitude between %u and %u" % (alt_min, alt_max))
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
climb_rate = m.alt - previous_alt
|
climb_rate = m.alt - previous_alt
|
||||||
previous_alt = m.alt
|
previous_alt = m.alt
|
||||||
print("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min , climb_rate))
|
print("Wait Altitude: Cur:%u, min_alt:%u, climb_rate: %u" % (m.alt, alt_min, climb_rate))
|
||||||
if m.alt >= alt_min and m.alt <= alt_max:
|
if m.alt >= alt_min and m.alt <= alt_max:
|
||||||
print("Altitude OK")
|
print("Altitude OK")
|
||||||
return True
|
return True
|
||||||
print("Failed to attain altitude range")
|
print("Failed to attain altitude range")
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
|
def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
|
||||||
'''wait for a given ground speed range'''
|
"""Wait for a given ground speed range."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
|
print("Waiting for groundspeed between %.1f and %.1f" % (gs_min, gs_max))
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
|
@ -90,7 +106,7 @@ def wait_groundspeed(mav, gs_min, gs_max, timeout=30):
|
||||||
|
|
||||||
|
|
||||||
def wait_roll(mav, roll, accuracy, timeout=30):
|
def wait_roll(mav, roll, accuracy, timeout=30):
|
||||||
'''wait for a given roll in degrees'''
|
"""Wait for a given roll in degrees."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for roll of %d at %s" % (roll, time.ctime()))
|
print("Waiting for roll of %d at %s" % (roll, time.ctime()))
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
|
@ -104,8 +120,9 @@ def wait_roll(mav, roll, accuracy, timeout=30):
|
||||||
print("Failed to attain roll %d" % roll)
|
print("Failed to attain roll %d" % roll)
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def wait_pitch(mav, pitch, accuracy, timeout=30):
|
def wait_pitch(mav, pitch, accuracy, timeout=30):
|
||||||
'''wait for a given pitch in degrees'''
|
"""Wait for a given pitch in degrees."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for pitch of %u at %s" % (pitch, time.ctime()))
|
print("Waiting for pitch of %u at %s" % (pitch, time.ctime()))
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
|
@ -120,9 +137,8 @@ def wait_pitch(mav, pitch, accuracy, timeout=30):
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def wait_heading(mav, heading, accuracy=5, timeout=30):
|
def wait_heading(mav, heading, accuracy=5, timeout=30):
|
||||||
'''wait for a given heading'''
|
"""Wait for a given heading."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
|
print("Waiting for heading %u with accuracy %u" % (heading, accuracy))
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
|
@ -136,7 +152,7 @@ def wait_heading(mav, heading, accuracy=5, timeout=30):
|
||||||
|
|
||||||
|
|
||||||
def wait_distance(mav, distance, accuracy=5, timeout=30):
|
def wait_distance(mav, distance, accuracy=5, timeout=30):
|
||||||
'''wait for flight of a given distance'''
|
"""Wait for flight of a given distance."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
start = mav.location()
|
start = mav.location()
|
||||||
while get_sim_time(mav) < tstart + timeout:
|
while get_sim_time(mav) < tstart + timeout:
|
||||||
|
@ -154,7 +170,7 @@ def wait_distance(mav, distance, accuracy=5, timeout=30):
|
||||||
|
|
||||||
|
|
||||||
def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
|
def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height_accuracy=-1):
|
||||||
'''wait for arrival at a location'''
|
"""Wait for arrival at a location."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
if target_altitude is None:
|
if target_altitude is None:
|
||||||
target_altitude = loc.alt
|
target_altitude = loc.alt
|
||||||
|
@ -172,8 +188,9 @@ def wait_location(mav, loc, accuracy=5, timeout=30, target_altitude=None, height
|
||||||
print("Failed to attain location")
|
print("Failed to attain location")
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400):
|
def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, timeout=400):
|
||||||
'''wait for waypoint ranges'''
|
"""Wait for waypoint ranges."""
|
||||||
tstart = get_sim_time(mav)
|
tstart = get_sim_time(mav)
|
||||||
# this message arrives after we set the current WP
|
# this message arrives after we set the current WP
|
||||||
start_wp = mav.waypoint_current()
|
start_wp = mav.waypoint_current()
|
||||||
|
@ -203,7 +220,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
|
||||||
current_wp = seq
|
current_wp = seq
|
||||||
# the wp_dist check is a hack until we can sort out the right seqnum
|
# the wp_dist check is a hack until we can sort out the right seqnum
|
||||||
# for end of mission
|
# for end of mission
|
||||||
#if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
|
# if current_wp == wpnum_end or (current_wp == wpnum_end-1 and wp_dist < 2):
|
||||||
if (current_wp == wpnum_end and wp_dist < max_dist):
|
if (current_wp == wpnum_end and wp_dist < max_dist):
|
||||||
print("Reached final waypoint %u" % seq)
|
print("Reached final waypoint %u" % seq)
|
||||||
return True
|
return True
|
||||||
|
@ -216,6 +233,7 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
|
||||||
print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
|
print("Failed: Timed out waiting for waypoint %u of %u" % (wpnum_end, wpnum_end))
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def save_wp(mavproxy, mav):
|
def save_wp(mavproxy, mav):
|
||||||
mavproxy.send('rc 7 1000\n')
|
mavproxy.send('rc 7 1000\n')
|
||||||
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
||||||
|
@ -227,27 +245,31 @@ def save_wp(mavproxy, mav):
|
||||||
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
mav.recv_match(condition='RC_CHANNELS_RAW.chan7_raw==1000', blocking=True)
|
||||||
wait_seconds(mav, 1)
|
wait_seconds(mav, 1)
|
||||||
|
|
||||||
|
|
||||||
def wait_mode(mav, mode, timeout=None):
|
def wait_mode(mav, mode, timeout=None):
|
||||||
print("Waiting for mode %s" % mode)
|
print("Waiting for mode %s" % mode)
|
||||||
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
|
mav.recv_match(condition='MAV.flightmode.upper()=="%s".upper()' % mode, timeout=timeout, blocking=True)
|
||||||
print("Got mode %s" % mode)
|
print("Got mode %s" % mode)
|
||||||
return mav.flightmode
|
return mav.flightmode
|
||||||
|
|
||||||
|
|
||||||
def mission_count(filename):
|
def mission_count(filename):
|
||||||
'''load a mission from a file and return number of waypoints'''
|
"""Load a mission from a file and return number of waypoints."""
|
||||||
wploader = mavwp.MAVWPLoader()
|
wploader = mavwp.MAVWPLoader()
|
||||||
wploader.load(filename)
|
wploader.load(filename)
|
||||||
num_wp = wploader.count()
|
num_wp = wploader.count()
|
||||||
return num_wp
|
return num_wp
|
||||||
|
|
||||||
|
|
||||||
def sim_location(mav):
|
def sim_location(mav):
|
||||||
'''return current simulator location'''
|
"""Return current simulator location."""
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
m = mav.recv_match(type='SIMSTATE', blocking=True)
|
m = mav.recv_match(type='SIMSTATE', blocking=True)
|
||||||
return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
|
return mavutil.location(m.lat*1.0e-7, m.lng*1.0e-7, 0, math.degrees(m.yaw))
|
||||||
|
|
||||||
|
|
||||||
def log_download(mavproxy, mav, filename, timeout=360):
|
def log_download(mavproxy, mav, filename, timeout=360):
|
||||||
'''download latest log'''
|
"""Download latest log."""
|
||||||
mavproxy.send("log list\n")
|
mavproxy.send("log list\n")
|
||||||
mavproxy.expect("numLogs")
|
mavproxy.expect("numLogs")
|
||||||
mav.wait_heartbeat()
|
mav.wait_heartbeat()
|
||||||
|
@ -258,4 +280,3 @@ def log_download(mavproxy, mav, filename, timeout=360):
|
||||||
mav.wait_heartbeat()
|
mav.wait_heartbeat()
|
||||||
mav.wait_heartbeat()
|
mav.wait_heartbeat()
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
|
@ -1,12 +1,15 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# dump flash logs from SITL
|
"""
|
||||||
# Andrew Tridgell, April 2013
|
dump flash logs from SITL
|
||||||
|
Andrew Tridgell, April 2013
|
||||||
|
"""
|
||||||
|
|
||||||
import pexpect, os, sys, shutil, atexit
|
import optparse
|
||||||
import optparse, fnmatch, time, glob, traceback, signal
|
import os
|
||||||
|
import sys
|
||||||
|
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim'))
|
|
||||||
import util
|
|
||||||
|
|
||||||
############## main program #############
|
############## main program #############
|
||||||
parser = optparse.OptionParser(sys.argv[0])
|
parser = optparse.OptionParser(sys.argv[0])
|
||||||
|
@ -17,13 +20,14 @@ opts, args = parser.parse_args()
|
||||||
|
|
||||||
os.environ['PYTHONUNBUFFERED'] = '1'
|
os.environ['PYTHONUNBUFFERED'] = '1'
|
||||||
|
|
||||||
|
|
||||||
def dump_logs(atype):
|
def dump_logs(atype):
|
||||||
'''dump DataFlash logs'''
|
"""Dump DataFlash logs."""
|
||||||
logfile = '%s.log' % atype
|
logfile = '%s.log' % atype
|
||||||
print("Dumping logs for %s to %s" % (atype, logfile))
|
print("Dumping logs for %s to %s" % (atype, logfile))
|
||||||
sil = util.start_SIL(atype)
|
sitl = util.start_SITL(atype)
|
||||||
log = open(logfile, mode='w')
|
log = open(logfile, mode='w')
|
||||||
mavproxy = util.start_MAVProxy_SIL(atype, setup=True, logfile=log)
|
mavproxy = util.start_MAVProxy_SITL(atype, setup=True, logfile=log)
|
||||||
mavproxy.send('\n\n\n')
|
mavproxy.send('\n\n\n')
|
||||||
print("navigating menus")
|
print("navigating menus")
|
||||||
mavproxy.expect(']')
|
mavproxy.expect(']')
|
||||||
|
@ -48,11 +52,10 @@ def dump_logs(atype):
|
||||||
mavproxy.expect("logs enabled:", timeout=120)
|
mavproxy.expect("logs enabled:", timeout=120)
|
||||||
mavproxy.expect("Log]")
|
mavproxy.expect("Log]")
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
log.close()
|
log.close()
|
||||||
print("Saved log for %s to %s" % (atype, logfile))
|
print("Saved log for %s to %s" % (atype, logfile))
|
||||||
return True
|
return True
|
||||||
|
|
||||||
vehicle = os.path.basename(os.getcwd())
|
vehicle = os.path.basename(os.getcwd())
|
||||||
dump_logs(vehicle)
|
dump_logs(vehicle)
|
||||||
|
|
||||||
|
|
|
@ -1,10 +1,15 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import socket, struct, time, errno
|
import errno
|
||||||
from math import *
|
import socket
|
||||||
|
import struct
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
from math import cos, fabs, radians, sin, sqrt
|
||||||
|
|
||||||
|
|
||||||
class udp_out(object):
|
class udp_out(object):
|
||||||
'''a UDP output socket'''
|
"""A UDP output socket."""
|
||||||
def __init__(self, device):
|
def __init__(self, device):
|
||||||
a = device.split(':')
|
a = device.split(':')
|
||||||
if len(a) != 2:
|
if len(a) != 2:
|
||||||
|
@ -15,11 +20,11 @@ class udp_out(object):
|
||||||
self.port.setblocking(0)
|
self.port.setblocking(0)
|
||||||
self.last_address = None
|
self.last_address = None
|
||||||
|
|
||||||
def recv(self,n=None):
|
def recv(self, n=None):
|
||||||
try:
|
try:
|
||||||
data, self.last_address = self.port.recvfrom(300)
|
data, self.last_address = self.port.recvfrom(300)
|
||||||
except socket.error as e:
|
except socket.error as e:
|
||||||
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
|
if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
|
||||||
return ""
|
return ""
|
||||||
raise
|
raise
|
||||||
return data
|
return data
|
||||||
|
@ -34,12 +39,15 @@ class udp_out(object):
|
||||||
def ft2m(x):
|
def ft2m(x):
|
||||||
return x * 0.3048
|
return x * 0.3048
|
||||||
|
|
||||||
|
|
||||||
def m2ft(x):
|
def m2ft(x):
|
||||||
return x / 0.3048
|
return x / 0.3048
|
||||||
|
|
||||||
|
|
||||||
def kt2mps(x):
|
def kt2mps(x):
|
||||||
return x * 0.514444444
|
return x * 0.514444444
|
||||||
|
|
||||||
|
|
||||||
def mps2kt(x):
|
def mps2kt(x):
|
||||||
return x / 0.514444444
|
return x / 0.514444444
|
||||||
|
|
||||||
|
@ -81,19 +89,19 @@ while True:
|
||||||
xAccel = sin(radians(pitchDeg))
|
xAccel = sin(radians(pitchDeg))
|
||||||
yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg))
|
yAccel = -sin(radians(rollDeg)) * cos(radians(pitchDeg))
|
||||||
zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg))
|
zAccel = -cos(radians(rollDeg)) * cos(radians(pitchDeg))
|
||||||
scale = 9.81 / sqrt((xAccel*xAccel)+(yAccel*yAccel)+(zAccel*zAccel))
|
scale = 9.81 / sqrt((xAccel*xAccel) + (yAccel*yAccel) + (zAccel*zAccel))
|
||||||
xAccel *= scale;
|
xAccel *= scale
|
||||||
yAccel *= scale;
|
yAccel *= scale
|
||||||
zAccel *= scale;
|
zAccel *= scale
|
||||||
|
|
||||||
buf = struct.pack('<17dI',
|
struc_buf = struct.pack('<17dI',
|
||||||
latitude, longitude, altitude, heading,
|
latitude, longitude, altitude, heading,
|
||||||
speedN, speedE, speedD,
|
speedN, speedE, speedD,
|
||||||
xAccel, yAccel, zAccel,
|
xAccel, yAccel, zAccel,
|
||||||
rollRate, pitchRate, yawRate,
|
rollRate, pitchRate, yawRate,
|
||||||
rollDeg, pitchDeg, yawDeg,
|
rollDeg, pitchDeg, yawDeg,
|
||||||
airspeed, magic)
|
airspeed, magic)
|
||||||
udp.write(buf)
|
udp.write(struc_buf)
|
||||||
time.sleep(deltaT)
|
time.sleep(deltaT)
|
||||||
|
|
||||||
yawDeg += yawRate * deltaT
|
yawDeg += yawRate * deltaT
|
||||||
|
|
|
@ -1,14 +1,26 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# run a jsbsim model as a child process
|
"""
|
||||||
|
Run a jsbsim model as a child process.
|
||||||
|
"""
|
||||||
|
|
||||||
import sys, os, pexpect, socket
|
import atexit
|
||||||
import math, time, select, struct, signal, errno
|
import errno
|
||||||
|
import fdpexpect
|
||||||
|
import math
|
||||||
|
import os
|
||||||
|
import select
|
||||||
|
import signal
|
||||||
|
import socket
|
||||||
|
import struct
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pysim'))
|
import pexpect
|
||||||
|
|
||||||
import util, atexit, fdpexpect
|
|
||||||
from pymavlink import fgFDM
|
from pymavlink import fgFDM
|
||||||
|
|
||||||
|
from .. pysim import util
|
||||||
|
|
||||||
|
|
||||||
class control_state(object):
|
class control_state(object):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.aileron = 0
|
self.aileron = 0
|
||||||
|
@ -21,18 +33,20 @@ sitl_state = control_state()
|
||||||
|
|
||||||
|
|
||||||
def interpret_address(addrstr):
|
def interpret_address(addrstr):
|
||||||
'''interpret a IP:port string'''
|
"""Interpret a IP:port string."""
|
||||||
a = addrstr.split(':')
|
a = addrstr.split(':')
|
||||||
a[1] = int(a[1])
|
a[1] = int(a[1])
|
||||||
return tuple(a)
|
return tuple(a)
|
||||||
|
|
||||||
|
|
||||||
def jsb_set(variable, value):
|
def jsb_set(variable, value):
|
||||||
'''set a JSBSim variable'''
|
"""Set a JSBSim variable."""
|
||||||
global jsb_console
|
global jsb_console
|
||||||
jsb_console.send('set %s %s\r\n' % (variable, value))
|
jsb_console.send('set %s %s\r\n' % (variable, value))
|
||||||
|
|
||||||
|
|
||||||
def setup_template(home):
|
def setup_template(home):
|
||||||
'''setup aircraft/Rascal/reset.xml'''
|
"""Setup aircraft/Rascal/reset.xml ."""
|
||||||
global opts
|
global opts
|
||||||
v = home.split(',')
|
v = home.split(',')
|
||||||
if len(v) != 4:
|
if len(v) != 4:
|
||||||
|
@ -45,50 +59,50 @@ def setup_template(home):
|
||||||
sitl_state.ground_height = altitude
|
sitl_state.ground_height = altitude
|
||||||
template = os.path.join('aircraft', 'Rascal', 'reset_template.xml')
|
template = os.path.join('aircraft', 'Rascal', 'reset_template.xml')
|
||||||
reset = os.path.join('aircraft', 'Rascal', 'reset.xml')
|
reset = os.path.join('aircraft', 'Rascal', 'reset.xml')
|
||||||
xml = open(template).read() % { 'LATITUDE' : str(latitude),
|
xml = open(template).read() % {'LATITUDE': str(latitude),
|
||||||
'LONGITUDE' : str(longitude),
|
'LONGITUDE': str(longitude),
|
||||||
'HEADING' : str(heading) }
|
'HEADING': str(heading)}
|
||||||
open(reset, mode='w').write(xml)
|
open(reset, mode='w').write(xml)
|
||||||
print("Wrote %s" % reset)
|
print("Wrote %s" % reset)
|
||||||
|
|
||||||
baseport = int(opts.simout.split(':')[1])
|
baseport = int(opts.simout.split(':')[1])
|
||||||
|
|
||||||
template = os.path.join('jsb_sim', 'fgout_template.xml')
|
template = os.path.join('jsb_sim', 'fgout_template.xml')
|
||||||
out = os.path.join('jsb_sim', 'fgout.xml')
|
out = os.path.join('jsb_sim', 'fgout.xml')
|
||||||
xml = open(template).read() % { 'FGOUTPORT' : str(baseport+3) }
|
xml = open(template).read() % {'FGOUTPORT': str(baseport+3)}
|
||||||
open(out, mode='w').write(xml)
|
open(out, mode='w').write(xml)
|
||||||
print("Wrote %s" % out)
|
print("Wrote %s" % out)
|
||||||
|
|
||||||
template = os.path.join('jsb_sim', 'rascal_test_template.xml')
|
template = os.path.join('jsb_sim', 'rascal_test_template.xml')
|
||||||
out = os.path.join('jsb_sim', 'rascal_test.xml')
|
out = os.path.join('jsb_sim', 'rascal_test.xml')
|
||||||
xml = open(template).read() % { 'JSBCONSOLEPORT' : str(baseport+4) }
|
xml = open(template).read() % {'JSBCONSOLEPORT': str(baseport+4)}
|
||||||
open(out, mode='w').write(xml)
|
open(out, mode='w').write(xml)
|
||||||
print("Wrote %s" % out)
|
print("Wrote %s" % out)
|
||||||
|
|
||||||
|
|
||||||
def process_sitl_input(buf):
|
def process_sitl_input(buf):
|
||||||
'''process control changes from SITL sim'''
|
"""Process control changes from SITL sim."""
|
||||||
control = list(struct.unpack('<14H', buf))
|
control = list(struct.unpack('<14H', buf))
|
||||||
pwm = control[:11]
|
pwm = control[:11]
|
||||||
(speed, direction, turbulance) = control[11:]
|
(speed, direction, turbulance) = control[11:]
|
||||||
|
|
||||||
global wind
|
global wind
|
||||||
wind.speed = speed*0.01
|
wind.speed = speed*0.01
|
||||||
wind.direction = direction*0.01
|
wind.direction = direction*0.01
|
||||||
wind.turbulance = turbulance*0.01
|
wind.turbulance = turbulance*0.01
|
||||||
|
|
||||||
aileron = (pwm[0]-1500)/500.0
|
aileron = (pwm[0]-1500)/500.0
|
||||||
elevator = (pwm[1]-1500)/500.0
|
elevator = (pwm[1]-1500)/500.0
|
||||||
throttle = (pwm[2]-1000)/1000.0
|
throttle = (pwm[2]-1000)/1000.0
|
||||||
if opts.revthr:
|
if opts.revthr:
|
||||||
throttle = 1.0 - throttle
|
throttle = 1.0 - throttle
|
||||||
rudder = (pwm[3]-1500)/500.0
|
rudder = (pwm[3]-1500)/500.0
|
||||||
|
|
||||||
if opts.elevon:
|
if opts.elevon:
|
||||||
# fake an elevon plane
|
# fake an elevon plane
|
||||||
ch1 = aileron
|
ch1 = aileron
|
||||||
ch2 = elevator
|
ch2 = elevator
|
||||||
aileron = (ch2-ch1)/2.0
|
aileron = (ch2-ch1)/2.0
|
||||||
# the minus does away with the need for RC2_REV=-1
|
# the minus does away with the need for RC2_REV=-1
|
||||||
elevator = -(ch2+ch1)/2.0
|
elevator = -(ch2+ch1)/2.0
|
||||||
|
|
||||||
|
@ -98,7 +112,7 @@ def process_sitl_input(buf):
|
||||||
ch2 = rudder
|
ch2 = rudder
|
||||||
# this matches VTAIL_OUTPUT==2
|
# this matches VTAIL_OUTPUT==2
|
||||||
elevator = (ch2-ch1)/2.0
|
elevator = (ch2-ch1)/2.0
|
||||||
rudder = (ch2+ch1)/2.0
|
rudder = (ch2+ch1)/2.0
|
||||||
|
|
||||||
buf = ''
|
buf = ''
|
||||||
if aileron != sitl_state.aileron:
|
if aileron != sitl_state.aileron:
|
||||||
|
@ -117,14 +131,16 @@ def process_sitl_input(buf):
|
||||||
global jsb_console
|
global jsb_console
|
||||||
jsb_console.send(buf)
|
jsb_console.send(buf)
|
||||||
|
|
||||||
|
|
||||||
def update_wind(wind):
|
def update_wind(wind):
|
||||||
'''update wind simulation'''
|
"""Update wind simulation."""
|
||||||
(speed, direction) = wind.current()
|
(speed, direction) = wind.current()
|
||||||
jsb_set('atmosphere/psiw-rad', math.radians(direction))
|
jsb_set('atmosphere/psiw-rad', math.radians(direction))
|
||||||
jsb_set('atmosphere/wind-mag-fps', speed/0.3048)
|
jsb_set('atmosphere/wind-mag-fps', speed/0.3048)
|
||||||
|
|
||||||
|
|
||||||
def process_jsb_input(buf, simtime):
|
def process_jsb_input(buf, simtime):
|
||||||
'''process FG FDM input from JSBSim'''
|
"""Process FG FDM input from JSBSim."""
|
||||||
global fdm, fg_out, sim_out
|
global fdm, fg_out, sim_out
|
||||||
fdm.parse(buf)
|
fdm.parse(buf)
|
||||||
if fg_out:
|
if fg_out:
|
||||||
|
@ -134,7 +150,7 @@ def process_jsb_input(buf, simtime):
|
||||||
fdm.set('rpm', sitl_state.throttle*1000)
|
fdm.set('rpm', sitl_state.throttle*1000)
|
||||||
fg_out.send(fdm.pack())
|
fg_out.send(fdm.pack())
|
||||||
except socket.error as e:
|
except socket.error as e:
|
||||||
if e.errno not in [ errno.ECONNREFUSED ]:
|
if e.errno not in [errno.ECONNREFUSED]:
|
||||||
raise
|
raise
|
||||||
|
|
||||||
timestamp = int(simtime*1.0e6)
|
timestamp = int(simtime*1.0e6)
|
||||||
|
@ -162,13 +178,11 @@ def process_jsb_input(buf, simtime):
|
||||||
try:
|
try:
|
||||||
sim_out.send(simbuf)
|
sim_out.send(simbuf)
|
||||||
except socket.error as e:
|
except socket.error as e:
|
||||||
if e.errno not in [ errno.ECONNREFUSED ]:
|
if e.errno not in [errno.ECONNREFUSED]:
|
||||||
raise
|
raise
|
||||||
|
|
||||||
|
|
||||||
|
################## main program ##################
|
||||||
##################
|
|
||||||
# main program
|
|
||||||
from optparse import OptionParser
|
from optparse import OptionParser
|
||||||
parser = OptionParser("runsim.py [options]")
|
parser = OptionParser("runsim.py [options]")
|
||||||
parser.add_option("--simin", help="SITL input (IP:port)", default="127.0.0.1:5502")
|
parser.add_option("--simin", help="SITL input (IP:port)", default="127.0.0.1:5502")
|
||||||
|
@ -186,7 +200,7 @@ parser.add_option("--speedup", type='float', default=1.0, help="speedup from rea
|
||||||
|
|
||||||
(opts, args) = parser.parse_args()
|
(opts, args) = parser.parse_args()
|
||||||
|
|
||||||
for m in [ 'home', 'script' ]:
|
for m in ['home', 'script']:
|
||||||
if not opts.__dict__[m]:
|
if not opts.__dict__[m]:
|
||||||
print("Missing required option '%s'" % m)
|
print("Missing required option '%s'" % m)
|
||||||
parser.print_help()
|
parser.print_help()
|
||||||
|
@ -233,7 +247,7 @@ jsb_in.setblocking(0)
|
||||||
|
|
||||||
# socket addresses
|
# socket addresses
|
||||||
sim_out_address = interpret_address(opts.simout)
|
sim_out_address = interpret_address(opts.simout)
|
||||||
sim_in_address = interpret_address(opts.simin)
|
sim_in_address = interpret_address(opts.simin)
|
||||||
|
|
||||||
# setup input from SITL sim
|
# setup input from SITL sim
|
||||||
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
sim_in = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
@ -259,15 +273,16 @@ fdm = fgFDM.fgFDM()
|
||||||
|
|
||||||
jsb_console.send('info\n')
|
jsb_console.send('info\n')
|
||||||
jsb_console.send('resume\n')
|
jsb_console.send('resume\n')
|
||||||
jsb.expect(["trim computation time","Trim Results"])
|
jsb.expect(["trim computation time", "Trim Results"])
|
||||||
time.sleep(1.5)
|
time.sleep(1.5)
|
||||||
jsb_console.send('step\n')
|
jsb_console.send('step\n')
|
||||||
jsb_console.logfile = None
|
jsb_console.logfile = None
|
||||||
|
|
||||||
print("Simulator ready to fly")
|
print("Simulator ready to fly")
|
||||||
|
|
||||||
|
|
||||||
def main_loop():
|
def main_loop():
|
||||||
'''run main loop'''
|
"""Run main loop."""
|
||||||
tnow = time.time()
|
tnow = time.time()
|
||||||
last_report = tnow
|
last_report = tnow
|
||||||
last_sim_input = tnow
|
last_sim_input = tnow
|
||||||
|
@ -348,8 +363,9 @@ def main_loop():
|
||||||
|
|
||||||
last_wall_time = now
|
last_wall_time = now
|
||||||
|
|
||||||
|
|
||||||
def exit_handler():
|
def exit_handler():
|
||||||
'''exit the sim'''
|
"""Exit the sim."""
|
||||||
print("running exit handler")
|
print("running exit handler")
|
||||||
signal.signal(signal.SIGINT, signal.SIG_IGN)
|
signal.signal(signal.SIGINT, signal.SIG_IGN)
|
||||||
signal.signal(signal.SIGTERM, signal.SIG_IGN)
|
signal.signal(signal.SIGTERM, signal.SIG_IGN)
|
||||||
|
|
|
@ -1,21 +1,25 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
"""
|
||||||
|
The standard interface emitters must implement
|
||||||
|
"""
|
||||||
|
|
||||||
import re
|
import re
|
||||||
from param import *
|
|
||||||
|
|
||||||
# The standard interface emitters must implement
|
|
||||||
class Emit:
|
class Emit:
|
||||||
|
def __init__(self):
|
||||||
|
pass
|
||||||
|
|
||||||
prog_values_field = re.compile(r"\s*(-?\w+:\w+)+,*")
|
prog_values_field = re.compile(r"\s*(-?\w+:\w+)+,*")
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def start_libraries(self):
|
def start_libraries(self):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def emit(self, g, f):
|
def emit(self, g, f):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def set_annotate_with_vehicle(self, value):
|
def set_annotate_with_vehicle(self, value):
|
||||||
self.annotate_with_vehicle = value
|
self.annotate_with_vehicle = value
|
||||||
|
|
||||||
|
|
|
@ -1,17 +1,20 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
"""
|
||||||
|
Emit docs in a form acceptable to the Ardupilot wordpress docs site
|
||||||
|
"""
|
||||||
|
|
||||||
import re
|
from param import known_param_fields
|
||||||
from param import *
|
|
||||||
from emit import Emit
|
from emit import Emit
|
||||||
import cgi
|
import cgi
|
||||||
|
|
||||||
# Emit docs in a form acceptable to the APM wordpress docs site
|
|
||||||
class HtmlEmit(Emit):
|
class HtmlEmit(Emit):
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
Emit.__init__(self)
|
||||||
html_fname = 'Parameters.html'
|
html_fname = 'Parameters.html'
|
||||||
self.f = open(html_fname, mode='w')
|
self.f = open(html_fname, mode='w')
|
||||||
self.preamble = '''<!-- Dynamically generated list of documented parameters
|
self.preamble = """<!-- Dynamically generated list of documented parameters
|
||||||
This page was generated using Tools/autotest/param_metadata/param_parse.py
|
This page was generated using Tools/autotest/param_metadata/param_parse.py
|
||||||
|
|
||||||
DO NOT EDIT
|
DO NOT EDIT
|
||||||
|
@ -26,7 +29,7 @@ DO NOT EDIT
|
||||||
<!-- add auto-generated table of contents with "Table of Contents Plus" plugin -->
|
<!-- add auto-generated table of contents with "Table of Contents Plus" plugin -->
|
||||||
[toc exclude="Complete Parameter List"]
|
[toc exclude="Complete Parameter List"]
|
||||||
|
|
||||||
'''
|
"""
|
||||||
self.t = ''
|
self.t = ''
|
||||||
|
|
||||||
def escape(self, s):
|
def escape(self, s):
|
||||||
|
@ -46,7 +49,7 @@ DO NOT EDIT
|
||||||
|
|
||||||
def emit(self, g, f):
|
def emit(self, g, f):
|
||||||
tag = '%s Parameters' % g.name
|
tag = '%s Parameters' % g.name
|
||||||
t = '\n\n<h1>%s</h1>\n' % tag
|
t = '\n\n<h1>%s</h1>\n' % tag
|
||||||
|
|
||||||
for param in g.params:
|
for param in g.params:
|
||||||
if not hasattr(param, 'DisplayName') or not hasattr(param, 'Description'):
|
if not hasattr(param, 'DisplayName') or not hasattr(param, 'Description'):
|
||||||
|
@ -54,7 +57,7 @@ DO NOT EDIT
|
||||||
d = param.__dict__
|
d = param.__dict__
|
||||||
tag = '%s (%s)' % (param.DisplayName, param.name)
|
tag = '%s (%s)' % (param.DisplayName, param.name)
|
||||||
t += '\n\n<h2>%s</h2>' % tag
|
t += '\n\n<h2>%s</h2>' % tag
|
||||||
if d.get('User',None) == 'Advanced':
|
if d.get('User', None) == 'Advanced':
|
||||||
t += '<em>Note: This parameter is for advanced users</em><br>'
|
t += '<em>Note: This parameter is for advanced users</em><br>'
|
||||||
t += "\n\n<p>%s</p>\n" % cgi.escape(param.Description)
|
t += "\n\n<p>%s</p>\n" % cgi.escape(param.Description)
|
||||||
t += "<ul>\n"
|
t += "<ul>\n"
|
||||||
|
@ -72,4 +75,3 @@ DO NOT EDIT
|
||||||
t += "<li>%s: %s</li>\n" % (field, cgi.escape(param.__dict__[field]))
|
t += "<li>%s: %s</li>\n" % (field, cgi.escape(param.__dict__[field]))
|
||||||
t += "</ul>\n"
|
t += "</ul>\n"
|
||||||
self.t += t
|
self.t += t
|
||||||
|
|
||||||
|
|
|
@ -5,13 +5,14 @@ class Parameter(object):
|
||||||
|
|
||||||
|
|
||||||
class Vehicle(object):
|
class Vehicle(object):
|
||||||
def __init__ (self, name, path):
|
def __init__(self, name, path):
|
||||||
self.name = name
|
self.name = name
|
||||||
self.path = path
|
self.path = path
|
||||||
self.params = []
|
self.params = []
|
||||||
|
|
||||||
|
|
||||||
class Library(object):
|
class Library(object):
|
||||||
def __init__ (self, name):
|
def __init__(self, name):
|
||||||
self.name = name
|
self.name = name
|
||||||
self.params = []
|
self.params = []
|
||||||
|
|
||||||
|
@ -26,15 +27,15 @@ known_param_fields = [
|
||||||
'RebootRequired',
|
'RebootRequired',
|
||||||
'Bitmask',
|
'Bitmask',
|
||||||
'Volatile',
|
'Volatile',
|
||||||
'ReadOnly'
|
'ReadOnly',
|
||||||
]
|
]
|
||||||
|
|
||||||
required_param_fields = [
|
required_param_fields = [
|
||||||
'Description',
|
'Description',
|
||||||
'DisplayName',
|
'DisplayName',
|
||||||
'User'
|
'User',
|
||||||
]
|
]
|
||||||
|
|
||||||
known_group_fields = [
|
known_group_fields = [
|
||||||
'Path'
|
'Path',
|
||||||
]
|
]
|
||||||
|
|
|
@ -1,14 +1,18 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import os, glob, re, sys
|
import glob
|
||||||
|
import os
|
||||||
|
import re
|
||||||
|
import sys
|
||||||
|
from optparse import OptionParser
|
||||||
|
|
||||||
from param import *
|
from param import (Library, Parameter, Vehicle, known_group_fields,
|
||||||
from wikiemit import WikiEmit
|
known_param_fields, required_param_fields)
|
||||||
from xmlemit import XmlEmit
|
|
||||||
from htmlemit import HtmlEmit
|
from htmlemit import HtmlEmit
|
||||||
from rstemit import RSTEmit
|
from rstemit import RSTEmit
|
||||||
|
from wikiemit import WikiEmit
|
||||||
|
from xmlemit import XmlEmit
|
||||||
|
|
||||||
from optparse import OptionParser
|
|
||||||
parser = OptionParser("param_parse.py [options]")
|
parser = OptionParser("param_parse.py [options]")
|
||||||
parser.add_option("-v", "--verbose", dest='verbose', action='store_true', default=False, help="show debugging output")
|
parser.add_option("-v", "--verbose", dest='verbose', action='store_true', default=False, help="show debugging output")
|
||||||
parser.add_option("--vehicle", default='*', help="Vehicle type to generate for")
|
parser.add_option("--vehicle", default='*', help="Vehicle type to generate for")
|
||||||
|
@ -38,16 +42,18 @@ libraries = []
|
||||||
|
|
||||||
error_count = 0
|
error_count = 0
|
||||||
|
|
||||||
def debug(str):
|
|
||||||
'''debug output if verbose is set'''
|
|
||||||
if opts.verbose:
|
|
||||||
print(str)
|
|
||||||
|
|
||||||
def error(str):
|
def debug(str_to_print):
|
||||||
'''show errors'''
|
"""Debug output if verbose is set."""
|
||||||
|
if opts.verbose:
|
||||||
|
print(str_to_print)
|
||||||
|
|
||||||
|
|
||||||
|
def error(str_to_print):
|
||||||
|
"""Show errors."""
|
||||||
global error_count
|
global error_count
|
||||||
error_count += 1
|
error_count += 1
|
||||||
print(str)
|
print(str_to_print)
|
||||||
|
|
||||||
for vehicle_path in vehicle_paths:
|
for vehicle_path in vehicle_paths:
|
||||||
name = os.path.basename(os.path.dirname(vehicle_path))
|
name = os.path.basename(os.path.dirname(vehicle_path))
|
||||||
|
@ -77,8 +83,6 @@ for vehicle in vehicles:
|
||||||
if not any(l.name == parsed_l.name for parsed_l in libraries):
|
if not any(l.name == parsed_l.name for parsed_l in libraries):
|
||||||
libraries.append(l)
|
libraries.append(l)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
for param_match in param_matches:
|
for param_match in param_matches:
|
||||||
p = Parameter(vehicle.name+":"+param_match[0])
|
p = Parameter(vehicle.name+":"+param_match[0])
|
||||||
debug(p.name + ' ')
|
debug(p.name + ' ')
|
||||||
|
@ -92,10 +96,9 @@ for vehicle in vehicles:
|
||||||
else:
|
else:
|
||||||
error("unknown parameter metadata field '%s'" % field[0])
|
error("unknown parameter metadata field '%s'" % field[0])
|
||||||
for req_field in required_param_fields:
|
for req_field in required_param_fields:
|
||||||
if not req_field in field_list:
|
if req_field not in field_list:
|
||||||
error("missing parameter metadata field '%s' in %s" % (req_field, field_text))
|
error("missing parameter metadata field '%s' in %s" % (req_field, field_text))
|
||||||
|
|
||||||
|
|
||||||
vehicle.params.append(p)
|
vehicle.params.append(p)
|
||||||
|
|
||||||
debug("Processed %u params" % len(vehicle.params))
|
debug("Processed %u params" % len(vehicle.params))
|
||||||
|
@ -161,20 +164,19 @@ for library in libraries:
|
||||||
if (len(rangeValues) != 2):
|
if (len(rangeValues) != 2):
|
||||||
error("Invalid Range values for %s" % (param.name))
|
error("Invalid Range values for %s" % (param.name))
|
||||||
return
|
return
|
||||||
min = rangeValues[0]
|
min_value = rangeValues[0]
|
||||||
max = rangeValues[1]
|
max_value = rangeValues[1]
|
||||||
if not is_number(min):
|
if not is_number(min_value):
|
||||||
error("Min value not number: %s %s" % (param.name, min))
|
error("Min value not number: %s %s" % (param.name, min_value))
|
||||||
return
|
return
|
||||||
if not is_number(max):
|
if not is_number(max_value):
|
||||||
error("Max value not number: %s %s" % (param.name, max))
|
error("Max value not number: %s %s" % (param.name, max_value))
|
||||||
return
|
return
|
||||||
|
|
||||||
for vehicle in vehicles:
|
for vehicle in vehicles:
|
||||||
for param in vehicle.params:
|
for param in vehicle.params:
|
||||||
validate(param)
|
validate(param)
|
||||||
|
|
||||||
|
|
||||||
for library in libraries:
|
for library in libraries:
|
||||||
for param in library.params:
|
for param in library.params:
|
||||||
validate(param)
|
validate(param)
|
||||||
|
|
|
@ -1,27 +1,29 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import re
|
import re
|
||||||
from param import *
|
from param import known_param_fields
|
||||||
from emit import Emit
|
from emit import Emit
|
||||||
import cgi
|
import cgi
|
||||||
|
|
||||||
|
|
||||||
# Emit docs in a RST format
|
# Emit docs in a RST format
|
||||||
class RSTEmit(Emit):
|
class RSTEmit(Emit):
|
||||||
|
|
||||||
def blurb(self):
|
def blurb(self):
|
||||||
return '''This is a complete list of the parameters which can be set (e.g. via the MAVLink protocol) to control vehicle behaviour. They are stored in persistent storage on the vehicle.
|
return """This is a complete list of the parameters which can be set (e.g. via the MAVLink protocol) to control vehicle behaviour. They are stored in persistent storage on the vehicle.
|
||||||
|
|
||||||
This list is automatically generated from the latest ardupilot source code, and so may contain parameters which are not yet in the stable released versions of the code.
|
This list is automatically generated from the latest ardupilot source code, and so may contain parameters which are not yet in the stable released versions of the code.
|
||||||
'''
|
"""
|
||||||
|
|
||||||
def toolname(self):
|
def toolname(self):
|
||||||
return "Tools/autotest/param_metadata/param_parse.py"
|
return "Tools/autotest/param_metadata/param_parse.py"
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
Emit.__init__(self)
|
||||||
output_fname = 'Parameters.rst'
|
output_fname = 'Parameters.rst'
|
||||||
self.f = open(output_fname, mode='w')
|
self.f = open(output_fname, mode='w')
|
||||||
self.spacer = re.compile("^", re.MULTILINE)
|
self.spacer = re.compile("^", re.MULTILINE)
|
||||||
self.rstescape = re.compile("([^a-zA-Z0-9\n ])")
|
self.rstescape = re.compile("([^a-zA-Z0-9\n ])")
|
||||||
self.preamble = '''.. Dynamically generated list of documented parameters
|
self.preamble = """.. Dynamically generated list of documented parameters
|
||||||
.. This page was generated using {toolname}
|
.. This page was generated using {toolname}
|
||||||
|
|
||||||
.. DO NOT EDIT
|
.. DO NOT EDIT
|
||||||
|
@ -34,7 +36,7 @@ Complete Parameter List
|
||||||
|
|
||||||
{blurb}
|
{blurb}
|
||||||
|
|
||||||
'''.format(blurb=self.escape(self.blurb()),
|
""".format(blurb=self.escape(self.blurb()),
|
||||||
toolname=self.escape(self.toolname()))
|
toolname=self.escape(self.toolname()))
|
||||||
self.t = ''
|
self.t = ''
|
||||||
|
|
||||||
|
@ -47,7 +49,6 @@ Complete Parameter List
|
||||||
self.f.write(self.t)
|
self.f.write(self.t)
|
||||||
self.f.close()
|
self.f.close()
|
||||||
|
|
||||||
|
|
||||||
def start_libraries(self):
|
def start_libraries(self):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
@ -55,34 +56,34 @@ Complete Parameter List
|
||||||
ret = ""
|
ret = ""
|
||||||
joiner = "|"
|
joiner = "|"
|
||||||
|
|
||||||
row_lines = [ x.split("\n") for x in row ]
|
row_lines = [x.split("\n") for x in row]
|
||||||
for row_line in row_lines:
|
for row_line in row_lines:
|
||||||
row_line.extend([""] * (height-len(row_line)))
|
row_line.extend([""] * (height - len(row_line)))
|
||||||
if rowheading is not None:
|
if rowheading is not None:
|
||||||
rowheading_lines = rowheading.split("\n")
|
rowheading_lines = rowheading.split("\n")
|
||||||
rowheading_lines.extend([""] * (height-len(rowheading_lines)))
|
rowheading_lines.extend([""] * (height - len(rowheading_lines)))
|
||||||
|
|
||||||
out_lines = []
|
out_lines = []
|
||||||
for i in range(0,height):
|
for i in range(0, height):
|
||||||
out_line = ""
|
out_line = ""
|
||||||
if rowheading is not None:
|
if rowheading is not None:
|
||||||
rowheading_line = rowheading_lines[i]
|
rowheading_line = rowheading_lines[i]
|
||||||
out_line += joiner + " " + rowheading_line + " "*(widths[0]-len(rowheading_line)-1)
|
out_line += joiner + " " + rowheading_line + " " * (widths[0] - len(rowheading_line) - 1)
|
||||||
joiner = "#"
|
joiner = "#"
|
||||||
j=0
|
j = 0
|
||||||
for item in row_lines:
|
for item in row_lines:
|
||||||
widthnum = j
|
widthnum = j
|
||||||
if rowheading is not None:
|
if rowheading is not None:
|
||||||
widthnum += 1
|
widthnum += 1
|
||||||
line = item[i]
|
line = item[i]
|
||||||
out_line += joiner + " " + line + " "*(widths[widthnum]-len(line)-1)
|
out_line += joiner + " " + line + " " * (widths[widthnum] - len(line) - 1)
|
||||||
joiner = "|"
|
joiner = "|"
|
||||||
j += 1
|
j += 1
|
||||||
out_line += "|"
|
out_line += "|"
|
||||||
out_lines.append(out_line)
|
out_lines.append(out_line)
|
||||||
return "\n".join(out_lines)
|
return "\n".join(out_lines)
|
||||||
|
|
||||||
def tablify_longest_row_length(self, rows, rowheadings,headings):
|
def tablify_longest_row_length(self, rows, rowheadings, headings):
|
||||||
check_width_rows = rows[:]
|
check_width_rows = rows[:]
|
||||||
if headings is not None:
|
if headings is not None:
|
||||||
check_width_rows.append(headings)
|
check_width_rows.append(headings)
|
||||||
|
@ -109,7 +110,7 @@ Complete Parameter List
|
||||||
|
|
||||||
heights = [0] * len(rows_to_check)
|
heights = [0] * len(rows_to_check)
|
||||||
|
|
||||||
longest_row_length = self.tablify_longest_row_length(rows,rowheadings,headings)
|
longest_row_length = self.tablify_longest_row_length(rows, rowheadings, headings)
|
||||||
widths = [0] * longest_row_length
|
widths = [0] * longest_row_length
|
||||||
|
|
||||||
all_rowheadings = []
|
all_rowheadings = []
|
||||||
|
@ -118,7 +119,7 @@ Complete Parameter List
|
||||||
all_rowheadings.append("")
|
all_rowheadings.append("")
|
||||||
all_rowheadings.extend(rowheadings)
|
all_rowheadings.extend(rowheadings)
|
||||||
|
|
||||||
for rownum in range(0,len(rows_to_check)):
|
for rownum in range(0, len(rows_to_check)):
|
||||||
row = rows_to_check[rownum]
|
row = rows_to_check[rownum]
|
||||||
values_to_check = []
|
values_to_check = []
|
||||||
if rowheadings is not None:
|
if rowheadings is not None:
|
||||||
|
@ -130,15 +131,15 @@ Complete Parameter List
|
||||||
if height > heights[rownum]:
|
if height > heights[rownum]:
|
||||||
heights[rownum] = height
|
heights[rownum] = height
|
||||||
longest_line = self.longest_line_in_string(value)
|
longest_line = self.longest_line_in_string(value)
|
||||||
width = longest_line + 2 # +2 for leading/trailing ws
|
width = longest_line + 2 # +2 for leading/trailing ws
|
||||||
if width > widths[colnum]:
|
if width > widths[colnum]:
|
||||||
widths[colnum] = width
|
widths[colnum] = width
|
||||||
colnum += 1
|
colnum += 1
|
||||||
return (widths,heights)
|
return (widths, heights)
|
||||||
|
|
||||||
def tablify(self, rows, headings=None, rowheadings=None):
|
def tablify(self, rows, headings=None, rowheadings=None):
|
||||||
|
|
||||||
(widths,heights) = self.tablify_calc_row_widths_heights(rows, rowheadings, headings)
|
(widths, heights) = self.tablify_calc_row_widths_heights(rows, rowheadings, headings)
|
||||||
|
|
||||||
# create dividing lines
|
# create dividing lines
|
||||||
bar = ""
|
bar = ""
|
||||||
|
@ -159,7 +160,7 @@ Complete Parameter List
|
||||||
rowheading = ""
|
rowheading = ""
|
||||||
ret += self.tablify_row(rowheading, headings, widths, heights[0]) + "\n"
|
ret += self.tablify_row(rowheading, headings, widths, heights[0]) + "\n"
|
||||||
ret += heading_bar + "\n"
|
ret += heading_bar + "\n"
|
||||||
for i in range(0,len(rows)):
|
for i in range(0, len(rows)):
|
||||||
rowheading = None
|
rowheading = None
|
||||||
height = i
|
height = i
|
||||||
if rowheadings is not None:
|
if rowheadings is not None:
|
||||||
|
@ -171,13 +172,11 @@ Complete Parameter List
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def render_prog_values_field(self, render_info, param, field):
|
def render_prog_values_field(self, render_info, param, field):
|
||||||
values = (param.__dict__[field]).split(',')
|
values = (param.__dict__[field]).split(',')
|
||||||
rows = []
|
rows = []
|
||||||
for value in values:
|
for value in values:
|
||||||
v = [ x.strip() for x in value.split(':') ]
|
v = [x.strip() for x in value.split(':')]
|
||||||
rows.append(v)
|
rows.append(v)
|
||||||
return self.tablify(rows, headings=render_info["headings"])
|
return self.tablify(rows, headings=render_info["headings"])
|
||||||
|
|
||||||
|
@ -187,20 +186,20 @@ Complete Parameter List
|
||||||
|
|
||||||
field_table_info = {
|
field_table_info = {
|
||||||
"Values": {
|
"Values": {
|
||||||
"headings": ['Value','Meaning']
|
"headings": ['Value', 'Meaning'],
|
||||||
},
|
},
|
||||||
"Bitmask": {
|
"Bitmask": {
|
||||||
"headings": ['Bit', 'Meaning']
|
"headings": ['Bit', 'Meaning'],
|
||||||
}
|
},
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = '''
|
ret = """
|
||||||
|
|
||||||
.. _{reference}:
|
.. _{reference}:
|
||||||
|
|
||||||
{tag}
|
{tag}
|
||||||
{underline}
|
{underline}
|
||||||
'''.format(tag=tag,underline="-" * len(tag),
|
""".format(tag=tag, underline="-" * len(tag),
|
||||||
reference=reference)
|
reference=reference)
|
||||||
|
|
||||||
for param in g.params:
|
for param in g.params:
|
||||||
|
@ -211,7 +210,7 @@ Complete Parameter List
|
||||||
name = param.name
|
name = param.name
|
||||||
else:
|
else:
|
||||||
name = param.name.split(':')[-1]
|
name = param.name.split(':')[-1]
|
||||||
tag = '%s: %s' % (self.escape(name), self.escape(param.DisplayName), )
|
tag = '%s: %s' % (self.escape(name), self.escape(param.DisplayName),)
|
||||||
tag = tag.strip()
|
tag = tag.strip()
|
||||||
reference = param.name
|
reference = param.name
|
||||||
# remove e.g. "ArduPlane:" from start of parameter name:
|
# remove e.g. "ArduPlane:" from start of parameter name:
|
||||||
|
@ -220,15 +219,15 @@ Complete Parameter List
|
||||||
else:
|
else:
|
||||||
reference = reference.split(":")[-1]
|
reference = reference.split(":")[-1]
|
||||||
|
|
||||||
ret += '''
|
ret += """
|
||||||
|
|
||||||
.. _{reference}:
|
.. _{reference}:
|
||||||
|
|
||||||
{tag}
|
{tag}
|
||||||
{tag_underline}
|
{tag_underline}
|
||||||
'''.format(tag=tag, tag_underline='~'*len(tag), reference=reference)
|
""".format(tag=tag, tag_underline='~' * len(tag), reference=reference)
|
||||||
|
|
||||||
if d.get('User',None) == 'Advanced':
|
if d.get('User', None) == 'Advanced':
|
||||||
ret += '\n| *Note: This parameter is for advanced users*'
|
ret += '\n| *Note: This parameter is for advanced users*'
|
||||||
ret += "\n\n%s\n" % self.escape(param.Description)
|
ret += "\n\n%s\n" % self.escape(param.Description)
|
||||||
|
|
||||||
|
@ -240,30 +239,32 @@ Complete Parameter List
|
||||||
if field in field_table_info and Emit.prog_values_field.match(param.__dict__[field]):
|
if field in field_table_info and Emit.prog_values_field.match(param.__dict__[field]):
|
||||||
row.append(self.render_prog_values_field(field_table_info[field], param, field))
|
row.append(self.render_prog_values_field(field_table_info[field], param, field))
|
||||||
elif field == "Range":
|
elif field == "Range":
|
||||||
(min,max) = (param.__dict__[field]).split(' ')
|
(param_min, param_max) = (param.__dict__[field]).split(' ')
|
||||||
row.append("%s - %s" % (min,max,))
|
row.append("%s - %s" % (param_min, param_max,))
|
||||||
else:
|
else:
|
||||||
row.append(cgi.escape(param.__dict__[field]))
|
row.append(cgi.escape(param.__dict__[field]))
|
||||||
if len(row):
|
if len(row):
|
||||||
ret += "\n\n" + self.tablify([row], headings=headings) + "\n\n"
|
ret += "\n\n" + self.tablify([row], headings=headings) + "\n\n"
|
||||||
self.t += ret + "\n"
|
self.t += ret + "\n"
|
||||||
|
|
||||||
|
|
||||||
def table_test():
|
def table_test():
|
||||||
e = RSTEmit()
|
e = RSTEmit()
|
||||||
print("Test 1")
|
print("Test 1")
|
||||||
print e.tablify([["A","B"],["C","D"]])
|
print(e.tablify([["A", "B"], ["C", "D"]]))
|
||||||
|
|
||||||
print("Test 2")
|
print("Test 2")
|
||||||
print e.tablify([["A","B"],["CD\nE","FG"]])
|
print e.tablify([["A", "B"], ["CD\nE", "FG"]])
|
||||||
|
|
||||||
print("Test 3")
|
print("Test 3")
|
||||||
print e.tablify([["A","B"],["CD\nEF","GH"]], rowheadings=["r1","row2"])
|
print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], rowheadings=["r1", "row2"]))
|
||||||
|
|
||||||
print("Test 4")
|
print("Test 4")
|
||||||
print e.tablify([["A","B"],["CD\nEF","GH"]], headings=["c1","col2"])
|
print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], headings=["c1", "col2"]))
|
||||||
|
|
||||||
print("Test 5")
|
print("Test 5")
|
||||||
print e.tablify([["A","B"],["CD\nEF","GH"]], headings=["c1","col2"], rowheadings=["r1","row2"])
|
print(e.tablify([["A", "B"], ["CD\nEF", "GH"]], headings=["c1", "col2"], rowheadings=["r1", "row2"]))
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
table_test()
|
table_test()
|
||||||
|
|
|
@ -1,13 +1,15 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import re
|
import re
|
||||||
from param import *
|
|
||||||
from emit import Emit
|
from emit import Emit
|
||||||
|
from param import known_param_fields
|
||||||
|
|
||||||
|
|
||||||
# Emit docs in a form acceptable to the APM wiki site
|
# Emit docs in a form acceptable to the APM wiki site
|
||||||
class WikiEmit(Emit):
|
class WikiEmit(Emit):
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
Emit.__init__(self)
|
||||||
wiki_fname = 'Parameters.wiki'
|
wiki_fname = 'Parameters.wiki'
|
||||||
self.f = open(wiki_fname, mode='w')
|
self.f = open(wiki_fname, mode='w')
|
||||||
preamble = '''#summary Dynamically generated list of documented parameters
|
preamble = '''#summary Dynamically generated list of documented parameters
|
||||||
|
@ -19,17 +21,17 @@ class WikiEmit(Emit):
|
||||||
self.f.write(preamble)
|
self.f.write(preamble)
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
self.f.close
|
self.f.close()
|
||||||
|
|
||||||
def camelcase_escape(self, word):
|
def camelcase_escape(self, word):
|
||||||
if re.match(r"([A-Z][a-z]+[A-Z][a-z]*)", word.strip()):
|
if re.match(r"([A-Z][a-z]+[A-Z][a-z]*)", word.strip()):
|
||||||
return "!"+word
|
return "!" + word
|
||||||
else:
|
else:
|
||||||
return word
|
return word
|
||||||
|
|
||||||
def wikichars_escape(self, text):
|
def wikichars_escape(self, text):
|
||||||
for c in "*,{,},[,],_,=,#,^,~,!,@,$,|,<,>,&,|,\,/".split(','):
|
for c in "*,{,},[,],_,=,#,^,~,!,@,$,|,<,>,&,|,\,/".split(','):
|
||||||
text = re.sub("\\"+c, '`'+c+'`', text)
|
text = re.sub("\\" + c, '`' + c + '`', text)
|
||||||
return text
|
return text
|
||||||
|
|
||||||
def emit_comment(self, s):
|
def emit_comment(self, s):
|
||||||
|
@ -39,12 +41,11 @@ class WikiEmit(Emit):
|
||||||
self.emit_comment("Libraries")
|
self.emit_comment("Libraries")
|
||||||
|
|
||||||
def emit(self, g, f):
|
def emit(self, g, f):
|
||||||
|
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
|
||||||
t = "\n\n== %s Parameters ==\n" % (self.camelcase_escape(g.name))
|
|
||||||
|
|
||||||
for param in g.params:
|
for param in g.params:
|
||||||
if hasattr(param, 'DisplayName'):
|
if hasattr(param, 'DisplayName'):
|
||||||
t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName),self.camelcase_escape(param.name))
|
t += "\n\n=== %s (%s) ===" % (self.camelcase_escape(param.DisplayName), self.camelcase_escape(param.name))
|
||||||
else:
|
else:
|
||||||
t += "\n\n=== %s ===" % self.camelcase_escape(param.name)
|
t += "\n\n=== %s ===" % self.camelcase_escape(param.name)
|
||||||
|
|
||||||
|
@ -56,17 +57,14 @@ class WikiEmit(Emit):
|
||||||
for field in param.__dict__.keys():
|
for field in param.__dict__.keys():
|
||||||
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
|
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
|
||||||
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
|
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
|
||||||
t+= " * Values \n"
|
t += " * Values \n"
|
||||||
values = (param.__dict__[field]).split(',')
|
values = (param.__dict__[field]).split(',')
|
||||||
t+="|| *Value* || *Meaning* ||\n"
|
t += "|| *Value* || *Meaning* ||\n"
|
||||||
for value in values:
|
for value in values:
|
||||||
v = value.split(':')
|
v = value.split(':')
|
||||||
t+="|| "+v[0]+" || "+self.camelcase_escape(v[1])+" ||\n"
|
t += "|| " + v[0] + " || " + self.camelcase_escape(v[1]) + " ||\n"
|
||||||
else:
|
else:
|
||||||
t += " * %s: %s\n" % (self.camelcase_escape(field), self.wikichars_escape(param.__dict__[field]))
|
t += " * %s: %s\n" % (self.camelcase_escape(field), self.wikichars_escape(param.__dict__[field]))
|
||||||
|
|
||||||
#print t
|
# print t
|
||||||
self.f.write(t)
|
self.f.write(t)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -2,13 +2,14 @@
|
||||||
|
|
||||||
from xml.sax.saxutils import escape, quoteattr
|
from xml.sax.saxutils import escape, quoteattr
|
||||||
|
|
||||||
from param import *
|
|
||||||
from emit import Emit
|
from emit import Emit
|
||||||
|
from param import known_param_fields
|
||||||
|
|
||||||
|
|
||||||
# Emit APM documentation in an machine readable XML format
|
# Emit APM documentation in an machine readable XML format
|
||||||
class XmlEmit(Emit):
|
class XmlEmit(Emit):
|
||||||
|
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
|
Emit.__init__(self)
|
||||||
wiki_fname = 'apm.pdef.xml'
|
wiki_fname = 'apm.pdef.xml'
|
||||||
self.f = open(wiki_fname, mode='w')
|
self.f = open(wiki_fname, mode='w')
|
||||||
preamble = '''<?xml version="1.0" encoding="utf-8"?>
|
preamble = '''<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
@ -21,7 +22,7 @@ class XmlEmit(Emit):
|
||||||
def close(self):
|
def close(self):
|
||||||
self.f.write('</libraries>')
|
self.f.write('</libraries>')
|
||||||
self.f.write('''</paramfile>\n''')
|
self.f.write('''</paramfile>\n''')
|
||||||
self.f.close
|
self.f.close()
|
||||||
|
|
||||||
def emit_comment(self, s):
|
def emit_comment(self, s):
|
||||||
self.f.write("<!-- " + s + " -->")
|
self.f.write("<!-- " + s + " -->")
|
||||||
|
@ -31,19 +32,19 @@ class XmlEmit(Emit):
|
||||||
self.f.write('<libraries>')
|
self.f.write('<libraries>')
|
||||||
|
|
||||||
def emit(self, g, f):
|
def emit(self, g, f):
|
||||||
t = '''<parameters name=%s>\n''' % quoteattr(g.name) # i.e. ArduPlane
|
t = '''<parameters name=%s>\n''' % quoteattr(g.name) # i.e. ArduPlane
|
||||||
|
|
||||||
for param in g.params:
|
for param in g.params:
|
||||||
# Begin our parameter node
|
# Begin our parameter node
|
||||||
if hasattr(param, 'DisplayName'):
|
if hasattr(param, 'DisplayName'):
|
||||||
t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName),quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM)
|
t += '<param humanName=%s name=%s' % (quoteattr(param.DisplayName), quoteattr(param.name)) # i.e. ArduPlane (ArduPlane:FOOPARM)
|
||||||
else:
|
else:
|
||||||
t += '<param name=%s' % quoteattr(param.name)
|
t += '<param name=%s' % quoteattr(param.name)
|
||||||
|
|
||||||
if hasattr(param, 'Description'):
|
if hasattr(param, 'Description'):
|
||||||
t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs
|
t += ' documentation=%s' % quoteattr(param.Description) # i.e. parameter docs
|
||||||
if hasattr(param, 'User'):
|
if hasattr(param, 'User'):
|
||||||
t += ' user=%s' % quoteattr(param.User) # i.e. Standard or Advanced
|
t += ' user=%s' % quoteattr(param.User) # i.e. Standard or Advanced
|
||||||
|
|
||||||
t += ">\n"
|
t += ">\n"
|
||||||
|
|
||||||
|
@ -51,22 +52,19 @@ class XmlEmit(Emit):
|
||||||
for field in param.__dict__.keys():
|
for field in param.__dict__.keys():
|
||||||
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
|
if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields:
|
||||||
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
|
if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]):
|
||||||
t+= "<values>\n"
|
t += "<values>\n"
|
||||||
|
|
||||||
values = (param.__dict__[field]).split(',')
|
values = (param.__dict__[field]).split(',')
|
||||||
for value in values:
|
for value in values:
|
||||||
v = value.split(':')
|
v = value.split(':')
|
||||||
t+='''<value code=%s>%s</value>\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label
|
t += '''<value code=%s>%s</value>\n''' % (quoteattr(v[0]), escape(v[1])) # i.e. numeric value, string label
|
||||||
|
|
||||||
t += "</values>\n"
|
t += "</values>\n"
|
||||||
else:
|
else:
|
||||||
t += '''<field name=%s>%s</field>\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10
|
t += '''<field name=%s>%s</field>\n''' % (quoteattr(field), escape(param.__dict__[field])) # i.e. Range: 0 10
|
||||||
|
|
||||||
t += '''</param>\n'''
|
t += '''</param>\n'''
|
||||||
t += '''</parameters>\n'''
|
t += '''</parameters>\n'''
|
||||||
|
|
||||||
#print t
|
# print t
|
||||||
self.f.write(t)
|
self.f.write(t)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
"""pysim tools"""
|
|
@ -1,9 +1,13 @@
|
||||||
import math, util, rotmat, time
|
import math
|
||||||
import random
|
import random
|
||||||
|
import time
|
||||||
|
import util
|
||||||
|
|
||||||
from rotmat import Vector3, Matrix3
|
from rotmat import Vector3, Matrix3
|
||||||
|
|
||||||
|
|
||||||
class Aircraft(object):
|
class Aircraft(object):
|
||||||
'''a basic aircraft class'''
|
"""A basic aircraft class."""
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.home_latitude = 0
|
self.home_latitude = 0
|
||||||
self.home_longitude = 0
|
self.home_longitude = 0
|
||||||
|
@ -11,20 +15,20 @@ class Aircraft(object):
|
||||||
self.ground_level = 0
|
self.ground_level = 0
|
||||||
self.frame_height = 0.0
|
self.frame_height = 0.0
|
||||||
|
|
||||||
self.latitude = self.home_latitude
|
self.latitude = self.home_latitude
|
||||||
self.longitude = self.home_longitude
|
self.longitude = self.home_longitude
|
||||||
self.altitude = self.home_altitude
|
self.altitude = self.home_altitude
|
||||||
|
|
||||||
self.dcm = Matrix3()
|
self.dcm = Matrix3()
|
||||||
|
|
||||||
# rotation rate in body frame
|
# rotation rate in body frame
|
||||||
self.gyro = Vector3(0,0,0) # rad/s
|
self.gyro = Vector3(0, 0, 0) # rad/s
|
||||||
|
|
||||||
self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
|
self.velocity = Vector3(0, 0, 0) # m/s, North, East, Down
|
||||||
self.position = Vector3(0, 0, 0) # m North, East, Down
|
self.position = Vector3(0, 0, 0) # m North, East, Down
|
||||||
self.mass = 0.0
|
self.mass = 0.0
|
||||||
self.update_frequency = 50 # in Hz
|
self.update_frequency = 50 # in Hz
|
||||||
self.gravity = 9.80665 # m/s/s
|
self.gravity = 9.80665 # m/s/s
|
||||||
self.accelerometer = Vector3(0, 0, -self.gravity)
|
self.accelerometer = Vector3(0, 0, -self.gravity)
|
||||||
|
|
||||||
self.wind = util.Wind('0,0,0')
|
self.wind = util.Wind('0,0,0')
|
||||||
|
@ -35,13 +39,13 @@ class Aircraft(object):
|
||||||
self.accel_noise = 0.3
|
self.accel_noise = 0.3
|
||||||
|
|
||||||
def on_ground(self, position=None):
|
def on_ground(self, position=None):
|
||||||
'''return true if we are on the ground'''
|
"""Return true if we are on the ground."""
|
||||||
if position is None:
|
if position is None:
|
||||||
position = self.position
|
position = self.position
|
||||||
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
|
return (-position.z) + self.home_altitude <= self.ground_level + self.frame_height
|
||||||
|
|
||||||
def update_position(self):
|
def update_position(self):
|
||||||
'''update lat/lon/alt from position'''
|
"""Update lat/lon/alt from position."""
|
||||||
|
|
||||||
bearing = math.degrees(math.atan2(self.position.y, self.position.x))
|
bearing = math.degrees(math.atan2(self.position.y, self.position.x))
|
||||||
distance = math.sqrt(self.position.x**2 + self.position.y**2)
|
distance = math.sqrt(self.position.x**2 + self.position.y**2)
|
||||||
|
@ -49,24 +53,24 @@ class Aircraft(object):
|
||||||
(self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
|
(self.latitude, self.longitude) = util.gps_newpos(self.home_latitude, self.home_longitude,
|
||||||
bearing, distance)
|
bearing, distance)
|
||||||
|
|
||||||
self.altitude = self.home_altitude - self.position.z
|
self.altitude = self.home_altitude - self.position.z
|
||||||
|
|
||||||
velocity_body = self.dcm.transposed() * self.velocity
|
velocity_body = self.dcm.transposed() * self.velocity
|
||||||
|
|
||||||
self.accelerometer = self.accel_body.copy()
|
self.accelerometer = self.accel_body.copy()
|
||||||
|
|
||||||
def set_yaw_degrees(self, yaw_degrees):
|
def set_yaw_degrees(self, yaw_degrees):
|
||||||
'''rotate to the given yaw'''
|
"""Rotate to the given yaw."""
|
||||||
(roll, pitch, yaw) = self.dcm.to_euler()
|
(roll, pitch, yaw) = self.dcm.to_euler()
|
||||||
yaw = math.radians(yaw_degrees)
|
yaw = math.radians(yaw_degrees)
|
||||||
self.dcm.from_euler(roll, pitch, yaw)
|
self.dcm.from_euler(roll, pitch, yaw)
|
||||||
|
|
||||||
def time_advance(self, deltat):
|
def time_advance(self, deltat):
|
||||||
'''advance time by deltat in seconds'''
|
"""Advance time by deltat in seconds."""
|
||||||
self.time_now += deltat
|
self.time_now += deltat
|
||||||
|
|
||||||
def setup_frame_time(self, rate, speedup):
|
def setup_frame_time(self, rate, speedup):
|
||||||
'''setup frame_time calculation'''
|
"""Setup frame_time calculation."""
|
||||||
self.rate = rate
|
self.rate = rate
|
||||||
self.speedup = speedup
|
self.speedup = speedup
|
||||||
self.frame_time = 1.0/rate
|
self.frame_time = 1.0/rate
|
||||||
|
@ -75,14 +79,14 @@ class Aircraft(object):
|
||||||
self.achieved_rate = rate
|
self.achieved_rate = rate
|
||||||
|
|
||||||
def adjust_frame_time(self, rate):
|
def adjust_frame_time(self, rate):
|
||||||
'''adjust frame_time calculation'''
|
"""Adjust frame_time calculation."""
|
||||||
self.rate = rate
|
self.rate = rate
|
||||||
self.frame_time = 1.0/rate
|
self.frame_time = 1.0/rate
|
||||||
self.scaled_frame_time = self.frame_time/self.speedup
|
self.scaled_frame_time = self.frame_time/self.speedup
|
||||||
|
|
||||||
def sync_frame_time(self):
|
def sync_frame_time(self):
|
||||||
'''try to synchronise simulation time with wall clock time, taking
|
"""Try to synchronise simulation time with wall clock time, taking
|
||||||
into account desired speedup'''
|
into account desired speedup."""
|
||||||
now = time.time()
|
now = time.time()
|
||||||
if now < self.last_wall_time + self.scaled_frame_time:
|
if now < self.last_wall_time + self.scaled_frame_time:
|
||||||
time.sleep(self.last_wall_time+self.scaled_frame_time - now)
|
time.sleep(self.last_wall_time+self.scaled_frame_time - now)
|
||||||
|
@ -99,13 +103,10 @@ class Aircraft(object):
|
||||||
self.last_wall_time = now
|
self.last_wall_time = now
|
||||||
|
|
||||||
def add_noise(self, throttle):
|
def add_noise(self, throttle):
|
||||||
'''add noise based on throttle level (from 0..1)'''
|
"""Add noise based on throttle level (from 0..1)."""
|
||||||
self.gyro += Vector3(random.gauss(0, 1),
|
self.gyro += Vector3(random.gauss(0, 1),
|
||||||
random.gauss(0, 1),
|
random.gauss(0, 1),
|
||||||
random.gauss(0, 1)) * throttle * self.gyro_noise
|
random.gauss(0, 1)) * throttle * self.gyro_noise
|
||||||
self.accel_body += Vector3(random.gauss(0, 1),
|
self.accel_body += Vector3(random.gauss(0, 1),
|
||||||
random.gauss(0, 1),
|
random.gauss(0, 1),
|
||||||
random.gauss(0, 1)) * throttle * self.accel_noise
|
random.gauss(0, 1)) * throttle * self.accel_noise
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -4,36 +4,38 @@ So you are responsible for opening and close the file descriptor.
|
||||||
$Id: fdpexpect.py 505 2007-12-26 21:33:50Z noah $
|
$Id: fdpexpect.py 505 2007-12-26 21:33:50Z noah $
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from pexpect import *
|
|
||||||
import os
|
import os
|
||||||
|
|
||||||
|
from pexpect import ExceptionPexpect, spawn
|
||||||
|
|
||||||
__all__ = ['fdspawn']
|
__all__ = ['fdspawn']
|
||||||
|
|
||||||
class fdspawn (spawn):
|
|
||||||
|
|
||||||
|
class fdspawn(spawn):
|
||||||
"""This is like pexpect.spawn but allows you to supply your own open file
|
"""This is like pexpect.spawn but allows you to supply your own open file
|
||||||
descriptor. For example, you could use it to read through a file looking
|
descriptor. For example, you could use it to read through a file looking
|
||||||
for patterns, or to control a modem or serial device. """
|
for patterns, or to control a modem or serial device. """
|
||||||
|
|
||||||
def __init__ (self, fd, args=[], timeout=30, maxread=2000, searchwindowsize=None, logfile=None):
|
def __init__(self, fd, args=[], timeout=30, maxread=2000, searchwindowsize=None, logfile=None):
|
||||||
|
|
||||||
"""This takes a file descriptor (an int) or an object that support the
|
"""This takes a file descriptor (an int) or an object that support the
|
||||||
fileno() method (returning an int). All Python file-like objects
|
fileno() method (returning an int). All Python file-like objects
|
||||||
support fileno(). """
|
support fileno(). """
|
||||||
|
|
||||||
### TODO: Add better handling of trying to use fdspawn in place of spawn
|
# TODO: Add better handling of trying to use fdspawn in place of spawn
|
||||||
### TODO: (overload to allow fdspawn to also handle commands as spawn does.
|
# TODO: (overload to allow fdspawn to also handle commands as spawn does.
|
||||||
|
|
||||||
if type(fd) != type(0) and hasattr(fd, 'fileno'):
|
if not isinstance(fd, int) and hasattr(fd, 'fileno'):
|
||||||
fd = fd.fileno()
|
fd = fd.fileno()
|
||||||
|
|
||||||
if type(fd) != type(0):
|
if not isinstance(fd, int):
|
||||||
raise ExceptionPexpect ('The fd argument is not an int. If this is a command string then maybe you want to use pexpect.spawn.')
|
raise ExceptionPexpect(
|
||||||
|
"The fd argument is not an int. If this is a command string then maybe you want to use pexpect.spawn.")
|
||||||
|
|
||||||
try: # make sure fd is a valid file descriptor
|
try: # make sure fd is a valid file descriptor
|
||||||
os.fstat(fd)
|
os.fstat(fd)
|
||||||
except OSError:
|
except OSError:
|
||||||
raise ExceptionPexpect, 'The fd argument is not a valid file descriptor.'
|
raise ExceptionPexpect("The fd argument is not a valid file descriptor.")
|
||||||
|
|
||||||
self.args = None
|
self.args = None
|
||||||
self.command = None
|
self.command = None
|
||||||
|
@ -43,23 +45,23 @@ class fdspawn (spawn):
|
||||||
self.closed = False
|
self.closed = False
|
||||||
self.name = '<file descriptor %d>' % fd
|
self.name = '<file descriptor %d>' % fd
|
||||||
|
|
||||||
def __del__ (self):
|
def __del__(self):
|
||||||
|
|
||||||
return
|
return
|
||||||
|
|
||||||
def close (self):
|
def close(self):
|
||||||
|
|
||||||
if self.child_fd == -1:
|
if self.child_fd == -1:
|
||||||
return
|
return
|
||||||
if self.own_fd:
|
if self.own_fd:
|
||||||
self.close (self)
|
self.close(self)
|
||||||
else:
|
else:
|
||||||
self.flush()
|
self.flush()
|
||||||
os.close(self.child_fd)
|
os.close(self.child_fd)
|
||||||
self.child_fd = -1
|
self.child_fd = -1
|
||||||
self.closed = True
|
self.closed = True
|
||||||
|
|
||||||
def isalive (self):
|
def isalive(self):
|
||||||
|
|
||||||
"""This checks if the file descriptor is still valid. If os.fstat()
|
"""This checks if the file descriptor is still valid. If os.fstat()
|
||||||
does not raise an exception then we assume it is alive. """
|
does not raise an exception then we assume it is alive. """
|
||||||
|
@ -72,11 +74,10 @@ class fdspawn (spawn):
|
||||||
except:
|
except:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
def terminate (self, force=False):
|
def terminate(self, force=False):
|
||||||
|
|
||||||
raise ExceptionPexpect ('This method is not valid for file descriptors.')
|
raise ExceptionPexpect('This method is not valid for file descriptors.')
|
||||||
|
|
||||||
def kill (self, sig):
|
def kill(self, sig):
|
||||||
|
|
||||||
return
|
return
|
||||||
|
|
||||||
|
|
|
@ -1,17 +1,22 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
|
|
||||||
import socket, struct, time, math, errno
|
import errno
|
||||||
|
import socket
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
from pymavlink import fgFDM
|
from pymavlink import fgFDM
|
||||||
|
|
||||||
|
|
||||||
class udp_socket(object):
|
class udp_socket(object):
|
||||||
'''a UDP socket'''
|
"""A UDP socket."""
|
||||||
def __init__(self, device, blocking=True, input=True):
|
def __init__(self, device, blocking=True, is_input=True):
|
||||||
a = device.split(':')
|
a = device.split(':')
|
||||||
if len(a) != 2:
|
if len(a) != 2:
|
||||||
print("UDP ports must be specified as host:port")
|
print("UDP ports must be specified as host:port")
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
if input:
|
if is_input:
|
||||||
self.port.bind((a[0], int(a[1])))
|
self.port.bind((a[0], int(a[1])))
|
||||||
self.destination_addr = None
|
self.destination_addr = None
|
||||||
else:
|
else:
|
||||||
|
@ -20,11 +25,11 @@ class udp_socket(object):
|
||||||
self.port.setblocking(0)
|
self.port.setblocking(0)
|
||||||
self.last_address = None
|
self.last_address = None
|
||||||
|
|
||||||
def recv(self,n=1000):
|
def recv(self, n=1000):
|
||||||
try:
|
try:
|
||||||
data, self.last_address = self.port.recvfrom(n)
|
data, self.last_address = self.port.recvfrom(n)
|
||||||
except socket.error as e:
|
except socket.error as e:
|
||||||
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
|
if e.errno in [errno.EAGAIN, errno.EWOULDBLOCK]:
|
||||||
return ""
|
return ""
|
||||||
raise
|
raise
|
||||||
return data
|
return data
|
||||||
|
@ -39,21 +44,23 @@ class udp_socket(object):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def ft2m(x):
|
def ft2m(x):
|
||||||
return x * 0.3048
|
return x * 0.3048
|
||||||
|
|
||||||
|
|
||||||
def m2ft(x):
|
def m2ft(x):
|
||||||
return x / 0.3048
|
return x / 0.3048
|
||||||
|
|
||||||
|
|
||||||
def kt2mps(x):
|
def kt2mps(x):
|
||||||
return x * 0.514444444
|
return x * 0.514444444
|
||||||
|
|
||||||
|
|
||||||
def mps2kt(x):
|
def mps2kt(x):
|
||||||
return x / 0.514444444
|
return x / 0.514444444
|
||||||
|
|
||||||
udp = udp_socket("127.0.0.1:5123")
|
udp = udp_socket("127.0.0.1:5123")
|
||||||
fgout = udp_socket("127.0.0.1:5124", input=False)
|
fgout = udp_socket("127.0.0.1:5124", is_input=False)
|
||||||
|
|
||||||
tlast = time.time()
|
tlast = time.time()
|
||||||
count = 0
|
count = 0
|
||||||
|
@ -61,12 +68,12 @@ count = 0
|
||||||
fg = fgFDM.fgFDM()
|
fg = fgFDM.fgFDM()
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
buf = udp.recv(1000)
|
udp_buffer = udp.recv(1000)
|
||||||
fg.parse(buf)
|
fg.parse(udp_buffer)
|
||||||
fgout.write(fg.pack())
|
fgout.write(fg.pack())
|
||||||
count += 1
|
count += 1
|
||||||
if time.time() - tlast > 1.0:
|
if time.time() - tlast > 1.0:
|
||||||
print("%u FPS len=%u" % (count, len(buf)))
|
print("%u FPS len=%u" % (count, len(udp_buffer)))
|
||||||
count = 0
|
count = 0
|
||||||
tlast = time.time()
|
tlast = time.time()
|
||||||
print(fg.get('latitude', units='degrees'),
|
print(fg.get('latitude', units='degrees'),
|
||||||
|
|
|
@ -1,23 +1,23 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
'''
|
"""
|
||||||
Python interface to euroc ROS multirotor simulator
|
Python interface to euroc ROS multirotor simulator
|
||||||
See https://pixhawk.org/dev/ros/sitl
|
See https://pixhawk.org/dev/ros/sitl
|
||||||
'''
|
"""
|
||||||
|
|
||||||
|
import time
|
||||||
|
|
||||||
|
import mav_msgs.msg as mav_msgs
|
||||||
|
import px4.msg as px4
|
||||||
|
import rosgraph_msgs.msg as rosgraph_msgs
|
||||||
|
import rospy
|
||||||
|
import sensor_msgs.msg as sensor_msgs
|
||||||
|
|
||||||
from aircraft import Aircraft
|
from aircraft import Aircraft
|
||||||
import util, time, math
|
|
||||||
from math import degrees, radians
|
|
||||||
from rotmat import Vector3, Matrix3
|
from rotmat import Vector3, Matrix3
|
||||||
from pymavlink.quaternion import Quaternion
|
|
||||||
import rospy
|
|
||||||
import std_msgs.msg as std_msgs
|
|
||||||
import mav_msgs.msg as mav_msgs
|
|
||||||
import rosgraph_msgs.msg as rosgraph_msgs
|
|
||||||
import sensor_msgs.msg as sensor_msgs
|
|
||||||
import px4.msg as px4
|
|
||||||
|
|
||||||
def quat_to_dcm(q1, q2, q3, q4):
|
def quat_to_dcm(q1, q2, q3, q4):
|
||||||
'''convert quaternion to DCM'''
|
"""Convert quaternion to DCM."""
|
||||||
q3q3 = q3 * q3
|
q3q3 = q3 * q3
|
||||||
q3q4 = q3 * q4
|
q3q4 = q3 * q4
|
||||||
q2q2 = q2 * q2
|
q2q2 = q2 * q2
|
||||||
|
@ -33,7 +33,7 @@ def quat_to_dcm(q1, q2, q3, q4):
|
||||||
m.a.y = 2.0*(q2q3 - q1q4)
|
m.a.y = 2.0*(q2q3 - q1q4)
|
||||||
m.a.z = 2.0*(q2q4 + q1q3)
|
m.a.z = 2.0*(q2q4 + q1q3)
|
||||||
m.b.x = 2.0*(q2q3 + q1q4)
|
m.b.x = 2.0*(q2q3 + q1q4)
|
||||||
m.b.y = 1.0-2.0*(q2q2 + q4q4)
|
m.b.y = 1.0-2.0*(q2q2 + q4q4)
|
||||||
m.b.z = 2.0*(q3q4 - q1q2)
|
m.b.z = 2.0*(q3q4 - q1q2)
|
||||||
m.c.x = 2.0*(q2q4 - q1q3)
|
m.c.x = 2.0*(q2q4 - q1q3)
|
||||||
m.c.y = 2.0*(q3q4 + q1q2)
|
m.c.y = 2.0*(q3q4 + q1q2)
|
||||||
|
@ -42,7 +42,7 @@ def quat_to_dcm(q1, q2, q3, q4):
|
||||||
|
|
||||||
|
|
||||||
class IrisRos(Aircraft):
|
class IrisRos(Aircraft):
|
||||||
'''a IRIS MultiCopter from ROS'''
|
"""A IRIS MultiCopter from ROS."""
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
Aircraft.__init__(self)
|
Aircraft.__init__(self)
|
||||||
self.max_rpm = 1200
|
self.max_rpm = 1200
|
||||||
|
@ -51,8 +51,8 @@ class IrisRos(Aircraft):
|
||||||
self.have_new_pos = False
|
self.have_new_pos = False
|
||||||
|
|
||||||
topics = {
|
topics = {
|
||||||
"/clock" : (self.clock_cb, rosgraph_msgs.Clock),
|
"/clock" : (self.clock_cb, rosgraph_msgs.Clock),
|
||||||
"/iris/imu" : (self.imu_cb, sensor_msgs.Imu),
|
"/iris/imu" : (self.imu_cb, sensor_msgs.Imu),
|
||||||
"/iris/vehicle_local_position" : (self.pos_cb, px4.vehicle_local_position),
|
"/iris/vehicle_local_position" : (self.pos_cb, px4.vehicle_local_position),
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -67,7 +67,7 @@ class IrisRos(Aircraft):
|
||||||
self.last_time = 0
|
self.last_time = 0
|
||||||
|
|
||||||
# spin() simply keeps python from exiting until this node is stopped
|
# spin() simply keeps python from exiting until this node is stopped
|
||||||
#rospy.spin()
|
# rospy.spin()
|
||||||
|
|
||||||
def clock_cb(self, msg):
|
def clock_cb(self, msg):
|
||||||
self.time_now = self.time_base + msg.clock.secs + msg.clock.nsecs*1.0e-9
|
self.time_now = self.time_base + msg.clock.secs + msg.clock.nsecs*1.0e-9
|
||||||
|
|
|
@ -1,42 +1,43 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
#
|
"""
|
||||||
# vector3 and rotation matrix classes
|
vector3 and rotation matrix classes
|
||||||
# This follows the conventions in the ArduPilot code,
|
This follows the conventions in the ArduPilot code,
|
||||||
# and is essentially a python version of the AP_Math library
|
and is essentially a python version of the AP_Math library
|
||||||
#
|
|
||||||
# Andrew Tridgell, March 2012
|
|
||||||
#
|
|
||||||
# This library is free software; you can redistribute it and/or modify it
|
|
||||||
# under the terms of the GNU Lesser General Public License as published by the
|
|
||||||
# Free Software Foundation; either version 2.1 of the License, or (at your
|
|
||||||
# option) any later version.
|
|
||||||
#
|
|
||||||
# This library is distributed in the hope that it will be useful, but WITHOUT
|
|
||||||
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
||||||
# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
|
|
||||||
# for more details.
|
|
||||||
#
|
|
||||||
# You should have received a copy of the GNU Lesser General Public License
|
|
||||||
# along with this library; if not, write to the Free Software Foundation,
|
|
||||||
# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
|
||||||
|
|
||||||
'''rotation matrix class
|
Andrew Tridgell, March 2012
|
||||||
'''
|
|
||||||
|
This library is free software; you can redistribute it and/or modify it
|
||||||
|
under the terms of the GNU Lesser General Public License as published by the
|
||||||
|
Free Software Foundation; either version 2.1 of the License, or (at your
|
||||||
|
option) any later version.
|
||||||
|
|
||||||
|
This library is distributed in the hope that it will be useful, but WITHOUT
|
||||||
|
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
|
||||||
|
for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU Lesser General Public License
|
||||||
|
along with this library; if not, write to the Free Software Foundation,
|
||||||
|
Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||||
|
|
||||||
|
"""
|
||||||
|
|
||||||
|
from math import acos, asin, atan2, cos, pi, radians, sin, sqrt
|
||||||
|
|
||||||
from math import sin, cos, sqrt, asin, atan2, pi, radians, acos
|
|
||||||
|
|
||||||
class Vector3:
|
class Vector3:
|
||||||
'''a vector'''
|
"""A vector."""
|
||||||
|
|
||||||
def __init__(self, x=None, y=None, z=None):
|
def __init__(self, x=None, y=None, z=None):
|
||||||
if x != None and y != None and z != None:
|
if x is not None and y is not None and z is not None:
|
||||||
self.x = float(x)
|
self.x = float(x)
|
||||||
self.y = float(y)
|
self.y = float(y)
|
||||||
self.z = float(z)
|
self.z = float(z)
|
||||||
elif x != None and len(x) == 3:
|
elif x is not None and len(x) == 3:
|
||||||
self.x = float(x[0])
|
self.x = float(x[0])
|
||||||
self.y = float(x[1])
|
self.y = float(x[1])
|
||||||
self.z = float(x[2])
|
self.z = float(x[2])
|
||||||
elif x != None:
|
elif x is not None:
|
||||||
raise ValueError('bad initialiser')
|
raise ValueError('bad initialiser')
|
||||||
else:
|
else:
|
||||||
self.x = float(0)
|
self.x = float(0)
|
||||||
|
@ -70,8 +71,8 @@ class Vector3:
|
||||||
|
|
||||||
def __mul__(self, v):
|
def __mul__(self, v):
|
||||||
if isinstance(v, Vector3):
|
if isinstance(v, Vector3):
|
||||||
'''dot product'''
|
"""dot product"""
|
||||||
return self.x*v.x + self.y*v.y + self.z*v.z
|
return self.x * v.x + self.y * v.y + self.z * v.z
|
||||||
return Vector3(self.x * v,
|
return Vector3(self.x * v,
|
||||||
self.y * v,
|
self.y * v,
|
||||||
self.z * v)
|
self.z * v)
|
||||||
|
@ -84,10 +85,10 @@ class Vector3:
|
||||||
self.z / v)
|
self.z / v)
|
||||||
|
|
||||||
def __mod__(self, v):
|
def __mod__(self, v):
|
||||||
'''cross product'''
|
"""Cross product."""
|
||||||
return Vector3(self.y*v.z - self.z*v.y,
|
return Vector3(self.y * v.z - self.z * v.y,
|
||||||
self.z*v.x - self.x*v.z,
|
self.z * v.x - self.x * v.z,
|
||||||
self.x*v.y - self.y*v.x)
|
self.x * v.y - self.y * v.x)
|
||||||
|
|
||||||
def __copy__(self):
|
def __copy__(self):
|
||||||
return Vector3(self.x, self.y, self.z)
|
return Vector3(self.x, self.y, self.z)
|
||||||
|
@ -95,13 +96,13 @@ class Vector3:
|
||||||
copy = __copy__
|
copy = __copy__
|
||||||
|
|
||||||
def length(self):
|
def length(self):
|
||||||
return sqrt(self.x**2 + self.y**2 + self.z**2)
|
return sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
|
||||||
|
|
||||||
def zero(self):
|
def zero(self):
|
||||||
self.x = self.y = self.z = 0
|
self.x = self.y = self.z = 0
|
||||||
|
|
||||||
def angle(self, v):
|
def angle(self, v):
|
||||||
'''return the angle between this vector and another vector'''
|
"""Return the angle between this vector and another vector."""
|
||||||
return acos(self * v) / (self.length() * v.length())
|
return acos(self * v) / (self.length() * v.length())
|
||||||
|
|
||||||
def normalized(self):
|
def normalized(self):
|
||||||
|
@ -113,8 +114,10 @@ class Vector3:
|
||||||
self.y = v.y
|
self.y = v.y
|
||||||
self.z = v.z
|
self.z = v.z
|
||||||
|
|
||||||
|
|
||||||
class Matrix3:
|
class Matrix3:
|
||||||
'''a 3x3 matrix, intended as a rotation matrix'''
|
"""A 3x3 matrix, intended as a rotation matrix."""
|
||||||
|
|
||||||
def __init__(self, a=None, b=None, c=None):
|
def __init__(self, a=None, b=None, c=None):
|
||||||
if a is not None and b is not None and c is not None:
|
if a is not None and b is not None and c is not None:
|
||||||
self.a = a.copy()
|
self.a = a.copy()
|
||||||
|
@ -130,18 +133,17 @@ class Matrix3:
|
||||||
self.c.x, self.c.y, self.c.z)
|
self.c.x, self.c.y, self.c.z)
|
||||||
|
|
||||||
def identity(self):
|
def identity(self):
|
||||||
self.a = Vector3(1,0,0)
|
self.a = Vector3(1, 0, 0)
|
||||||
self.b = Vector3(0,1,0)
|
self.b = Vector3(0, 1, 0)
|
||||||
self.c = Vector3(0,0,1)
|
self.c = Vector3(0, 0, 1)
|
||||||
|
|
||||||
def transposed(self):
|
def transposed(self):
|
||||||
return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
|
return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
|
||||||
Vector3(self.a.y, self.b.y, self.c.y),
|
Vector3(self.a.y, self.b.y, self.c.y),
|
||||||
Vector3(self.a.z, self.b.z, self.c.z))
|
Vector3(self.a.z, self.b.z, self.c.z))
|
||||||
|
|
||||||
|
|
||||||
def from_euler(self, roll, pitch, yaw):
|
def from_euler(self, roll, pitch, yaw):
|
||||||
'''fill the matrix from Euler angles in radians'''
|
"""Fill the matrix from Euler angles in radians."""
|
||||||
cp = cos(pitch)
|
cp = cos(pitch)
|
||||||
sp = sin(pitch)
|
sp = sin(pitch)
|
||||||
sr = sin(roll)
|
sr = sin(roll)
|
||||||
|
@ -159,9 +161,8 @@ class Matrix3:
|
||||||
self.c.y = sr * cp
|
self.c.y = sr * cp
|
||||||
self.c.z = cr * cp
|
self.c.z = cr * cp
|
||||||
|
|
||||||
|
|
||||||
def to_euler(self):
|
def to_euler(self):
|
||||||
'''find Euler angles (321 convention) for the matrix'''
|
"""Find Euler angles (321 convention) for the matrix."""
|
||||||
if self.c.x >= 1.0:
|
if self.c.x >= 1.0:
|
||||||
pitch = pi
|
pitch = pi
|
||||||
elif self.c.x <= -1.0:
|
elif self.c.x <= -1.0:
|
||||||
|
@ -169,14 +170,13 @@ class Matrix3:
|
||||||
else:
|
else:
|
||||||
pitch = -asin(self.c.x)
|
pitch = -asin(self.c.x)
|
||||||
roll = atan2(self.c.y, self.c.z)
|
roll = atan2(self.c.y, self.c.z)
|
||||||
yaw = atan2(self.b.x, self.a.x)
|
yaw = atan2(self.b.x, self.a.x)
|
||||||
return (roll, pitch, yaw)
|
return (roll, pitch, yaw)
|
||||||
|
|
||||||
|
|
||||||
def to_euler312(self):
|
def to_euler312(self):
|
||||||
'''find Euler angles (312 convention) for the matrix.
|
"""Find Euler angles (312 convention) for the matrix.
|
||||||
See http://www.atacolorado.com/eulersequences.doc
|
See http://www.atacolorado.com/eulersequences.doc
|
||||||
'''
|
"""
|
||||||
T21 = self.a.y
|
T21 = self.a.y
|
||||||
T22 = self.b.y
|
T22 = self.b.y
|
||||||
T23 = self.c.y
|
T23 = self.c.y
|
||||||
|
@ -188,7 +188,7 @@ class Matrix3:
|
||||||
return (roll, pitch, yaw)
|
return (roll, pitch, yaw)
|
||||||
|
|
||||||
def from_euler312(self, roll, pitch, yaw):
|
def from_euler312(self, roll, pitch, yaw):
|
||||||
'''fill the matrix from Euler angles in radians in 312 convention'''
|
"""Fill the matrix from Euler angles in radians in 312 convention."""
|
||||||
c3 = cos(pitch)
|
c3 = cos(pitch)
|
||||||
s3 = sin(pitch)
|
s3 = sin(pitch)
|
||||||
s2 = sin(roll)
|
s2 = sin(roll)
|
||||||
|
@ -199,11 +199,11 @@ class Matrix3:
|
||||||
self.a.x = c1 * c3 - s1 * s2 * s3
|
self.a.x = c1 * c3 - s1 * s2 * s3
|
||||||
self.b.y = c1 * c2
|
self.b.y = c1 * c2
|
||||||
self.c.z = c3 * c2
|
self.c.z = c3 * c2
|
||||||
self.a.y = -c2*s1
|
self.a.y = -c2 * s1
|
||||||
self.a.z = s3*c1 + c3*s2*s1
|
self.a.z = s3 * c1 + c3 * s2 * s1
|
||||||
self.b.x = c3*s1 + s3*s2*c1
|
self.b.x = c3 * s1 + s3 * s2 * c1
|
||||||
self.b.z = s1*s3 - s2*c1*c3
|
self.b.z = s1 * s3 - s2 * c1 * c3
|
||||||
self.c.x = -s3*c2
|
self.c.x = -s3 * c2
|
||||||
self.c.y = s2
|
self.c.y = s2
|
||||||
|
|
||||||
def __add__(self, m):
|
def __add__(self, m):
|
||||||
|
@ -249,7 +249,7 @@ class Matrix3:
|
||||||
copy = __copy__
|
copy = __copy__
|
||||||
|
|
||||||
def rotate(self, g):
|
def rotate(self, g):
|
||||||
'''rotate the matrix by a given amount on 3 axes'''
|
"""Rotate the matrix by a given amount on 3 axes."""
|
||||||
temp_matrix = Matrix3()
|
temp_matrix = Matrix3()
|
||||||
a = self.a
|
a = self.a
|
||||||
b = self.b
|
b = self.b
|
||||||
|
@ -268,7 +268,7 @@ class Matrix3:
|
||||||
self.c += temp_matrix.c
|
self.c += temp_matrix.c
|
||||||
|
|
||||||
def normalize(self):
|
def normalize(self):
|
||||||
'''re-normalise a rotation matrix'''
|
"""Re-normalise a rotation matrix."""
|
||||||
error = self.a * self.b
|
error = self.a * self.b
|
||||||
t0 = self.a - (self.b * (0.5 * error))
|
t0 = self.a - (self.b * (0.5 * error))
|
||||||
t1 = self.b - (self.a * (0.5 * error))
|
t1 = self.b - (self.a * (0.5 * error))
|
||||||
|
@ -278,11 +278,12 @@ class Matrix3:
|
||||||
self.c = t2 * (1.0 / t2.length())
|
self.c = t2 * (1.0 / t2.length())
|
||||||
|
|
||||||
def trace(self):
|
def trace(self):
|
||||||
'''the trace of the matrix'''
|
"""The trace of the matrix."""
|
||||||
return self.a.x + self.b.y + self.c.z
|
return self.a.x + self.b.y + self.c.z
|
||||||
|
|
||||||
|
|
||||||
def test_euler():
|
def test_euler():
|
||||||
'''check that from_euler() and to_euler() are consistent'''
|
"""Check that from_euler() and to_euler() are consistent."""
|
||||||
m = Matrix3()
|
m = Matrix3()
|
||||||
from math import radians, degrees
|
from math import radians, degrees
|
||||||
for r in range(-179, 179, 3):
|
for r in range(-179, 179, 3):
|
||||||
|
@ -290,38 +291,41 @@ def test_euler():
|
||||||
for y in range(-179, 179, 3):
|
for y in range(-179, 179, 3):
|
||||||
m.from_euler(radians(r), radians(p), radians(y))
|
m.from_euler(radians(r), radians(p), radians(y))
|
||||||
(r2, p2, y2) = m.to_euler()
|
(r2, p2, y2) = m.to_euler()
|
||||||
v1 = Vector3(r,p,y)
|
v1 = Vector3(r, p, y)
|
||||||
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
|
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
|
||||||
diff = v1 - v2
|
diff = v1 - v2
|
||||||
if diff.length() > 1.0e-12:
|
if diff.length() > 1.0e-12:
|
||||||
print('EULER ERROR:', v1, v2, diff.length())
|
print('EULER ERROR:', v1, v2, diff.length())
|
||||||
|
|
||||||
def test_euler312_single(r,p,y):
|
|
||||||
'''check that from_euler312() and to_euler312() are consistent for one set of values'''
|
def test_euler312_single(r, p, y):
|
||||||
|
"""Check that from_euler312() and to_euler312() are consistent for one set of values."""
|
||||||
from math import degrees, radians
|
from math import degrees, radians
|
||||||
m = Matrix3()
|
m = Matrix3()
|
||||||
m.from_euler312(radians(r), radians(p), radians(y))
|
m.from_euler312(radians(r), radians(p), radians(y))
|
||||||
(r2, p2, y2) = m.to_euler312()
|
(r2, p2, y2) = m.to_euler312()
|
||||||
v1 = Vector3(r,p,y)
|
v1 = Vector3(r, p, y)
|
||||||
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
|
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
|
||||||
diff = v1 - v2
|
diff = v1 - v2
|
||||||
if diff.length() > 1.0e-12:
|
if diff.length() > 1.0e-12:
|
||||||
print('EULER ERROR:', v1, v2, diff.length())
|
print('EULER ERROR:', v1, v2, diff.length())
|
||||||
|
|
||||||
def test_one_axis(r,p,y):
|
|
||||||
'''check that from_euler312() and from_euler() are consistent for one set of values on one axis'''
|
def test_one_axis(r, p, y):
|
||||||
|
"""Check that from_euler312() and from_euler() are consistent for one set of values on one axis."""
|
||||||
from math import degrees, radians
|
from math import degrees, radians
|
||||||
m = Matrix3()
|
m = Matrix3()
|
||||||
m.from_euler312(radians(r), radians(p), radians(y))
|
m.from_euler312(radians(r), radians(p), radians(y))
|
||||||
(r2, p2, y2) = m.to_euler()
|
(r2, p2, y2) = m.to_euler()
|
||||||
v1 = Vector3(r,p,y)
|
v1 = Vector3(r, p, y)
|
||||||
v2 = Vector3(degrees(r2),degrees(p2),degrees(y2))
|
v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
|
||||||
diff = v1 - v2
|
diff = v1 - v2
|
||||||
if diff.length() > 1.0e-12:
|
if diff.length() > 1.0e-12:
|
||||||
print('EULER ERROR:', v1, v2, diff.length())
|
print('EULER ERROR:', v1, v2, diff.length())
|
||||||
|
|
||||||
|
|
||||||
def test_euler312():
|
def test_euler312():
|
||||||
'''check that from_euler312() and to_euler312() are consistent'''
|
"""Check that from_euler312() and to_euler312() are consistent."""
|
||||||
m = Matrix3()
|
m = Matrix3()
|
||||||
|
|
||||||
for x in range(-89, 89, 3):
|
for x in range(-89, 89, 3):
|
||||||
|
@ -331,10 +335,12 @@ def test_euler312():
|
||||||
for r in range(-89, 89, 3):
|
for r in range(-89, 89, 3):
|
||||||
for p in range(-179, 179, 3):
|
for p in range(-179, 179, 3):
|
||||||
for y in range(-179, 179, 3):
|
for y in range(-179, 179, 3):
|
||||||
test_euler312_single(r,p,y)
|
test_euler312_single(r, p, y)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
import doctest
|
import doctest
|
||||||
|
|
||||||
doctest.testmod()
|
doctest.testmod()
|
||||||
test_euler()
|
test_euler()
|
||||||
test_euler312()
|
test_euler312()
|
||||||
|
|
|
@ -1,13 +1,16 @@
|
||||||
#!/usr/bin/env python
|
#!/usr/bin/env python
|
||||||
# simple test of wind generation code
|
"""
|
||||||
|
simple test of wind generation code
|
||||||
|
"""
|
||||||
|
|
||||||
import util, time, random
|
import time
|
||||||
|
import util
|
||||||
from rotmat import Vector3
|
from rotmat import Vector3
|
||||||
|
|
||||||
wind = util.Wind('7,90,0.1')
|
wind = util.Wind('7,90,0.1')
|
||||||
|
|
||||||
t0 = time.time()
|
t0 = time.time()
|
||||||
velocity = Vector3(0,0,0)
|
velocity = Vector3(0, 0, 0)
|
||||||
|
|
||||||
t = 0
|
t = 0
|
||||||
deltat = 0.01
|
deltat = 0.01
|
||||||
|
|
|
@ -1,82 +1,100 @@
|
||||||
import math
|
import math
|
||||||
from math import sqrt, acos, cos, pi, sin, atan2
|
import os
|
||||||
import os, sys, time, random
|
import random
|
||||||
from rotmat import Vector3, Matrix3
|
import re
|
||||||
from subprocess import call, check_call,Popen, PIPE
|
import sys
|
||||||
|
import time
|
||||||
|
from math import acos, atan2, cos, pi, sqrt
|
||||||
|
from subprocess import PIPE, Popen, call, check_call
|
||||||
|
|
||||||
|
import pexpect
|
||||||
|
|
||||||
|
from rotmat import Matrix3, Vector3
|
||||||
|
|
||||||
|
|
||||||
def m2ft(x):
|
def m2ft(x):
|
||||||
'''meters to feet'''
|
"""Meters to feet."""
|
||||||
return float(x) / 0.3048
|
return float(x) / 0.3048
|
||||||
|
|
||||||
|
|
||||||
def ft2m(x):
|
def ft2m(x):
|
||||||
'''feet to meters'''
|
"""Feet to meters."""
|
||||||
return float(x) * 0.3048
|
return float(x) * 0.3048
|
||||||
|
|
||||||
|
|
||||||
def kt2mps(x):
|
def kt2mps(x):
|
||||||
return x * 0.514444444
|
return x * 0.514444444
|
||||||
|
|
||||||
|
|
||||||
def mps2kt(x):
|
def mps2kt(x):
|
||||||
return x / 0.514444444
|
return x / 0.514444444
|
||||||
|
|
||||||
|
|
||||||
def topdir():
|
def topdir():
|
||||||
'''return top of git tree where autotest is running from'''
|
"""Return top of git tree where autotest is running from."""
|
||||||
d = os.path.dirname(os.path.realpath(__file__))
|
d = os.path.dirname(os.path.realpath(__file__))
|
||||||
assert(os.path.basename(d)=='pysim')
|
assert(os.path.basename(d) == 'pysim')
|
||||||
d = os.path.dirname(d)
|
d = os.path.dirname(d)
|
||||||
assert(os.path.basename(d)=='autotest')
|
assert(os.path.basename(d) == 'autotest')
|
||||||
d = os.path.dirname(d)
|
d = os.path.dirname(d)
|
||||||
assert(os.path.basename(d)=='Tools')
|
assert(os.path.basename(d) == 'Tools')
|
||||||
d = os.path.dirname(d)
|
d = os.path.dirname(d)
|
||||||
return d
|
return d
|
||||||
|
|
||||||
|
|
||||||
def reltopdir(path):
|
def reltopdir(path):
|
||||||
'''return a path relative to topdir()'''
|
"""Return a path relative to topdir()."""
|
||||||
return os.path.normpath(os.path.join(topdir(), path))
|
return os.path.normpath(os.path.join(topdir(), path))
|
||||||
|
|
||||||
|
|
||||||
def run_cmd(cmd, dir=".", show=True, output=False, checkfail=True):
|
def run_cmd(cmd, directory=".", show=True, output=False, checkfail=True):
|
||||||
'''run a shell command'''
|
"""Run a shell command."""
|
||||||
shell = False
|
shell = False
|
||||||
if not isinstance(cmd, list):
|
if not isinstance(cmd, list):
|
||||||
cmd = [cmd]
|
cmd = [cmd]
|
||||||
shell = True
|
shell = True
|
||||||
if show:
|
if show:
|
||||||
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), dir,))
|
print("Running: (%s) in (%s)" % (cmd_as_shell(cmd), directory,))
|
||||||
if output:
|
if output:
|
||||||
return Popen(cmd, shell=shell, stdout=PIPE, cwd=dir).communicate()[0]
|
return Popen(cmd, shell=shell, stdout=PIPE, cwd=directory).communicate()[0]
|
||||||
elif checkfail:
|
elif checkfail:
|
||||||
return check_call(cmd, shell=shell, cwd=dir)
|
return check_call(cmd, shell=shell, cwd=directory)
|
||||||
else:
|
else:
|
||||||
return call(cmd, shell=shell, cwd=dir)
|
return call(cmd, shell=shell, cwd=directory)
|
||||||
|
|
||||||
|
|
||||||
def rmfile(path):
|
def rmfile(path):
|
||||||
'''remove a file if it exists'''
|
"""Remove a file if it exists."""
|
||||||
try:
|
try:
|
||||||
os.unlink(path)
|
os.unlink(path)
|
||||||
except Exception:
|
except Exception:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
|
||||||
def deltree(path):
|
def deltree(path):
|
||||||
'''delete a tree of files'''
|
"""Delete a tree of files."""
|
||||||
run_cmd('rm -rf %s' % path)
|
run_cmd('rm -rf %s' % path)
|
||||||
|
|
||||||
|
|
||||||
def relwaf():
|
def relwaf():
|
||||||
return "./modules/waf/waf-light"
|
return "./modules/waf/waf-light"
|
||||||
|
|
||||||
|
|
||||||
def waf_configure(board, j=None, debug=False):
|
def waf_configure(board, j=None, debug=False):
|
||||||
cmd_configure = [relwaf(), "configure", "--board", board]
|
cmd_configure = [relwaf(), "configure", "--board", board]
|
||||||
if debug:
|
if debug:
|
||||||
cmd_configure.append('--debug')
|
cmd_configure.append('--debug')
|
||||||
if j is not None:
|
if j is not None:
|
||||||
cmd_configure.extend(['-j', str(j)])
|
cmd_configure.extend(['-j', str(j)])
|
||||||
run_cmd(cmd_configure, dir=topdir(), checkfail=True)
|
run_cmd(cmd_configure, directory=topdir(), checkfail=True)
|
||||||
|
|
||||||
|
|
||||||
def waf_clean():
|
def waf_clean():
|
||||||
run_cmd([relwaf(), "clean"], dir=topdir(), checkfail=True)
|
run_cmd([relwaf(), "clean"], directory=topdir(), checkfail=True)
|
||||||
|
|
||||||
def build_SIL(build_target, j=None, debug=False, board='sitl'):
|
|
||||||
'''build desktop SIL'''
|
def build_SITL(build_target, j=None, debug=False, board='sitl'):
|
||||||
|
"""Build desktop SITL."""
|
||||||
|
|
||||||
# first configure
|
# first configure
|
||||||
waf_configure(board, j=j, debug=debug)
|
waf_configure(board, j=j, debug=debug)
|
||||||
|
@ -88,9 +106,10 @@ def build_SIL(build_target, j=None, debug=False, board='sitl'):
|
||||||
cmd_make = [relwaf(), "build", "--target", build_target]
|
cmd_make = [relwaf(), "build", "--target", build_target]
|
||||||
if j is not None:
|
if j is not None:
|
||||||
cmd_make.extend(['-j', str(j)])
|
cmd_make.extend(['-j', str(j)])
|
||||||
run_cmd(cmd_make, dir=topdir(), checkfail=True, show=True)
|
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def build_examples(board, j=None, debug=False, clean=False):
|
def build_examples(board, j=None, debug=False, clean=False):
|
||||||
# first configure
|
# first configure
|
||||||
waf_configure(board, j=j, debug=debug)
|
waf_configure(board, j=j, debug=debug)
|
||||||
|
@ -101,20 +120,22 @@ def build_examples(board, j=None, debug=False, clean=False):
|
||||||
|
|
||||||
# then build
|
# then build
|
||||||
cmd_make = [relwaf(), "examples"]
|
cmd_make = [relwaf(), "examples"]
|
||||||
run_cmd(cmd_make, dir=topdir(), checkfail=True, show=True)
|
run_cmd(cmd_make, directory=topdir(), checkfail=True, show=True)
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
# list of pexpect children to close on exit
|
# list of pexpect children to close on exit
|
||||||
close_list = []
|
close_list = []
|
||||||
|
|
||||||
|
|
||||||
def pexpect_autoclose(p):
|
def pexpect_autoclose(p):
|
||||||
'''mark for autoclosing'''
|
"""Mark for autoclosing."""
|
||||||
global close_list
|
global close_list
|
||||||
close_list.append(p)
|
close_list.append(p)
|
||||||
|
|
||||||
|
|
||||||
def pexpect_close(p):
|
def pexpect_close(p):
|
||||||
'''close a pexpect child'''
|
"""Close a pexpect child."""
|
||||||
global close_list
|
global close_list
|
||||||
|
|
||||||
try:
|
try:
|
||||||
|
@ -128,44 +149,47 @@ def pexpect_close(p):
|
||||||
if p in close_list:
|
if p in close_list:
|
||||||
close_list.remove(p)
|
close_list.remove(p)
|
||||||
|
|
||||||
|
|
||||||
def pexpect_close_all():
|
def pexpect_close_all():
|
||||||
'''close all pexpect children'''
|
"""Close all pexpect children."""
|
||||||
global close_list
|
global close_list
|
||||||
for p in close_list[:]:
|
for p in close_list[:]:
|
||||||
pexpect_close(p)
|
pexpect_close(p)
|
||||||
|
|
||||||
|
|
||||||
def pexpect_drain(p):
|
def pexpect_drain(p):
|
||||||
'''drain any pending input'''
|
"""Drain any pending input."""
|
||||||
import pexpect
|
import pexpect
|
||||||
try:
|
try:
|
||||||
p.read_nonblocking(1000, timeout=0)
|
p.read_nonblocking(1000, timeout=0)
|
||||||
except pexpect.TIMEOUT:
|
except pexpect.TIMEOUT:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def cmd_as_shell(cmd):
|
|
||||||
return (" ".join([ '"%s"' % x for x in cmd ]))
|
|
||||||
|
|
||||||
import re
|
def cmd_as_shell(cmd):
|
||||||
|
return (" ".join(['"%s"' % x for x in cmd]))
|
||||||
|
|
||||||
|
|
||||||
def make_safe_filename(text):
|
def make_safe_filename(text):
|
||||||
'''return a version of text safe for use as a filename'''
|
"""Return a version of text safe for use as a filename."""
|
||||||
r = re.compile("([^a-zA-Z0-9_.+-])")
|
r = re.compile("([^a-zA-Z0-9_.+-])")
|
||||||
text.replace('/','-')
|
text.replace('/', '-')
|
||||||
filename = r.sub(lambda m : "%" + str(hex(ord(str(m.group(1))))).upper(), text)
|
filename = r.sub(lambda m: "%" + str(hex(ord(str(m.group(1))))).upper(), text)
|
||||||
return filename
|
return filename
|
||||||
|
|
||||||
import pexpect
|
|
||||||
class SIL(pexpect.spawn):
|
class SITL(pexpect.spawn):
|
||||||
|
|
||||||
def __init__(self, binary, valgrind=False, gdb=False, wipe=False, synthetic_clock=True, home=None, model=None, speedup=1, defaults_file=None):
|
def __init__(self, binary, valgrind=False, gdb=False, wipe=False, synthetic_clock=True, home=None, model=None, speedup=1, defaults_file=None):
|
||||||
self.binary = binary
|
self.binary = binary
|
||||||
self.model = model
|
self.model = model
|
||||||
|
|
||||||
cmd=[]
|
cmd = []
|
||||||
if valgrind and os.path.exists('/usr/bin/valgrind'):
|
if valgrind and os.path.exists('/usr/bin/valgrind'):
|
||||||
cmd.extend(['valgrind', '-q', '--log-file=%s' % self.valgrind_log_filepath()])
|
cmd.extend(['valgrind', '-q', '--log-file=%s' % self.valgrind_log_filepath()])
|
||||||
if gdb:
|
if gdb:
|
||||||
f = open("/tmp/x.gdb", "w")
|
f = open("/tmp/x.gdb", "w")
|
||||||
f.write("r\n");
|
f.write("r\n")
|
||||||
f.close()
|
f.close()
|
||||||
cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])
|
cmd.extend(['xterm', '-e', 'gdb', '-x', '/tmp/x.gdb', '--args'])
|
||||||
|
|
||||||
|
@ -185,7 +209,7 @@ class SIL(pexpect.spawn):
|
||||||
print("Running: %s" % cmd_as_shell(cmd))
|
print("Running: %s" % cmd_as_shell(cmd))
|
||||||
first = cmd[0]
|
first = cmd[0]
|
||||||
rest = cmd[1:]
|
rest = cmd[1:]
|
||||||
super(SIL,self).__init__(first, rest, logfile=sys.stdout, timeout=5)
|
super(SITL, self).__init__(first, rest, logfile=sys.stdout, timeout=5)
|
||||||
delaybeforesend = 0
|
delaybeforesend = 0
|
||||||
pexpect_autoclose(self)
|
pexpect_autoclose(self)
|
||||||
# give time for parameters to properly setup
|
# give time for parameters to properly setup
|
||||||
|
@ -193,24 +217,25 @@ class SIL(pexpect.spawn):
|
||||||
if gdb:
|
if gdb:
|
||||||
# if we run GDB we do so in an xterm. "Waiting for
|
# if we run GDB we do so in an xterm. "Waiting for
|
||||||
# connection" is never going to appear on xterm's output.
|
# connection" is never going to appear on xterm's output.
|
||||||
#... so let's give it another magic second.
|
# ... so let's give it another magic second.
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
# TODO: have a SITL-compiled ardupilot able to have its
|
# TODO: have a SITL-compiled ardupilot able to have its
|
||||||
# console on an output fd.
|
# console on an output fd.
|
||||||
else:
|
else:
|
||||||
self.expect('Waiting for connection',timeout=300)
|
self.expect('Waiting for connection', timeout=300)
|
||||||
|
|
||||||
|
|
||||||
def valgrind_log_filepath(self):
|
def valgrind_log_filepath(self):
|
||||||
return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(self.binary),self.model,))
|
return make_safe_filename('%s-%s-valgrind.log' % (os.path.basename(self.binary), self.model,))
|
||||||
|
|
||||||
def start_SIL(binary, **kwargs):
|
|
||||||
'''launch a SIL instance'''
|
|
||||||
return SIL(binary, **kwargs)
|
|
||||||
|
|
||||||
def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760',
|
def start_SITL(binary, **kwargs):
|
||||||
options=None, logfile=sys.stdout):
|
"""Launch a SITL instance."""
|
||||||
'''launch mavproxy connected to a SIL instance'''
|
return SITL(binary, **kwargs)
|
||||||
|
|
||||||
|
|
||||||
|
def start_MAVProxy_SITL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:5760',
|
||||||
|
options=None, logfile=sys.stdout):
|
||||||
|
"""Launch mavproxy connected to a SITL instance."""
|
||||||
import pexpect
|
import pexpect
|
||||||
global close_list
|
global close_list
|
||||||
MAVPROXY = os.getenv('MAVPROXY_CMD', 'mavproxy.py')
|
MAVPROXY = os.getenv('MAVPROXY_CMD', 'mavproxy.py')
|
||||||
|
@ -229,9 +254,10 @@ def start_MAVProxy_SIL(atype, aircraft=None, setup=False, master='tcp:127.0.0.1:
|
||||||
|
|
||||||
|
|
||||||
def expect_setup_callback(e, callback):
|
def expect_setup_callback(e, callback):
|
||||||
'''setup a callback that is called once a second while waiting for
|
"""Setup a callback that is called once a second while waiting for
|
||||||
patterns'''
|
patterns."""
|
||||||
import pexpect
|
import pexpect
|
||||||
|
|
||||||
def _expect_callback(pattern, timeout=e.timeout):
|
def _expect_callback(pattern, timeout=e.timeout):
|
||||||
tstart = time.time()
|
tstart = time.time()
|
||||||
while time.time() < tstart + timeout:
|
while time.time() < tstart + timeout:
|
||||||
|
@ -248,27 +274,30 @@ def expect_setup_callback(e, callback):
|
||||||
e.expect_saved = e.expect
|
e.expect_saved = e.expect
|
||||||
e.expect = _expect_callback
|
e.expect = _expect_callback
|
||||||
|
|
||||||
def mkdir_p(dir):
|
|
||||||
'''like mkdir -p'''
|
def mkdir_p(directory):
|
||||||
if not dir:
|
"""Like mkdir -p ."""
|
||||||
|
if not directory:
|
||||||
return
|
return
|
||||||
if dir.endswith("/"):
|
if directory.endswith("/"):
|
||||||
mkdir_p(dir[:-1])
|
mkdir_p(directory[:-1])
|
||||||
return
|
return
|
||||||
if os.path.isdir(dir):
|
if os.path.isdir(directory):
|
||||||
return
|
return
|
||||||
mkdir_p(os.path.dirname(dir))
|
mkdir_p(os.path.dirname(directory))
|
||||||
os.mkdir(dir)
|
os.mkdir(directory)
|
||||||
|
|
||||||
|
|
||||||
def loadfile(fname):
|
def loadfile(fname):
|
||||||
'''load a file as a string'''
|
"""Load a file as a string."""
|
||||||
f = open(fname, mode='r')
|
f = open(fname, mode='r')
|
||||||
r = f.read()
|
r = f.read()
|
||||||
f.close()
|
f.close()
|
||||||
return r
|
return r
|
||||||
|
|
||||||
|
|
||||||
def lock_file(fname):
|
def lock_file(fname):
|
||||||
'''lock a file'''
|
"""Lock a file."""
|
||||||
import fcntl
|
import fcntl
|
||||||
f = open(fname, mode='w')
|
f = open(fname, mode='w')
|
||||||
try:
|
try:
|
||||||
|
@ -277,8 +306,9 @@ def lock_file(fname):
|
||||||
return None
|
return None
|
||||||
return f
|
return f
|
||||||
|
|
||||||
|
|
||||||
def check_parent(parent_pid=None):
|
def check_parent(parent_pid=None):
|
||||||
'''check our parent process is still alive'''
|
"""Check our parent process is still alive."""
|
||||||
if parent_pid is None:
|
if parent_pid is None:
|
||||||
try:
|
try:
|
||||||
parent_pid = os.getppid()
|
parent_pid = os.getppid()
|
||||||
|
@ -294,13 +324,13 @@ def check_parent(parent_pid=None):
|
||||||
|
|
||||||
|
|
||||||
def EarthRatesToBodyRates(dcm, earth_rates):
|
def EarthRatesToBodyRates(dcm, earth_rates):
|
||||||
'''convert the angular velocities from earth frame to
|
"""Convert the angular velocities from earth frame to
|
||||||
body frame. Thanks to James Goppert for the formula
|
body frame. Thanks to James Goppert for the formula
|
||||||
|
|
||||||
all inputs and outputs are in radians
|
all inputs and outputs are in radians
|
||||||
|
|
||||||
returns a gyro vector in body frame, in rad/s
|
returns a gyro vector in body frame, in rad/s .
|
||||||
'''
|
"""
|
||||||
from math import sin, cos
|
from math import sin, cos
|
||||||
|
|
||||||
(phi, theta, psi) = dcm.to_euler()
|
(phi, theta, psi) = dcm.to_euler()
|
||||||
|
@ -308,19 +338,20 @@ def EarthRatesToBodyRates(dcm, earth_rates):
|
||||||
thetaDot = earth_rates.y
|
thetaDot = earth_rates.y
|
||||||
psiDot = earth_rates.z
|
psiDot = earth_rates.z
|
||||||
|
|
||||||
p = phiDot - psiDot*sin(theta)
|
p = phiDot - psiDot * sin(theta)
|
||||||
q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta)
|
q = cos(phi) * thetaDot + sin(phi) * psiDot * cos(theta)
|
||||||
r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot
|
r = cos(phi) * psiDot * cos(theta) - sin(phi) * thetaDot
|
||||||
return Vector3(p, q, r)
|
return Vector3(p, q, r)
|
||||||
|
|
||||||
|
|
||||||
def BodyRatesToEarthRates(dcm, gyro):
|
def BodyRatesToEarthRates(dcm, gyro):
|
||||||
'''convert the angular velocities from body frame to
|
"""Convert the angular velocities from body frame to
|
||||||
earth frame.
|
earth frame.
|
||||||
|
|
||||||
all inputs and outputs are in radians/s
|
all inputs and outputs are in radians/s
|
||||||
|
|
||||||
returns a earth rate vector
|
returns a earth rate vector.
|
||||||
'''
|
"""
|
||||||
from math import sin, cos, tan, fabs
|
from math import sin, cos, tan, fabs
|
||||||
|
|
||||||
p = gyro.x
|
p = gyro.x
|
||||||
|
@ -329,37 +360,38 @@ def BodyRatesToEarthRates(dcm, gyro):
|
||||||
|
|
||||||
(phi, theta, psi) = dcm.to_euler()
|
(phi, theta, psi) = dcm.to_euler()
|
||||||
|
|
||||||
phiDot = p + tan(theta)*(q*sin(phi) + r*cos(phi))
|
phiDot = p + tan(theta) * (q * sin(phi) + r * cos(phi))
|
||||||
thetaDot = q*cos(phi) - r*sin(phi)
|
thetaDot = q * cos(phi) - r * sin(phi)
|
||||||
if fabs(cos(theta)) < 1.0e-20:
|
if fabs(cos(theta)) < 1.0e-20:
|
||||||
theta += 1.0e-10
|
theta += 1.0e-10
|
||||||
psiDot = (q*sin(phi) + r*cos(phi))/cos(theta)
|
psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta)
|
||||||
return Vector3(phiDot, thetaDot, psiDot)
|
return Vector3(phiDot, thetaDot, psiDot)
|
||||||
|
|
||||||
radius_of_earth = 6378100.0 # in meters
|
radius_of_earth = 6378100.0 # in meters
|
||||||
|
|
||||||
|
|
||||||
def gps_newpos(lat, lon, bearing, distance):
|
def gps_newpos(lat, lon, bearing, distance):
|
||||||
'''extrapolate latitude/longitude given a heading and distance
|
"""Extrapolate latitude/longitude given a heading and distance
|
||||||
thanks to http://www.movable-type.co.uk/scripts/latlong.html
|
thanks to http://www.movable-type.co.uk/scripts/latlong.html .
|
||||||
'''
|
"""
|
||||||
from math import sin, asin, cos, atan2, radians, degrees
|
from math import sin, asin, cos, atan2, radians, degrees
|
||||||
|
|
||||||
lat1 = radians(lat)
|
lat1 = radians(lat)
|
||||||
lon1 = radians(lon)
|
lon1 = radians(lon)
|
||||||
brng = radians(bearing)
|
brng = radians(bearing)
|
||||||
dr = distance/radius_of_earth
|
dr = distance / radius_of_earth
|
||||||
|
|
||||||
lat2 = asin(sin(lat1)*cos(dr) +
|
lat2 = asin(sin(lat1) * cos(dr) +
|
||||||
cos(lat1)*sin(dr)*cos(brng))
|
cos(lat1) * sin(dr) * cos(brng))
|
||||||
lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1),
|
lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
|
||||||
cos(dr)-sin(lat1)*sin(lat2))
|
cos(dr) - sin(lat1) * sin(lat2))
|
||||||
return (degrees(lat2), degrees(lon2))
|
return (degrees(lat2), degrees(lon2))
|
||||||
|
|
||||||
|
|
||||||
def gps_distance(lat1, lon1, lat2, lon2):
|
def gps_distance(lat1, lon1, lat2, lon2):
|
||||||
'''return distance between two points in meters,
|
"""Return distance between two points in meters,
|
||||||
coordinates are in degrees
|
coordinates are in degrees
|
||||||
thanks to http://www.movable-type.co.uk/scripts/latlong.html'''
|
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
|
||||||
lat1 = math.radians(lat1)
|
lat1 = math.radians(lat1)
|
||||||
lat2 = math.radians(lat2)
|
lat2 = math.radians(lat2)
|
||||||
lon1 = math.radians(lon1)
|
lon1 = math.radians(lon1)
|
||||||
|
@ -367,35 +399,36 @@ def gps_distance(lat1, lon1, lat2, lon2):
|
||||||
dLat = lat2 - lat1
|
dLat = lat2 - lat1
|
||||||
dLon = lon2 - lon1
|
dLon = lon2 - lon1
|
||||||
|
|
||||||
a = math.sin(0.5*dLat)**2 + math.sin(0.5*dLon)**2 * math.cos(lat1) * math.cos(lat2)
|
a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)
|
||||||
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0-a))
|
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
|
||||||
return radius_of_earth * c
|
return radius_of_earth * c
|
||||||
|
|
||||||
|
|
||||||
def gps_bearing(lat1, lon1, lat2, lon2):
|
def gps_bearing(lat1, lon1, lat2, lon2):
|
||||||
'''return bearing between two points in degrees, in range 0-360
|
"""Return bearing between two points in degrees, in range 0-360
|
||||||
thanks to http://www.movable-type.co.uk/scripts/latlong.html'''
|
thanks to http://www.movable-type.co.uk/scripts/latlong.html ."""
|
||||||
lat1 = math.radians(lat1)
|
lat1 = math.radians(lat1)
|
||||||
lat2 = math.radians(lat2)
|
lat2 = math.radians(lat2)
|
||||||
lon1 = math.radians(lon1)
|
lon1 = math.radians(lon1)
|
||||||
lon2 = math.radians(lon2)
|
lon2 = math.radians(lon2)
|
||||||
dLat = lat2 - lat1
|
|
||||||
dLon = lon2 - lon1
|
dLon = lon2 - lon1
|
||||||
y = math.sin(dLon) * math.cos(lat2)
|
y = math.sin(dLon) * math.cos(lat2)
|
||||||
x = math.cos(lat1)*math.sin(lat2) - math.sin(lat1)*math.cos(lat2)*math.cos(dLon)
|
x = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dLon)
|
||||||
bearing = math.degrees(math.atan2(y, x))
|
bearing = math.degrees(math.atan2(y, x))
|
||||||
if bearing < 0:
|
if bearing < 0:
|
||||||
bearing += 360.0
|
bearing += 360.0
|
||||||
return bearing
|
return bearing
|
||||||
|
|
||||||
|
|
||||||
class Wind(object):
|
class Wind(object):
|
||||||
'''a wind generation object'''
|
"""A wind generation object."""
|
||||||
def __init__(self, windstring, cross_section=0.1):
|
def __init__(self, windstring, cross_section=0.1):
|
||||||
a = windstring.split(',')
|
a = windstring.split(',')
|
||||||
if len(a) != 3:
|
if len(a) != 3:
|
||||||
raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring)
|
raise RuntimeError("Expected wind in speed,direction,turbulance form, not %s" % windstring)
|
||||||
self.speed = float(a[0]) # m/s
|
self.speed = float(a[0]) # m/s
|
||||||
self.direction = float(a[1]) # direction the wind is going in
|
self.direction = float(a[1]) # direction the wind is going in
|
||||||
self.turbulance= float(a[2]) # turbulance factor (standard deviation)
|
self.turbulance = float(a[2]) # turbulance factor (standard deviation)
|
||||||
|
|
||||||
# the cross-section of the aircraft to wind. This is multiplied by the
|
# the cross-section of the aircraft to wind. This is multiplied by the
|
||||||
# difference in the wind and the velocity of the aircraft to give the acceleration
|
# difference in the wind and the velocity of the aircraft to give the acceleration
|
||||||
|
@ -412,26 +445,24 @@ class Wind(object):
|
||||||
self.turbulance_mul = 1.0
|
self.turbulance_mul = 1.0
|
||||||
|
|
||||||
def current(self, deltat=None):
|
def current(self, deltat=None):
|
||||||
'''return current wind speed and direction as a tuple
|
"""Return current wind speed and direction as a tuple
|
||||||
speed is in m/s, direction in degrees
|
speed is in m/s, direction in degrees."""
|
||||||
'''
|
|
||||||
if deltat is None:
|
if deltat is None:
|
||||||
tnow = time.time()
|
tnow = time.time()
|
||||||
deltat = tnow - self.tlast
|
deltat = tnow - self.tlast
|
||||||
self.tlast = tnow
|
self.tlast = tnow
|
||||||
|
|
||||||
# update turbulance random walk
|
# update turbulance random walk
|
||||||
w_delta = math.sqrt(deltat)*(1.0-random.gauss(1.0, self.turbulance))
|
w_delta = math.sqrt(deltat) * (1.0 - random.gauss(1.0, self.turbulance))
|
||||||
w_delta -= (self.turbulance_mul-1.0)*(deltat/self.turbulance_time_constant)
|
w_delta -= (self.turbulance_mul - 1.0) * (deltat / self.turbulance_time_constant)
|
||||||
self.turbulance_mul += w_delta
|
self.turbulance_mul += w_delta
|
||||||
speed = self.speed * math.fabs(self.turbulance_mul)
|
speed = self.speed * math.fabs(self.turbulance_mul)
|
||||||
return (speed, self.direction)
|
return (speed, self.direction)
|
||||||
|
|
||||||
|
|
||||||
# Calculate drag.
|
# Calculate drag.
|
||||||
def drag(self, velocity, deltat=None, testing=None):
|
def drag(self, velocity, deltat=None, testing=None):
|
||||||
'''return current wind force in Earth frame. The velocity parameter is
|
"""Return current wind force in Earth frame. The velocity parameter is
|
||||||
a Vector3 of the current velocity of the aircraft in earth frame, m/s'''
|
a Vector3 of the current velocity of the aircraft in earth frame, m/s ."""
|
||||||
from math import radians
|
from math import radians
|
||||||
|
|
||||||
# (m/s, degrees) : wind vector as a magnitude and angle.
|
# (m/s, degrees) : wind vector as a magnitude and angle.
|
||||||
|
@ -463,16 +494,15 @@ class Wind(object):
|
||||||
relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x))
|
relWindVec = toVec(rel_speed, beta + atan2(velocity.y, velocity.x))
|
||||||
|
|
||||||
# Combine them to get the acceleration vector.
|
# Combine them to get the acceleration vector.
|
||||||
return Vector3( acc(relWindVec.x, drag_force(self, relWindVec.x))
|
return Vector3(acc(relWindVec.x, drag_force(self, relWindVec.x)), acc(relWindVec.y, drag_force(self, relWindVec.y)), 0)
|
||||||
, acc(relWindVec.y, drag_force(self, relWindVec.y))
|
|
||||||
, 0 )
|
|
||||||
|
|
||||||
# http://en.wikipedia.org/wiki/Apparent_wind
|
|
||||||
#
|
|
||||||
# Returns apparent wind speed and angle of apparent wind. Alpha is the angle
|
|
||||||
# between the object and the true wind. alpha of 0 rads is a headwind; pi a
|
|
||||||
# tailwind. Speeds should always be positive.
|
|
||||||
def apparent_wind(wind_sp, obj_speed, alpha):
|
def apparent_wind(wind_sp, obj_speed, alpha):
|
||||||
|
"""http://en.wikipedia.org/wiki/Apparent_wind
|
||||||
|
|
||||||
|
Returns apparent wind speed and angle of apparent wind. Alpha is the angle
|
||||||
|
between the object and the true wind. alpha of 0 rads is a headwind; pi a
|
||||||
|
tailwind. Speeds should always be positive."""
|
||||||
delta = wind_sp * cos(alpha)
|
delta = wind_sp * cos(alpha)
|
||||||
x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta
|
x = wind_sp**2 + obj_speed**2 + 2 * obj_speed * delta
|
||||||
rel_speed = sqrt(x)
|
rel_speed = sqrt(x)
|
||||||
|
@ -483,37 +513,41 @@ def apparent_wind(wind_sp, obj_speed, alpha):
|
||||||
|
|
||||||
return (rel_speed, beta)
|
return (rel_speed, beta)
|
||||||
|
|
||||||
# See http://en.wikipedia.org/wiki/Drag_equation
|
|
||||||
#
|
|
||||||
# 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
|
return (sp**2.0) * 0.1 * wind.cross_section
|
||||||
|
|
||||||
# Function to make the force vector. relWindVec is the direction the apparent
|
|
||||||
# wind comes *from*. We want to compute the accleration vector in the direction
|
|
||||||
# the wind blows to.
|
|
||||||
def acc(val, mag):
|
def acc(val, mag):
|
||||||
|
""" Function to make the force vector. relWindVec is the direction the apparent
|
||||||
|
wind comes *from*. We want to compute the accleration vector in the direction
|
||||||
|
the wind blows to."""
|
||||||
if val == 0:
|
if val == 0:
|
||||||
return mag
|
return mag
|
||||||
else:
|
else:
|
||||||
return (val / abs(val)) * (0 - mag)
|
return (val / abs(val)) * (0 - mag)
|
||||||
|
|
||||||
# Converts a magnitude and angle (radians) to a vector in the xy plane.
|
|
||||||
def toVec(magnitude, angle):
|
def toVec(magnitude, angle):
|
||||||
|
"""Converts a magnitude and angle (radians) to a vector in the xy plane."""
|
||||||
v = Vector3(magnitude, 0, 0)
|
v = Vector3(magnitude, 0, 0)
|
||||||
m = Matrix3()
|
m = Matrix3()
|
||||||
m.from_euler(0, 0, angle)
|
m.from_euler(0, 0, angle)
|
||||||
return m.transposed() * v
|
return m.transposed() * v
|
||||||
|
|
||||||
|
|
||||||
def constrain(value, minv, maxv):
|
def constrain(value, minv, maxv):
|
||||||
'''constrain a value to a range'''
|
"""Constrain a value to a range."""
|
||||||
if value < minv:
|
if value < minv:
|
||||||
value = minv
|
value = minv
|
||||||
if value > maxv:
|
if value > maxv:
|
||||||
|
@ -523,4 +557,3 @@ def constrain(value, minv, maxv):
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
import doctest
|
import doctest
|
||||||
doctest.testmod()
|
doctest.testmod()
|
||||||
|
|
||||||
|
|
|
@ -1,23 +1,27 @@
|
||||||
# fly ArduPlane QuadPlane in SITL
|
# fly ArduPlane QuadPlane in SITL
|
||||||
|
|
||||||
import util, pexpect, sys, time, math, shutil, os
|
import os
|
||||||
from common import *
|
import pexpect
|
||||||
|
import shutil
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
import random
|
|
||||||
|
from common import *
|
||||||
|
from pysim import util
|
||||||
|
|
||||||
# get location of scripts
|
# get location of scripts
|
||||||
testdir=os.path.dirname(os.path.realpath(__file__))
|
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||||
|
|
||||||
|
|
||||||
HOME_LOCATION='-27.274439,151.290064,343,8.7'
|
HOME_LOCATION = '-27.274439,151.290064,343,8.7'
|
||||||
MISSION='ArduPlane-Missions/Dalby-OBC2016.txt'
|
MISSION = 'ArduPlane-Missions/Dalby-OBC2016.txt'
|
||||||
FENCE='ArduPlane-Missions/Dalby-OBC2016-fence.txt'
|
FENCE = 'ArduPlane-Missions/Dalby-OBC2016-fence.txt'
|
||||||
WIND="0,180,0.2" # speed,direction,variance
|
WIND = "0,180,0.2" # speed,direction,variance
|
||||||
|
|
||||||
homeloc = None
|
homeloc = None
|
||||||
|
|
||||||
|
|
||||||
def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
|
def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
|
||||||
'''fly a mission from a file'''
|
"""Fly a mission from a file."""
|
||||||
print("Flying mission %s" % filename)
|
print("Flying mission %s" % filename)
|
||||||
mavproxy.send('wp load %s\n' % filename)
|
mavproxy.send('wp load %s\n' % filename)
|
||||||
mavproxy.expect('Flight plan received')
|
mavproxy.expect('Flight plan received')
|
||||||
|
@ -40,23 +44,23 @@ def fly_mission(mavproxy, mav, filename, fence, height_accuracy=-1):
|
||||||
return True
|
return True
|
||||||
|
|
||||||
|
|
||||||
def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
def fly_QuadPlane(binary, viewerip=None, use_map=False, valgrind=False, gdb=False):
|
||||||
'''fly QuadPlane in SIL
|
"""Fly QuadPlane in SITL.
|
||||||
|
|
||||||
you can pass viewerip as an IP address to optionally send fg and
|
you can pass viewerip as an IP address to optionally send fg and
|
||||||
mavproxy packets too for local viewing of the flight in real time
|
mavproxy packets too for local viewing of the flight in real time.
|
||||||
'''
|
"""
|
||||||
global homeloc
|
global homeloc
|
||||||
|
|
||||||
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
options = '--sitl=127.0.0.1:5501 --out=127.0.0.1:19550 --streamrate=10'
|
||||||
if viewerip:
|
if viewerip:
|
||||||
options += " --out=%s:14550" % viewerip
|
options += " --out=%s:14550" % viewerip
|
||||||
if map:
|
if use_map:
|
||||||
options += ' --map'
|
options += ' --map'
|
||||||
|
|
||||||
sil = util.start_SIL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
|
sitl = util.start_SITL(binary, model='quadplane', wipe=True, home=HOME_LOCATION, speedup=10,
|
||||||
defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb)
|
defaults_file=os.path.join(testdir, 'default_params/quadplane.parm'), valgrind=valgrind, gdb=gdb)
|
||||||
mavproxy = util.start_MAVProxy_SIL('QuadPlane', options=options)
|
mavproxy = util.start_MAVProxy_SITL('QuadPlane', options=options)
|
||||||
mavproxy.expect('Telemetry log: (\S+)')
|
mavproxy.expect('Telemetry log: (\S+)')
|
||||||
logfile = mavproxy.match.group(1)
|
logfile = mavproxy.match.group(1)
|
||||||
print("LOGFILE %s" % logfile)
|
print("LOGFILE %s" % logfile)
|
||||||
|
@ -75,14 +79,14 @@ def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
mavproxy.expect('Received [0-9]+ parameters')
|
mavproxy.expect('Received [0-9]+ parameters')
|
||||||
|
|
||||||
expect_list_clear()
|
expect_list_clear()
|
||||||
expect_list_extend([sil, mavproxy])
|
expect_list_extend([sitl, mavproxy])
|
||||||
|
|
||||||
print("Started simulator")
|
print("Started simulator")
|
||||||
|
|
||||||
# get a mavlink connection going
|
# get a mavlink connection going
|
||||||
try:
|
try:
|
||||||
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
mav = mavutil.mavlink_connection('127.0.0.1:19550', robust_parsing=True)
|
||||||
except Exception, msg:
|
except Exception as msg:
|
||||||
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
print("Failed to start mavlink connection on 127.0.0.1:19550" % msg)
|
||||||
raise
|
raise
|
||||||
mav.message_hooks.append(message_hook)
|
mav.message_hooks.append(message_hook)
|
||||||
|
@ -112,15 +116,15 @@ def fly_QuadPlane(binary, viewerip=None, map=False, valgrind=False, gdb=False):
|
||||||
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")):
|
os.path.join(testdir, "ArduPlane-Missions/Dalby-OBC2016-fence.txt")):
|
||||||
print("Failed mission")
|
print("Failed mission")
|
||||||
failed = True
|
failed = True
|
||||||
except pexpect.TIMEOUT, e:
|
except pexpect.TIMEOUT as e:
|
||||||
print("Failed with timeout")
|
print("Failed with timeout")
|
||||||
failed = True
|
failed = True
|
||||||
|
|
||||||
mav.close()
|
mav.close()
|
||||||
util.pexpect_close(mavproxy)
|
util.pexpect_close(mavproxy)
|
||||||
util.pexpect_close(sil)
|
util.pexpect_close(sitl)
|
||||||
|
|
||||||
valgrind_log = sil.valgrind_log_filepath()
|
valgrind_log = sitl.valgrind_log_filepath()
|
||||||
if os.path.exists(valgrind_log):
|
if os.path.exists(valgrind_log):
|
||||||
os.chmod(valgrind_log, 0644)
|
os.chmod(valgrind_log, 0644)
|
||||||
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log"))
|
shutil.copy(valgrind_log, util.reltopdir("../buildlogs/QuadPlane-valgrind.log"))
|
||||||
|
|
|
@ -142,7 +142,7 @@ def kill_tasks():
|
||||||
"""Clean up stray processes by name. This is a somewhat shotgun approach"""
|
"""Clean up stray processes by name. This is a somewhat shotgun approach"""
|
||||||
progress("Killing tasks")
|
progress("Killing tasks")
|
||||||
try:
|
try:
|
||||||
victim_names = set([
|
victim_names = {
|
||||||
'JSBSim',
|
'JSBSim',
|
||||||
'lt-JSBSim',
|
'lt-JSBSim',
|
||||||
'ArduPlane.elf',
|
'ArduPlane.elf',
|
||||||
|
@ -153,9 +153,9 @@ def kill_tasks():
|
||||||
'MAVProxy.exe',
|
'MAVProxy.exe',
|
||||||
'runsim.py',
|
'runsim.py',
|
||||||
'AntennaTracker.elf',
|
'AntennaTracker.elf',
|
||||||
])
|
}
|
||||||
for frame in _options_for_frame.keys():
|
for frame in _options_for_frame.keys():
|
||||||
if not _options_for_frame[frame].has_key("waf_target"):
|
if "waf_target" not in _options_for_frame[frame]:
|
||||||
continue
|
continue
|
||||||
exe_name = os.path.basename(_options_for_frame[frame]["waf_target"])
|
exe_name = os.path.basename(_options_for_frame[frame]["waf_target"])
|
||||||
victim_names.add(exe_name)
|
victim_names.add(exe_name)
|
||||||
|
|
Loading…
Reference in New Issue