Browse Source

Tools: pep8 corrections + correct variable shadowing

master
Pierre Kancir 7 years ago committed by Peter Barker
parent
commit
f6cc934678
  1. 16
      Tools/autotest/apmrover2.py
  2. 102
      Tools/autotest/arducopter.py
  3. 2
      Tools/autotest/arduplane.py
  4. 2
      Tools/autotest/ardusub.py
  5. 2
      Tools/autotest/autotest.py
  6. 39
      Tools/autotest/common.py
  7. 13
      Tools/autotest/pysim/util.py
  8. 4
      Tools/autotest/quadplane.py
  9. 20
      Tools/autotest/sim_vehicle.py

16
Tools/autotest/apmrover2.py

@ -481,8 +481,8 @@ class AutoTestRover(AutoTest): @@ -481,8 +481,8 @@ class AutoTestRover(AutoTest):
fnoo = [(1, 'MANUAL'),
(2, 'MANUAL'),
(3, 'RTL'),
# (4, 'AUTO'), // no mission, can't set auto
(5, 'RTL'), # non-existant mode, should stay in RTL
# (4, 'AUTO'), # no mission, can't set auto
(5, 'RTL'), # non-existant mode, should stay in RTL
(6, 'MANUAL')]
for (num, expected) in fnoo:
self.mavproxy.send('switch %u\n' % num)
@ -505,7 +505,7 @@ class AutoTestRover(AutoTest): @@ -505,7 +505,7 @@ class AutoTestRover(AutoTest):
def test_setting_modes_via_modeswitch(self):
# test setting of modes through mode switch
self.context_push();
self.context_push()
ex = None
try:
self.set_parameter("MODE_CH", 8)
@ -526,13 +526,13 @@ class AutoTestRover(AutoTest): @@ -526,13 +526,13 @@ class AutoTestRover(AutoTest):
self.progress("Exception caught")
ex = e
self.context_pop();
self.context_pop()
if ex is not None:
raise ex
def test_setting_modes_via_auxswitches(self):
self.context_push();
self.context_push()
ex = None
try:
self.set_parameter("MODE5", 1)
@ -564,7 +564,7 @@ class AutoTestRover(AutoTest): @@ -564,7 +564,7 @@ class AutoTestRover(AutoTest):
self.progress("Exception caught")
ex = e
self.context_pop();
self.context_pop()
if ex is not None:
raise ex
@ -670,7 +670,7 @@ class AutoTestRover(AutoTest): @@ -670,7 +670,7 @@ class AutoTestRover(AutoTest):
self.drive_mission_rover1)
# disabled due to frequent failures in travis. This test needs re-writing
#self.run_test("Drive Brake", self.drive_brake)
# self.run_test("Drive Brake", self.drive_brake)
self.run_test("Disarm Vehicle", self.disarm_vehicle)
@ -699,7 +699,7 @@ class AutoTestRover(AutoTest): @@ -699,7 +699,7 @@ class AutoTestRover(AutoTest):
# self.progress("Failed RTL")
# failed = True
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.progress("Failed with timeout")
self.fail_list.append(("*timeout*", None))

102
Tools/autotest/arducopter.py

@ -5,7 +5,6 @@ from __future__ import print_function @@ -5,7 +5,6 @@ from __future__ import print_function
import math
import os
import shutil
import sys
import time
import pexpect
@ -13,14 +12,15 @@ from pymavlink import mavutil @@ -13,14 +12,15 @@ from pymavlink import mavutil
from pysim import util
from common import AutoTest, AutoTestTimeoutException
from common import NotAchievedException, PreconditionFailedException
from common import AutoTest
from common import NotAchievedException, AutoTestTimeoutException, PreconditionFailedException
# get location of scripts
testdir = os.path.dirname(os.path.realpath(__file__))
HOME = mavutil.location(-35.362938, 149.165085, 584, 270)
AVCHOME = mavutil.location(40.072842, -105.230575, 1586, 0)
# Flight mode switch positions are set-up in arducopter.param to be
# switch 1 = Circle
# switch 2 = Land
@ -158,7 +158,7 @@ class AutoTestCopter(AutoTest): @@ -158,7 +158,7 @@ class AutoTestCopter(AutoTest):
self.progress("TAKEOFF COMPLETE")
def wait_for_alt(self, alt_min=30):
'''wait for altitude to be reached'''
"""Wait for altitude to be reached."""
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m
if alt < alt_min:
@ -696,7 +696,6 @@ class AutoTestCopter(AutoTest): @@ -696,7 +696,6 @@ class AutoTestCopter(AutoTest):
# disable gps glitch
if glitch_current != -1:
glitch_current = -1
self.set_parameter("SIM_GPS_GLITCH_X", 0)
self.set_parameter("SIM_GPS_GLITCH_Y", 0)
if self.use_map:
@ -1043,9 +1042,9 @@ class AutoTestCopter(AutoTest): @@ -1043,9 +1042,9 @@ class AutoTestCopter(AutoTest):
# we only expect an octocopter to survive ATM:
servo_counts = {
# 2: 6, # hexa
3: 8, # octa
# 5: 6, # Y6
# 2: 6, # hexa
3: 8, # octa
# 5: 6, # Y6
}
frame_class = int(self.get_parameter("FRAME_CLASS"))
if frame_class not in servo_counts:
@ -1111,7 +1110,7 @@ class AutoTestCopter(AutoTest): @@ -1111,7 +1110,7 @@ class AutoTestCopter(AutoTest):
else:
state = "OK"
if failed and i==fail_servo:
if failed and i == fail_servo:
state += " (failed)"
self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state))
@ -1164,11 +1163,10 @@ class AutoTestCopter(AutoTest): @@ -1164,11 +1163,10 @@ class AutoTestCopter(AutoTest):
self.set_rc(3, 1500)
def fly_vision_position(self):
'''disable GPS navigation, enable Vicon input'''
"""Disable GPS navigation, enable Vicon input."""
# scribble down a location we can set origin to:
self.progress("Waiting for location")
start = self.mav.location()
self.mavproxy.send('switch 6\n') # stabilize mode
self.mav.wait_heartbeat()
self.wait_mode('STABILIZE')
@ -1178,7 +1176,7 @@ class AutoTestCopter(AutoTest): @@ -1178,7 +1176,7 @@ class AutoTestCopter(AutoTest):
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
print("old_pos=%s" % str(old_pos))
self.context_push();
self.context_push()
ex = None
try:
@ -1198,7 +1196,7 @@ class AutoTestCopter(AutoTest): @@ -1198,7 +1196,7 @@ class AutoTestCopter(AutoTest):
while True:
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
# self.progress("gpi=%s" % str(gpi))
# self.progress("gpi=%s" % str(gpi))
if gpi.lat != 0:
break
@ -1211,10 +1209,10 @@ class AutoTestCopter(AutoTest): @@ -1211,10 +1209,10 @@ class AutoTestCopter(AutoTest):
while True:
vicon_pos = self.mav.recv_match(type='VICON_POSITION_ESTIMATE',
blocking=True)
# print("vpe=%s" % str(vicon_pos))
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
# self.progress("gpi=%s" % str(gpi))
# print("vpe=%s" % str(vicon_pos))
self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
# self.progress("gpi=%s" % str(gpi))
if vicon_pos.x > 40:
break
@ -1230,12 +1228,12 @@ class AutoTestCopter(AutoTest): @@ -1230,12 +1228,12 @@ class AutoTestCopter(AutoTest):
while True:
if self.get_sim_time() - tstart > 200:
raise NotAchievedException()
gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
# print("gpi=%s" % str(gpi))
ss = self.mav.recv_match(type='SIMSTATE',
blocking=True)
# print("ss=%s" % str(ss))
self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
# print("gpi=%s" % str(gpi))
self.mav.recv_match(type='SIMSTATE',
blocking=True)
# print("ss=%s" % str(ss))
# wait for RTL disarm:
if not self.armed():
break
@ -1244,7 +1242,7 @@ class AutoTestCopter(AutoTest): @@ -1244,7 +1242,7 @@ class AutoTestCopter(AutoTest):
self.progress("Exception caught")
ex = e
self.context_pop();
self.context_pop()
self.set_rc(3, 1000)
self.reboot_sitl()
@ -1252,9 +1250,9 @@ class AutoTestCopter(AutoTest): @@ -1252,9 +1250,9 @@ class AutoTestCopter(AutoTest):
raise ex
def fly_nav_delay(self):
"""fly a simple mission that has a delay in it"""
"""Fly a simple mission that has a delay in it."""
num_wp = self.load_mission("copter_nav_delay.txt")
self.load_mission("copter_nav_delay.txt")
self.mavproxy.send('mode loiter\n')
self.mav.wait_heartbeat()
@ -1262,7 +1260,7 @@ class AutoTestCopter(AutoTest): @@ -1262,7 +1260,7 @@ class AutoTestCopter(AutoTest):
self.progress("Waiting reading for arm")
self.wait_ready_to_arm()
self.context_push();
self.context_push()
ex = None
try:
@ -1302,22 +1300,21 @@ class AutoTestCopter(AutoTest): @@ -1302,22 +1300,21 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def get_system_clock_utc(self, time):
def get_system_clock_utc(self, time_seconds):
# this is a copy of ArduPilot's AP_RTC function!
# separate time into ms, sec, min, hour and days but all expressed
# in milliseconds
time_ms = time * 1000
ms = time_ms % 1000;
sec_ms = (time_ms % (60 * 1000)) - ms;
min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms;
hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms;
time_ms = time_seconds * 1000
ms = time_ms % 1000
sec_ms = (time_ms % (60 * 1000)) - ms
min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms
hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms
# convert times as milliseconds into appropriate units
sec = sec_ms / 1000;
min = min_ms / (60 * 1000);
hour = hour_ms / (60 * 60 * 1000);
return (hour, min, sec, 0)
secs = sec_ms / 1000
mins = min_ms / (60 * 1000)
hours = hour_ms / (60 * 60 * 1000)
return (hours, mins, secs, 0)
def calc_delay(self, seconds):
# delay-for-seconds has to be long enough that we're at the
@ -1442,7 +1439,7 @@ class AutoTestCopter(AutoTest): @@ -1442,7 +1439,7 @@ class AutoTestCopter(AutoTest):
def fly_nav_delay_abstime(self):
"""fly a simple mission that has a delay in it"""
num_wp = self.load_mission("copter_nav_delay.txt")
self.load_mission("copter_nav_delay.txt")
self.progress("Starting mission")
@ -1458,7 +1455,7 @@ class AutoTestCopter(AutoTest): @@ -1458,7 +1455,7 @@ class AutoTestCopter(AutoTest):
reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)
reset_at = reset_at_m.time_unix_usec/1000000
self.context_push();
self.context_push()
ex = None
try:
@ -1558,7 +1555,7 @@ class AutoTestCopter(AutoTest): @@ -1558,7 +1555,7 @@ class AutoTestCopter(AutoTest):
raise ex
def test_setting_modes_via_modeswitch(self):
self.context_push();
self.context_push()
ex = None
try:
fltmode_ch = 5
@ -1570,7 +1567,7 @@ class AutoTestCopter(AutoTest): @@ -1570,7 +1567,7 @@ class AutoTestCopter(AutoTest):
("FLTMODE4", 7, "CIRCLE", 1555),
("FLTMODE5", 1, "ACRO", 1685),
("FLTMODE6", 17, "BRAKE", 1815),
]
]
for mode in testmodes:
(parm, parm_value, name, pwm) = mode
self.set_parameter(parm, parm_value)
@ -1605,7 +1602,7 @@ class AutoTestCopter(AutoTest): @@ -1605,7 +1602,7 @@ class AutoTestCopter(AutoTest):
raise ex
def test_setting_modes_via_auxswitch(self):
self.context_push();
self.context_push()
ex = None
try:
fltmode_ch = int(self.get_parameter("FLTMODE_CH"))
@ -1625,15 +1622,15 @@ class AutoTestCopter(AutoTest): @@ -1625,15 +1622,15 @@ class AutoTestCopter(AutoTest):
self.progress("Exception caught")
ex = e
self.context_pop();
self.context_pop()
if ex is not None:
raise ex
def fly_guided_change_submode(self):
'''Ensure we can move around in guided after a takeoff command'''
""""Ensure we can move around in guided after a takeoff command."""
self.context_push();
self.context_push()
ex = None
try:
@ -1651,11 +1648,11 @@ class AutoTestCopter(AutoTest): @@ -1651,11 +1648,11 @@ class AutoTestCopter(AutoTest):
startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
blocking=True)
'''yaw through absolute angles using MAV_CMD_CONDITION_YAW'''
"""yaw through absolute angles using MAV_CMD_CONDITION_YAW"""
self.guided_achieve_heading(45)
self.guided_achieve_heading(135)
'''move the vehicle using set_position_target_global_int'''
"""move the vehicle using set_position_target_global_int"""
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > 200:
@ -1695,14 +1692,13 @@ class AutoTestCopter(AutoTest): @@ -1695,14 +1692,13 @@ class AutoTestCopter(AutoTest):
self.progress("Exception caught")
ex = e
self.context_pop();
self.context_pop()
self.set_rc(3, 1000)
self.reboot_sitl()
if ex is not None:
raise ex
def autotest(self):
"""Autotest ArduCopter in SITL."""
self.check_test_syntax(test_file=os.path.realpath(__file__))
@ -1910,7 +1906,7 @@ class AutoTestCopter(AutoTest): @@ -1910,7 +1906,7 @@ class AutoTestCopter(AutoTest):
# Gripper test
self.run_test("Test gripper", self.test_gripper)
'''vision position''' # expects vehicle to be disarmed
'''vision position''' # expects vehicle to be disarmed
self.run_test("Fly Vision Position", self.fly_vision_position)
# Download logs
@ -1918,7 +1914,7 @@ class AutoTestCopter(AutoTest): @@ -1918,7 +1914,7 @@ class AutoTestCopter(AutoTest):
lambda: self.log_download(
self.buildlogs_path("ArduCopter-log.bin")))
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.progress("Failed with timeout")
self.fail_list.append("Failed with timeout")
self.close()
@ -1963,7 +1959,7 @@ class AutoTestCopter(AutoTest): @@ -1963,7 +1959,7 @@ class AutoTestCopter(AutoTest):
lambda: self.log_download(
self.buildlogs_path("Helicopter-log.bin")))
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.fail_list.append("Failed with timeout")
self.close()

2
Tools/autotest/arduplane.py

@ -587,7 +587,7 @@ class AutoTestPlane(AutoTest): @@ -587,7 +587,7 @@ class AutoTestPlane(AutoTest):
lambda: self.log_download(
self.buildlogs_path("ArduPlane-log.bin")))
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.progress("Failed with timeout")
self.fail_list.append("timeout")

2
Tools/autotest/ardusub.py

@ -167,7 +167,7 @@ class AutoTestSub(AutoTest): @@ -167,7 +167,7 @@ class AutoTestSub(AutoTest):
lambda: self.log_download(
self.buildlogs_path("ArduSub-log.bin")))
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.progress("Failed with timeout")
self.fail_list.append("Failed with timeout")

2
Tools/autotest/autotest.py

@ -33,7 +33,7 @@ def buildlogs_dirpath(): @@ -33,7 +33,7 @@ def buildlogs_dirpath():
def buildlogs_path(path):
'''return a string representing path in the buildlogs directory'''
"""return a string representing path in the buildlogs directory"""
bits = [buildlogs_dirpath()]
if isinstance(path, list):
bits.extend(path)

39
Tools/autotest/common.py

@ -103,6 +103,7 @@ class Context(object): @@ -103,6 +103,7 @@ class Context(object):
def __init__(self):
self.parameters = []
class AutoTest(ABC):
"""Base abstract class.
It implements the common function for all vehicle types.
@ -131,7 +132,7 @@ class AutoTest(ABC): @@ -131,7 +132,7 @@ class AutoTest(ABC):
return os.getenv("BUILDLOGS", util.reltopdir("../buildlogs"))
def buildlogs_path(self, path):
'''return a string representing path in the buildlogs directory'''
"""Return a string representing path in the buildlogs directory."""
bits = [self.buildlogs_dirpath()]
if isinstance(path, list):
bits.extend(path)
@ -140,7 +141,7 @@ class AutoTest(ABC): @@ -140,7 +141,7 @@ class AutoTest(ABC):
return os.path.join(*bits)
def sitl_streamrate(self):
'''allow subclasses to override SITL streamrate'''
"""Allow subclasses to override SITL streamrate."""
return 10
def autotest_connection_hostport(self):
@ -155,7 +156,7 @@ class AutoTest(ABC): @@ -155,7 +156,7 @@ class AutoTest(ABC):
return "tcp:" + self.autotest_connection_hostport()
def mavproxy_options(self):
'''returns options to be passed to MAVProxy'''
"""Returns options to be passed to MAVProxy."""
ret = ['--sitl=127.0.0.1:5501',
'--out=' + self.autotest_connection_string_from_mavproxy(),
'--streamrate=%u' % self.sitl_streamrate()]
@ -170,8 +171,7 @@ class AutoTest(ABC): @@ -170,8 +171,7 @@ class AutoTest(ABC):
return self.log_name
def apply_defaultfile_parameters(self):
'''apply parameter file'''
"""Apply parameter file."""
# setup test parameters
vinfo = vehicleinfo.VehicleInfo()
if self.params is None:
@ -192,6 +192,7 @@ class AutoTest(ABC): @@ -192,6 +192,7 @@ class AutoTest(ABC):
self.mavproxy.expect("Received [0-9]+ parameters")
def reboot_sitl(self):
"""Reboot SITL instance and wait it to reconnect."""
self.mavproxy.send("reboot\n")
self.mavproxy.expect("tilt alignment complete")
# empty mav to avoid getting old timestamps:
@ -222,7 +223,7 @@ class AutoTest(ABC): @@ -222,7 +223,7 @@ class AutoTest(ABC):
self.progress("Reboot complete")
def close(self):
'''tidy up after running all tests'''
"""Tidy up after running all tests."""
if self.use_map:
self.mavproxy.send("module unload map\n")
self.mavproxy.expect("Unloaded module map")
@ -407,7 +408,7 @@ class AutoTest(ABC): @@ -407,7 +408,7 @@ class AutoTest(ABC):
self.set_rc(3, 1000)
def armed(self):
'''Return true if vehicle is armed and safetyoff'''
"""Return true if vehicle is armed and safetyoff"""
return self.mav.motors_armed()
def arm_vehicle(self):
@ -482,6 +483,7 @@ class AutoTest(ABC): @@ -482,6 +483,7 @@ class AutoTest(ABC):
return False
def set_parameter(self, name, value, add_to_context=True):
"""Set parameters from vehicle."""
old_value = self.get_parameter(name, retry=2)
for i in range(1, 10):
self.mavproxy.send("param set %s %s\n" % (name, str(value)))
@ -489,13 +491,14 @@ class AutoTest(ABC): @@ -489,13 +491,14 @@ class AutoTest(ABC):
if returned_value == float(value):
# yes, exactly equal.
if add_to_context:
self.context_get().parameters.append( (name, old_value) )
self.context_get().parameters.append((name, old_value))
return
self.progress("Param fetch returned incorrect value (%s) vs (%s)"
% (returned_value, value))
raise ValueError()
def get_parameter(self, name, retry=1, timeout=60):
"""Get parameters from vehicle."""
for i in range(0, retry):
self.mavproxy.send("param fetch %s\n" % name)
try:
@ -506,15 +509,17 @@ class AutoTest(ABC): @@ -506,15 +509,17 @@ class AutoTest(ABC):
continue
def context_get(self):
"""Get Saved parameters."""
return self.contexts[-1]
def context_push(self):
"""Save a copy of the parameters."""
self.contexts.append(Context())
def context_pop(self):
"""Set parameters to origin values in reverse order."""
dead = self.contexts.pop()
'''set paramters to origin values in reverse order'''
dead_parameters = dead.parameters
dead_parameters.reverse()
for p in dead_parameters:
@ -533,6 +538,7 @@ class AutoTest(ABC): @@ -533,6 +538,7 @@ class AutoTest(ABC):
p6,
p7,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
"""Send a MAVLink command long."""
self.mav.mav.command_long_send(1,
1,
command,
@ -644,7 +650,7 @@ class AutoTest(ABC): @@ -644,7 +650,7 @@ class AutoTest(ABC):
self.mav.recv_match(condition='RC_CHANNELS.chan1_raw==1500',
blocking=True)
def reach_distance_manual(self, distance):
def reach_distance_manual(self, distance):
"""Manually direct the vehicle to the target distance from home."""
if self.mav.mav_type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_HELICOPTER,
@ -697,7 +703,6 @@ class AutoTest(ABC): @@ -697,7 +703,6 @@ class AutoTest(ABC):
def wait_altitude(self, alt_min, alt_max, timeout=30, relative=False):
"""Wait for a given altitude range."""
climb_rate = 0
previous_alt = 0
tstart = self.get_sim_time()
@ -836,7 +841,7 @@ class AutoTest(ABC): @@ -836,7 +841,7 @@ class AutoTest(ABC):
self.progress("Distance %.2f meters alt %.1f" % (delta, pos.alt))
if delta <= accuracy:
height_delta = math.fabs(pos.alt - target_altitude)
if (height_accuracy != -1 and height_delta > height_accuracy):
if height_accuracy != -1 and height_delta > height_accuracy:
continue
self.progress("Reached location (%.2f meters)" % delta)
return True
@ -912,11 +917,11 @@ class AutoTest(ABC): @@ -912,11 +917,11 @@ class AutoTest(ABC):
self.mav.wait_heartbeat()
while self.mav.flightmode != mode:
if (timeout is not None and
self.get_sim_time() > tstart + timeout):
self.get_sim_time() > tstart + timeout):
raise WaitModeTimeout()
self.mav.wait_heartbeat()
# self.progress("heartbeat mode %s Want: %s" % (
# self.mav.flightmode, mode))
# self.progress("heartbeat mode %s Want: %s" % (
# self.mav.flightmode, mode))
self.progress("Got mode %s" % mode)
def wait_ready_to_arm(self, timeout=None):
@ -990,11 +995,11 @@ class AutoTest(ABC): @@ -990,11 +995,11 @@ class AutoTest(ABC):
self.mav.message_hooks.append(self.message_hook)
self.mav.idle_hooks.append(self.idle_hook)
def run_test(self, desc, function, interact=False):
def run_test(self, desc, test_function, interact=False):
self.start_test(desc)
try:
function()
test_function()
except Exception as e:
self.progress('FAILED: "%s": %s' % (desc, repr(e)))
self.fail_list.append((desc, e))

13
Tools/autotest/pysim/util.py

@ -21,6 +21,8 @@ if (sys.version_info[0] >= 3): @@ -21,6 +21,8 @@ if (sys.version_info[0] >= 3):
else:
ENCODING = None
RADIUS_OF_EARTH = 6378100.0 # in meters
def m2ft(x):
"""Meters to feet."""
return float(x) / 0.3048
@ -208,6 +210,7 @@ def kill_screen_gdb(): @@ -208,6 +210,7 @@ def kill_screen_gdb():
cmd = ["screen", "-X", "-S", "ardupilot-gdb", "quit"]
subprocess.Popen(cmd)
def start_SITL(binary,
valgrind=False,
gdb=False,
@ -297,7 +300,6 @@ def start_SITL(binary, @@ -297,7 +300,6 @@ def start_SITL(binary,
first = cmd[0]
rest = cmd[1:]
child = pexpect.spawn(first, rest, logfile=sys.stdout, encoding=ENCODING, timeout=5)
delaybeforesend = 0
pexpect_autoclose(child)
# give time for parameters to properly setup
time.sleep(3)
@ -446,8 +448,6 @@ def BodyRatesToEarthRates(dcm, gyro): @@ -446,8 +448,6 @@ def BodyRatesToEarthRates(dcm, gyro):
psiDot = (q * sin(phi) + r * cos(phi)) / cos(theta)
return Vector3(phiDot, thetaDot, psiDot)
radius_of_earth = 6378100.0 # in meters
def gps_newpos(lat, lon, bearing, distance):
"""Extrapolate latitude/longitude given a heading and distance
@ -458,7 +458,7 @@ def gps_newpos(lat, lon, bearing, distance): @@ -458,7 +458,7 @@ def gps_newpos(lat, lon, bearing, distance):
lat1 = radians(lat)
lon1 = radians(lon)
brng = radians(bearing)
dr = distance / radius_of_earth
dr = distance / RADIUS_OF_EARTH
lat2 = asin(sin(lat1) * cos(dr) +
cos(lat1) * sin(dr) * cos(brng))
@ -480,7 +480,7 @@ def gps_distance(lat1, lon1, lat2, lon2): @@ -480,7 +480,7 @@ def gps_distance(lat1, lon1, lat2, lon2):
a = math.sin(0.5 * dLat)**2 + math.sin(0.5 * dLon)**2 * math.cos(lat1) * math.cos(lat2)
c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
return radius_of_earth * c
return RADIUS_OF_EARTH * c
def gps_bearing(lat1, lon1, lat2, lon2):
@ -539,7 +539,7 @@ class Wind(object): @@ -539,7 +539,7 @@ class Wind(object):
return (speed, self.direction)
# Calculate drag.
def drag(self, velocity, deltat=None, testing=None):
def drag(self, velocity, deltat=None):
"""Return current wind force in Earth frame. The velocity parameter is
a Vector3 of the current velocity of the aircraft in earth frame, m/s ."""
from math import radians
@ -633,6 +633,7 @@ def constrain(value, minv, maxv): @@ -633,6 +633,7 @@ def constrain(value, minv, maxv):
value = maxv
return value
if __name__ == "__main__":
import doctest
doctest.testmod()

4
Tools/autotest/quadplane.py

@ -65,7 +65,7 @@ class AutoTestQuadPlane(AutoTest): @@ -65,7 +65,7 @@ class AutoTestQuadPlane(AutoTest):
gdb=self.gdb,
gdbserver=self.gdbserver,
breakpoints=self.breakpoints,
)
)
self.mavproxy = util.start_MAVProxy_SITL(
'QuadPlane', options=self.mavproxy_options())
self.mavproxy.expect('Telemetry log: (\S+)\r\n')
@ -161,7 +161,7 @@ class AutoTestQuadPlane(AutoTest): @@ -161,7 +161,7 @@ class AutoTestQuadPlane(AutoTest):
"ArduPlane-Missions/Dalby-OBC2016-fence.txt")
self.run_test("Mission", lambda: self.fly_mission(m, f))
except pexpect.TIMEOUT as e:
except pexpect.TIMEOUT:
self.progress("Failed with timeout")
self.fail_list.append("Failed with timeout")

20
Tools/autotest/sim_vehicle.py

@ -80,10 +80,10 @@ class CompatOptionParser(optparse.OptionParser): @@ -80,10 +80,10 @@ class CompatOptionParser(optparse.OptionParser):
for line in help_text.split("\n"):
help_lines = tw.wrap(line)
for line in help_lines:
for wline in help_lines:
result.extend(["%*s%s\n" % (self.help_position,
"",
line)])
wline)])
elif opts[-1] != "\n":
result.append("\n")
return "".join(result)
@ -101,10 +101,10 @@ class CompatOptionParser(optparse.OptionParser): @@ -101,10 +101,10 @@ class CompatOptionParser(optparse.OptionParser):
**kwargs)
def error(self, error):
'''Override default error handler called by
"""Override default error handler called by
optparse.OptionParser.parse_args when a parse error occurs;
raise a detailed exception which can be caught
'''
"""
if error.find("no such option") != -1:
raise CompatError(error, self.values, self.rargs)
@ -504,18 +504,18 @@ def run_in_terminal_window(autotest, name, cmd): @@ -504,18 +504,18 @@ def run_in_terminal_window(autotest, name, cmd):
tracker_uarta = None # blemish
def start_antenna_tracker(autotest, cmd_opts):
def start_antenna_tracker(autotest, opts):
"""Compile and run the AntennaTracker, add tracker to mavproxy"""
global tracker_uarta
progress("Preparing antenna tracker")
tracker_home = find_location_by_name(find_autotest_dir(),
cmd_opts.tracker_location)
opts.tracker_location)
vehicledir = os.path.join(autotest, "../../" + "AntennaTracker")
opts = vinfo.options["AntennaTracker"]
tracker_default_frame = opts["default_frame"]
tracker_frame_options = opts["frames"][tracker_default_frame]
do_build(vehicledir, cmd_opts, tracker_frame_options)
options = vinfo.options["AntennaTracker"]
tracker_default_frame = options["default_frame"]
tracker_frame_options = options["frames"][tracker_default_frame]
do_build(vehicledir, opts, tracker_frame_options)
tracker_instance = 1
oldpwd = os.getcwd()
os.chdir(vehicledir)

Loading…
Cancel
Save