mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
Tools: autotest: flake8 compliance
This commit is contained in:
parent
ba0926e101
commit
88a003edd9
@ -439,7 +439,8 @@ class AutoTestRover(AutoTest):
|
|||||||
if delta < distance_without_brakes * 0.05: # 5% isn't asking for much
|
if delta < distance_without_brakes * 0.05: # 5% isn't asking for much
|
||||||
raise NotAchievedException("""
|
raise NotAchievedException("""
|
||||||
Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
||||||
""" % (distance_with_brakes,
|
""" %
|
||||||
|
(distance_with_brakes,
|
||||||
distance_without_brakes,
|
distance_without_brakes,
|
||||||
delta))
|
delta))
|
||||||
|
|
||||||
@ -488,7 +489,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
home_distance = None
|
home_distance = None
|
||||||
tstart = self.get_sim_time()
|
tstart = self.get_sim_time()
|
||||||
while self.get_sim_time() - tstart < timeout:
|
while self.get_sim_time() - tstart < timeout:
|
||||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
# m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
pos = self.mav.location()
|
pos = self.mav.location()
|
||||||
home_distance = self.get_distance(HOME, pos)
|
home_distance = self.get_distance(HOME, pos)
|
||||||
if home_distance > distance:
|
if home_distance > distance:
|
||||||
@ -506,7 +507,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.set_parameter("PRX_TYPE", 10)
|
self.set_parameter("PRX_TYPE", 10)
|
||||||
self.set_parameter("RC10_OPTION", 40) # proximity-enable
|
self.set_parameter("RC10_OPTION", 40) # proximity-enable
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
start = self.mav.location()
|
# start = self.mav.location()
|
||||||
self.wait_ready_to_arm()
|
self.wait_ready_to_arm()
|
||||||
self.arm_vehicle()
|
self.arm_vehicle()
|
||||||
# first make sure we can breach the fence:
|
# first make sure we can breach the fence:
|
||||||
@ -638,7 +639,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
def test_rc_overrides(self):
|
def test_rc_overrides(self):
|
||||||
self.context_push();
|
self.context_push()
|
||||||
ex = None
|
ex = None
|
||||||
try:
|
try:
|
||||||
self.set_parameter("RC12_OPTION", 46)
|
self.set_parameter("RC12_OPTION", 46)
|
||||||
@ -721,7 +722,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.progress("Exception caught")
|
self.progress("Exception caught")
|
||||||
ex = e
|
ex = e
|
||||||
|
|
||||||
self.context_pop();
|
self.context_pop()
|
||||||
self.reboot_sitl()
|
self.reboot_sitl()
|
||||||
|
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
@ -752,7 +753,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
expected_distance = 2
|
expected_distance = 2
|
||||||
elif mc.seq == 4:
|
elif mc.seq == 4:
|
||||||
expected_distance = 5
|
expected_distance = 5
|
||||||
elif mc.seq ==5:
|
elif mc.seq == 5:
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
continue
|
continue
|
||||||
@ -820,7 +821,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.drive_square)
|
self.drive_square)
|
||||||
|
|
||||||
self.run_test("Drive Mission %s" % "rover1.txt",
|
self.run_test("Drive Mission %s" % "rover1.txt",
|
||||||
lambda : self.drive_mission("rover1.txt"))
|
lambda: self.drive_mission("rover1.txt"))
|
||||||
|
|
||||||
# disabled due to frequent failures in travis. This test needs re-writing
|
# 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)
|
||||||
@ -855,7 +856,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||||||
self.run_test("Download logs", lambda:
|
self.run_test("Download logs", lambda:
|
||||||
self.log_download(
|
self.log_download(
|
||||||
self.buildlogs_path("APMrover2-log.bin"),
|
self.buildlogs_path("APMrover2-log.bin"),
|
||||||
upload_logs=len(self.fail_list)>0))
|
upload_logs=len(self.fail_list) > 0))
|
||||||
# if not drive_left_circuit(self):
|
# if not drive_left_circuit(self):
|
||||||
# self.progress("Failed left circuit")
|
# self.progress("Failed left circuit")
|
||||||
# failed = True
|
# failed = True
|
||||||
|
@ -11,7 +11,6 @@ import traceback
|
|||||||
import pexpect
|
import pexpect
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
from pymavlink import mavextra
|
from pymavlink import mavextra
|
||||||
from pymavlink import quaternion
|
|
||||||
|
|
||||||
from pysim import util, rotmat
|
from pysim import util, rotmat
|
||||||
|
|
||||||
@ -1397,8 +1396,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
if now - tstart > 120:
|
if now - tstart > 120:
|
||||||
raise AutoTestTimeoutException("Did not disarm as expected")
|
raise AutoTestTimeoutException("Did not disarm as expected")
|
||||||
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
||||||
if ((now - last_mission_current_msg) > 1 or
|
if ((now - last_mission_current_msg) > 1 or m.seq != last_seq):
|
||||||
m.seq != last_seq):
|
|
||||||
dist = None
|
dist = None
|
||||||
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
|
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
|
||||||
if x is not None:
|
if x is not None:
|
||||||
@ -1539,12 +1537,14 @@ class AutoTestCopter(AutoTest):
|
|||||||
command = mavutil.mavlink.MAV_CMD_NAV_DELAY
|
command = mavutil.mavlink.MAV_CMD_NAV_DELAY
|
||||||
# retrieve mission item and check it:
|
# retrieve mission item and check it:
|
||||||
tried_set = False
|
tried_set = False
|
||||||
|
hours = None
|
||||||
|
mins = None
|
||||||
|
secs = None
|
||||||
while True:
|
while True:
|
||||||
self.progress("Requesting item")
|
self.progress("Requesting item")
|
||||||
self.mav.mav.mission_request_send(1,
|
self.mav.mav.mission_request_send(1,
|
||||||
1,
|
1,
|
||||||
seq
|
seq)
|
||||||
)
|
|
||||||
st = self.mav.recv_match(type='MISSION_ITEM',
|
st = self.mav.recv_match(type='MISSION_ITEM',
|
||||||
blocking=True,
|
blocking=True,
|
||||||
timeout=1)
|
timeout=1)
|
||||||
@ -1552,13 +1552,15 @@ class AutoTestCopter(AutoTest):
|
|||||||
continue
|
continue
|
||||||
|
|
||||||
print("Item: %s" % str(st))
|
print("Item: %s" % str(st))
|
||||||
if (tried_set and
|
have_match = (tried_set and
|
||||||
st.seq == seq and
|
st.seq == seq and
|
||||||
st.command == command and
|
st.command == command and
|
||||||
st.param2 == hours and
|
st.param2 == hours and
|
||||||
st.param3 == mins and
|
st.param3 == mins and
|
||||||
st.param4 == secs):
|
st.param4 == secs)
|
||||||
|
if have_match:
|
||||||
return
|
return
|
||||||
|
|
||||||
self.progress("Mission mismatch")
|
self.progress("Mission mismatch")
|
||||||
|
|
||||||
m = None
|
m = None
|
||||||
@ -1675,6 +1677,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
|
|
||||||
def fly_nav_takeoff_delay_abstime(self):
|
def fly_nav_takeoff_delay_abstime(self):
|
||||||
"""make sure taking off at a specific time works"""
|
"""make sure taking off at a specific time works"""
|
||||||
|
global num_wp
|
||||||
num_wp = self.load_mission("copter_nav_delay_takeoff.txt")
|
num_wp = self.load_mission("copter_nav_delay_takeoff.txt")
|
||||||
|
|
||||||
self.progress("Starting mission")
|
self.progress("Starting mission")
|
||||||
@ -1688,9 +1691,9 @@ class AutoTestCopter(AutoTest):
|
|||||||
delay_item_seq = 2
|
delay_item_seq = 2
|
||||||
self.reset_delay_item_seventyseven(delay_item_seq)
|
self.reset_delay_item_seventyseven(delay_item_seq)
|
||||||
delay_for_seconds = 77
|
delay_for_seconds = 77
|
||||||
reset_at = self.get_sim_time_cached();
|
reset_at = self.get_sim_time_cached()
|
||||||
|
|
||||||
self.context_push();
|
self.context_push()
|
||||||
|
|
||||||
ex = None
|
ex = None
|
||||||
try:
|
try:
|
||||||
@ -2132,8 +2135,8 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.mav.mav.mount_control_send(
|
self.mav.mav.mount_control_send(
|
||||||
1, # target system
|
1, # target system
|
||||||
1, # target component
|
1, # target component
|
||||||
20 *100, # pitch
|
20 * 100, # pitch
|
||||||
20 *100, # roll (centidegrees)
|
20 * 100, # roll (centidegrees)
|
||||||
0, # yaw
|
0, # yaw
|
||||||
0 # save position
|
0 # save position
|
||||||
)
|
)
|
||||||
@ -2318,6 +2321,9 @@ class AutoTestCopter(AutoTest):
|
|||||||
|
|
||||||
self.reboot_sitl() # to handle MNT_TYPE changing
|
self.reboot_sitl() # to handle MNT_TYPE changing
|
||||||
|
|
||||||
|
if ex is not None:
|
||||||
|
raise ex
|
||||||
|
|
||||||
def fly_precision_companion(self):
|
def fly_precision_companion(self):
|
||||||
"""Use Companion PrecLand backend precision messages to loiter."""
|
"""Use Companion PrecLand backend precision messages to loiter."""
|
||||||
|
|
||||||
@ -2612,7 +2618,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.run_test("Test gripper", self.test_gripper)
|
self.run_test("Test gripper", self.test_gripper)
|
||||||
|
|
||||||
self.run_test("Test gripper mission items",
|
self.run_test("Test gripper mission items",
|
||||||
self.test_gripper_mission);
|
self.test_gripper_mission)
|
||||||
|
|
||||||
'''vision position''' # expects vehicle to be disarmed
|
'''vision position''' # expects vehicle to be disarmed
|
||||||
self.run_test("Fly Vision Position", self.fly_vision_position)
|
self.run_test("Fly Vision Position", self.fly_vision_position)
|
||||||
@ -2624,7 +2630,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.run_test("log download",
|
self.run_test("log download",
|
||||||
lambda: self.log_download(
|
lambda: self.log_download(
|
||||||
self.buildlogs_path("ArduCopter-log.bin"),
|
self.buildlogs_path("ArduCopter-log.bin"),
|
||||||
upload_logs=len(self.fail_list)>0))
|
upload_logs=len(self.fail_list) > 0))
|
||||||
|
|
||||||
except pexpect.TIMEOUT:
|
except pexpect.TIMEOUT:
|
||||||
self.progress("Failed with timeout")
|
self.progress("Failed with timeout")
|
||||||
@ -2671,7 +2677,7 @@ class AutoTestCopter(AutoTest):
|
|||||||
self.run_test("log download",
|
self.run_test("log download",
|
||||||
lambda: self.log_download(
|
lambda: self.log_download(
|
||||||
self.buildlogs_path("Helicopter-log.bin"),
|
self.buildlogs_path("Helicopter-log.bin"),
|
||||||
upload_logs=len(self.fail_list)>0))
|
upload_logs=len(self.fail_list) > 0))
|
||||||
|
|
||||||
except pexpect.TIMEOUT:
|
except pexpect.TIMEOUT:
|
||||||
self.fail_list.append("Failed with timeout")
|
self.fail_list.append("Failed with timeout")
|
||||||
|
@ -362,7 +362,7 @@ class AutoTestPlane(AutoTest):
|
|||||||
type_mask = 0b10000001 ^ 0xFF # FIXME
|
type_mask = 0b10000001 ^ 0xFF # FIXME
|
||||||
# attitude in radians:
|
# attitude in radians:
|
||||||
q = quaternion.Quaternion([math.radians(target_roll_degrees),
|
q = quaternion.Quaternion([math.radians(target_roll_degrees),
|
||||||
0 ,
|
0,
|
||||||
0])
|
0])
|
||||||
roll_rate_radians = 0.5
|
roll_rate_radians = 0.5
|
||||||
pitch_rate_radians = 0
|
pitch_rate_radians = 0
|
||||||
@ -594,14 +594,13 @@ class AutoTestPlane(AutoTest):
|
|||||||
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
|
||||||
time_delta = (self.get_sim_time_cached() -
|
time_delta = (self.get_sim_time_cached() -
|
||||||
last_mission_current_msg)
|
last_mission_current_msg)
|
||||||
if (time_delta >1 or
|
if (time_delta > 1 or m.seq != last_seq):
|
||||||
m.seq != last_seq):
|
|
||||||
dist = None
|
dist = None
|
||||||
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
|
x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None)
|
||||||
if x is not None:
|
if x is not None:
|
||||||
dist = x.wp_dist
|
dist = x.wp_dist
|
||||||
self.progress("MISSION_CURRENT.seq=%u (dist=%s)" %
|
self.progress("MISSION_CURRENT.seq=%u (dist=%s)" %
|
||||||
(m.seq,str(dist)))
|
(m.seq, str(dist)))
|
||||||
last_mission_current_msg = self.get_sim_time_cached()
|
last_mission_current_msg = self.get_sim_time_cached()
|
||||||
last_seq = m.seq
|
last_seq = m.seq
|
||||||
# flaps should undeploy at the end
|
# flaps should undeploy at the end
|
||||||
@ -767,6 +766,6 @@ class AutoTestPlane(AutoTest):
|
|||||||
self.progress("FAILED: %s" % self.fail_list)
|
self.progress("FAILED: %s" % self.fail_list)
|
||||||
return False
|
return False
|
||||||
|
|
||||||
self.progress("Max set_rc_timeout=%s" % self.max_set_rc_timeout);
|
self.progress("Max set_rc_timeout=%s" % self.max_set_rc_timeout)
|
||||||
|
|
||||||
return True
|
return True
|
||||||
|
@ -186,12 +186,12 @@ class AutoTestSub(AutoTest):
|
|||||||
lambda: self.dive_mission("sub_mission.txt"))
|
lambda: self.dive_mission("sub_mission.txt"))
|
||||||
|
|
||||||
self.run_test("Test gripper mission items",
|
self.run_test("Test gripper mission items",
|
||||||
self.test_gripper_mission);
|
self.test_gripper_mission)
|
||||||
|
|
||||||
self.run_test("Log download",
|
self.run_test("Log download",
|
||||||
lambda: self.log_download(
|
lambda: self.log_download(
|
||||||
self.buildlogs_path("ArduSub-log.bin"),
|
self.buildlogs_path("ArduSub-log.bin"),
|
||||||
upload_logs=len(self.fail_list)>0))
|
upload_logs=len(self.fail_list) > 0))
|
||||||
|
|
||||||
except pexpect.TIMEOUT:
|
except pexpect.TIMEOUT:
|
||||||
self.progress("Failed with timeout")
|
self.progress("Failed with timeout")
|
||||||
|
@ -302,7 +302,6 @@ class AutoTest(ABC):
|
|||||||
count += 1
|
count += 1
|
||||||
self.progress("Drained %u messages from mav" % count)
|
self.progress("Drained %u messages from mav" % count)
|
||||||
|
|
||||||
|
|
||||||
#################################################
|
#################################################
|
||||||
# SIM UTILITIES
|
# SIM UTILITIES
|
||||||
#################################################
|
#################################################
|
||||||
@ -366,11 +365,13 @@ class AutoTest(ABC):
|
|||||||
self.mav.wait_heartbeat()
|
self.mav.wait_heartbeat()
|
||||||
if upload_logs and not os.getenv("AUTOTEST_NO_UPLOAD"):
|
if upload_logs and not os.getenv("AUTOTEST_NO_UPLOAD"):
|
||||||
# optionally upload logs to server so we can see travis failure logs
|
# optionally upload logs to server so we can see travis failure logs
|
||||||
import subprocess, glob, datetime
|
import datetime
|
||||||
logdir=os.path.dirname(filename)
|
import glob
|
||||||
datedir=datetime.datetime.now().strftime("%Y-%m-%d-%H-%M")
|
import subprocess
|
||||||
|
logdir = os.path.dirname(filename)
|
||||||
|
datedir = datetime.datetime.now().strftime("%Y-%m-%d-%H-%M")
|
||||||
flist = glob.glob("logs/*.BIN")
|
flist = glob.glob("logs/*.BIN")
|
||||||
for e in ['BIN','bin','tlog']:
|
for e in ['BIN', 'bin', 'tlog']:
|
||||||
flist += glob.glob(os.path.join(logdir, '*.%s' % e))
|
flist += glob.glob(os.path.join(logdir, '*.%s' % e))
|
||||||
print("Uploading %u logs to http://firmware.ardupilot.org/CI-Logs/%s" % (len(flist), datedir))
|
print("Uploading %u logs to http://firmware.ardupilot.org/CI-Logs/%s" % (len(flist), datedir))
|
||||||
cmd = ['rsync', '-avz'] + flist + ['cilogs@autotest.ardupilot.org::CI-Logs/%s/' % datedir]
|
cmd = ['rsync', '-avz'] + flist + ['cilogs@autotest.ardupilot.org::CI-Logs/%s/' % datedir]
|
||||||
@ -402,7 +403,7 @@ class AutoTest(ABC):
|
|||||||
self.progress("Comparing (%s) and (%s)" % (file1, file2, ))
|
self.progress("Comparing (%s) and (%s)" % (file1, file2, ))
|
||||||
f1 = open(file1)
|
f1 = open(file1)
|
||||||
f2 = open(file2)
|
f2 = open(file2)
|
||||||
for l1,l2 in itertools.izip(f1,f2):
|
for l1, l2 in itertools.izip(f1, f2):
|
||||||
if l1 == l2:
|
if l1 == l2:
|
||||||
# e.g. the first "QGC WPL 110" line
|
# e.g. the first "QGC WPL 110" line
|
||||||
continue
|
continue
|
||||||
@ -413,9 +414,9 @@ class AutoTest(ABC):
|
|||||||
l2 = l2.rstrip()
|
l2 = l2.rstrip()
|
||||||
fields1 = re.split("\s+", l1)
|
fields1 = re.split("\s+", l1)
|
||||||
fields2 = re.split("\s+", l2)
|
fields2 = re.split("\s+", l2)
|
||||||
line = int(fields1[0])
|
# line = int(fields1[0])
|
||||||
t = int(fields1[3]) # mission item type
|
t = int(fields1[3]) # mission item type
|
||||||
for (count, (i1,i2)) in enumerate(itertools.izip(fields1, fields2)):
|
for (count, (i1, i2)) in enumerate(itertools.izip(fields1, fields2)):
|
||||||
if count == 2: # frame
|
if count == 2: # frame
|
||||||
if t in [mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
|
if t in [mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
|
||||||
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
|
mavutil.mavlink.MAV_CMD_CONDITION_YAW,
|
||||||
@ -445,15 +446,16 @@ class AutoTest(ABC):
|
|||||||
i2 = 1
|
i2 = 1
|
||||||
if 0 <= count <= 3 or 11 <= count <= 11:
|
if 0 <= count <= 3 or 11 <= count <= 11:
|
||||||
if int(i1) != int(i2):
|
if int(i1) != int(i2):
|
||||||
raise ValueError("Files have different content: (%s vs %s) (%s vs %s) (%d vs %d) (count=%u)" % (file1, file2, l1, l2, int(i1), int(i2), count)) # NOCI
|
raise ValueError("Files have different content: (%s vs %s) (%s vs %s) (%d vs %d) (count=%u)" %
|
||||||
|
(file1, file2, l1, l2, int(i1), int(i2), count)) # NOCI
|
||||||
continue
|
continue
|
||||||
if 4 <= count <= 10:
|
if 4 <= count <= 10:
|
||||||
delta = abs(float(i1) - float(i2))
|
delta = abs(float(i1) - float(i2))
|
||||||
max_allowed_delta = 0.000009
|
max_allowed_delta = 0.000009
|
||||||
if delta > max_allowed_delta:
|
if delta > max_allowed_delta:
|
||||||
# print("Files have different (float) content: (%s) and (%s) (%s vs %s) (%f vs %f) (%.10f) (count=%u)" % (file1, file2, l1, l2, float(i1), float(i2), delta, count))
|
raise ValueError("Files have different (float) content: (%s) and " +
|
||||||
# sys.exit(0)
|
"(%s) (%s vs %s) (%f vs %f) (%.10f) (count=%u)" %
|
||||||
raise ValueError("Files have different (float) content: (%s) and (%s) (%s vs %s) (%f vs %f) (%.10f) (count=%u)" % (file1, file2, l1, l2, float(i1), float(i2), delta, count)) # NOCI
|
(file1, file2, l1, l2, float(i1), float(i2), delta, count)) # NOCI
|
||||||
continue
|
continue
|
||||||
raise ValueError("count %u not handled" % count)
|
raise ValueError("count %u not handled" % count)
|
||||||
self.progress("Files same")
|
self.progress("Files same")
|
||||||
@ -546,8 +548,7 @@ class AutoTest(ABC):
|
|||||||
if delta > self.max_set_rc_timeout:
|
if delta > self.max_set_rc_timeout:
|
||||||
self.max_set_rc_timeout = delta
|
self.max_set_rc_timeout = delta
|
||||||
return True
|
return True
|
||||||
raise SetRCTimeout((
|
raise SetRCTimeout("Failed to send RC commands to channel %s" % str(chan))
|
||||||
"Failed to send RC commands to channel %s" % str(chan)))
|
|
||||||
|
|
||||||
def set_throttle_zero(self):
|
def set_throttle_zero(self):
|
||||||
"""Set throttle to zero."""
|
"""Set throttle to zero."""
|
||||||
@ -1068,7 +1069,7 @@ class AutoTest(ABC):
|
|||||||
if self.get_sim_time_cached() - last_print > 1:
|
if self.get_sim_time_cached() - last_print > 1:
|
||||||
self.progress("Wait groundspeed %.1f, target:%.1f" %
|
self.progress("Wait groundspeed %.1f, target:%.1f" %
|
||||||
(m.groundspeed, gs_min))
|
(m.groundspeed, gs_min))
|
||||||
last_print = self.get_sim_time_cached();
|
last_print = self.get_sim_time_cached()
|
||||||
if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
|
if m.groundspeed >= gs_min and m.groundspeed <= gs_max:
|
||||||
return True
|
return True
|
||||||
raise WaitGroundSpeedTimeout("Failed to attain groundspeed range")
|
raise WaitGroundSpeedTimeout("Failed to attain groundspeed range")
|
||||||
@ -1113,8 +1114,8 @@ class AutoTest(ABC):
|
|||||||
break
|
break
|
||||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||||
if now - last_print_time > 1:
|
if now - last_print_time > 1:
|
||||||
self.progress("Heading %u (want %f +- %f)" % (
|
self.progress("Heading %u (want %f +- %f)" %
|
||||||
m.heading, heading, accuracy))
|
(m.heading, heading, accuracy))
|
||||||
last_print_time = now
|
last_print_time = now
|
||||||
if math.fabs(m.heading - heading) <= accuracy:
|
if math.fabs(m.heading - heading) <= accuracy:
|
||||||
self.progress("Attained heading %u" % heading)
|
self.progress("Attained heading %u" % heading)
|
||||||
@ -1137,8 +1138,7 @@ class AutoTest(ABC):
|
|||||||
self.progress("Attained distance %.2f meters OK" % delta)
|
self.progress("Attained distance %.2f meters OK" % delta)
|
||||||
return True
|
return True
|
||||||
if delta > (distance + accuracy):
|
if delta > (distance + accuracy):
|
||||||
raise WaitDistanceTimeout(
|
raise WaitDistanceTimeout("Failed distance - overshoot delta=%f dist=%f"
|
||||||
"Failed distance - overshoot delta=%f dist=%f"
|
|
||||||
% (delta, distance))
|
% (delta, distance))
|
||||||
raise WaitDistanceTimeout("Failed to attain distance %u" % distance)
|
raise WaitDistanceTimeout("Failed to attain distance %u" % distance)
|
||||||
|
|
||||||
@ -1195,7 +1195,7 @@ class AutoTest(ABC):
|
|||||||
seq = self.mav.waypoint_current()
|
seq = self.mav.waypoint_current()
|
||||||
self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
|
self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
|
||||||
if seq == wpnum:
|
if seq == wpnum:
|
||||||
break;
|
break
|
||||||
|
|
||||||
def wait_waypoint(self,
|
def wait_waypoint(self,
|
||||||
wpnum_start,
|
wpnum_start,
|
||||||
@ -1249,8 +1249,7 @@ class AutoTest(ABC):
|
|||||||
self.progress("Reached final waypoint %u" % seq)
|
self.progress("Reached final waypoint %u" % seq)
|
||||||
return True
|
return True
|
||||||
if seq > current_wp+1:
|
if seq > current_wp+1:
|
||||||
raise WaitWaypointTimeout((
|
raise WaitWaypointTimeout(("Skipped waypoint! Got wp %u expected %u"
|
||||||
"Skipped waypoint! Got wp %u expected %u"
|
|
||||||
% (seq, current_wp+1)))
|
% (seq, current_wp+1)))
|
||||||
raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" %
|
raise WaitWaypointTimeout("Timed out waiting for waypoint %u of %u" %
|
||||||
(wpnum_end, wpnum_end))
|
(wpnum_end, wpnum_end))
|
||||||
@ -1525,8 +1524,7 @@ class AutoTest(ABC):
|
|||||||
(channel_field, m_value, interlock_value))
|
(channel_field, m_value, interlock_value))
|
||||||
if m_value != interlock_value:
|
if m_value != interlock_value:
|
||||||
self.set_rc(8, 1000)
|
self.set_rc(8, 1000)
|
||||||
raise NotAchievedException(
|
raise NotAchievedException("Motor interlock was changed while disarmed")
|
||||||
"Motor interlock was changed while disarmed")
|
|
||||||
|
|
||||||
self.set_rc(8, 1000)
|
self.set_rc(8, 1000)
|
||||||
self.progress("ALL PASS")
|
self.progress("ALL PASS")
|
||||||
|
Loading…
Reference in New Issue
Block a user