mirror of https://github.com/ArduPilot/ardupilot
autotest: CI fixes for 4.4
This commit is contained in:
parent
b06ba1bacd
commit
379a905abd
|
@ -40,7 +40,7 @@ def check_log(logfile, progress=print, ekf2_only=False, ekf3_only=False, verbose
|
|||
if not hasattr(m,'C'):
|
||||
continue
|
||||
mtype = m.get_type()
|
||||
if not mtype in counts:
|
||||
if mtype not in counts:
|
||||
counts[mtype] = 0
|
||||
base_counts[mtype] = 0
|
||||
core = m.C
|
||||
|
|
|
@ -134,7 +134,7 @@ class AutoTestCopter(AutoTest):
|
|||
0, # param6
|
||||
alt_min # param7
|
||||
)
|
||||
self.wait_for_alt(alt_min, timeout=timeout, max_err=max_err)
|
||||
self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout)
|
||||
|
||||
def takeoff(self,
|
||||
alt_min=30,
|
||||
|
@ -154,17 +154,10 @@ class AutoTestCopter(AutoTest):
|
|||
self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err)
|
||||
else:
|
||||
self.set_rc(3, takeoff_throttle)
|
||||
self.wait_for_alt(alt_min=alt_min, timeout=timeout, max_err=max_err)
|
||||
self.wait_altitude(alt_min-1, alt_min+max_err, relative=True, timeout=timeout)
|
||||
self.hover()
|
||||
self.progress("TAKEOFF COMPLETE")
|
||||
|
||||
def wait_for_alt(self, alt_min=30, timeout=30, max_err=5):
|
||||
"""Wait for minimum altitude to be reached."""
|
||||
self.wait_altitude(alt_min - 1,
|
||||
(alt_min + max_err),
|
||||
relative=True,
|
||||
timeout=timeout)
|
||||
|
||||
def land_and_disarm(self, timeout=60):
|
||||
"""Land the quad."""
|
||||
self.progress("STARTING LANDING")
|
||||
|
@ -176,7 +169,7 @@ class AutoTestCopter(AutoTest):
|
|||
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||
alt = m.relative_alt / 1000.0 # mm -> m
|
||||
if alt > min_alt:
|
||||
self.wait_for_alt(min_alt, timeout=timeout)
|
||||
self.wait_altitude(min_alt-1, min_alt+5, relative=True, timeout=timeout)
|
||||
# self.wait_statustext("SIM Hit ground", timeout=timeout)
|
||||
self.wait_disarmed()
|
||||
|
||||
|
@ -2086,7 +2079,7 @@ class AutoTestCopter(AutoTest):
|
|||
|
||||
self.progress("Regaining altitude")
|
||||
self.change_mode('ALT_HOLD')
|
||||
self.wait_for_alt(20, max_err=40)
|
||||
self.wait_altitude(19, 60, relative=True)
|
||||
|
||||
self.progress("Flipping in pitch")
|
||||
self.set_rc(2, 1700)
|
||||
|
@ -2426,7 +2419,7 @@ class AutoTestCopter(AutoTest):
|
|||
raise NotAchievedException("AUTOTUNE gains not present in pilot testing")
|
||||
# land without changing mode
|
||||
self.set_rc(3, 1000)
|
||||
self.wait_for_alt(0)
|
||||
self.wait_altitude(-1, 5, relative=True)
|
||||
self.wait_disarmed()
|
||||
# Check gains are still there after disarm
|
||||
if (rlld == self.get_parameter("ATC_RAT_RLL_D") or
|
||||
|
@ -2677,7 +2670,7 @@ class AutoTestCopter(AutoTest):
|
|||
0,
|
||||
timeout=10,
|
||||
want_result=mavutil.mavlink.MAV_RESULT_FAILED)
|
||||
self.wait_statustext("Node {} unhealthy".format(gps1_nodeid), check_context=True)
|
||||
self.wait_statustext(".*Node .* unhealthy", check_context=True, regex=True)
|
||||
self.stop_sup_program(instance=0)
|
||||
self.start_sup_program(instance=0)
|
||||
self.context_stop_collecting('STATUSTEXT')
|
||||
|
@ -3850,7 +3843,7 @@ class AutoTestCopter(AutoTest):
|
|||
new_pos = self.mav.location()
|
||||
delta = self.get_distance(target, new_pos)
|
||||
self.progress("Landed %f metres from target position" % delta)
|
||||
max_delta = 1
|
||||
max_delta = 1.5
|
||||
if delta > max_delta:
|
||||
raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (delta, max_delta))
|
||||
|
||||
|
@ -4567,7 +4560,7 @@ class AutoTestCopter(AutoTest):
|
|||
# determine if we've successfully navigated to close to
|
||||
# where we should be:
|
||||
dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y)
|
||||
dist_max = 0.15
|
||||
dist_max = 1
|
||||
self.progress("dist=%f want <%f" % (dist, dist_max))
|
||||
if dist < dist_max:
|
||||
# success! We've gotten within our target distance
|
||||
|
@ -4645,6 +4638,7 @@ class AutoTestCopter(AutoTest):
|
|||
|
||||
except Exception as e:
|
||||
self.print_exception_caught(e)
|
||||
self.disarm_vehicle(force=True)
|
||||
ex = e
|
||||
|
||||
self.context_pop()
|
||||
|
@ -6825,6 +6819,7 @@ class AutoTestCopter(AutoTest):
|
|||
failed = False
|
||||
wants = []
|
||||
gots = []
|
||||
epsilon = 20
|
||||
while True:
|
||||
if self.get_sim_time_cached() - tstart > 30:
|
||||
raise AutoTestTimeoutException("Failed to get distances")
|
||||
|
@ -6837,7 +6832,7 @@ class AutoTestCopter(AutoTest):
|
|||
want = expected_distances_copy[m.orientation]
|
||||
wants.append(want)
|
||||
gots.append(got)
|
||||
if abs(want - got) > 5:
|
||||
if abs(want - got) > epsilon:
|
||||
failed = True
|
||||
del expected_distances_copy[m.orientation]
|
||||
if failed:
|
||||
|
@ -6856,9 +6851,11 @@ class AutoTestCopter(AutoTest):
|
|||
})
|
||||
self.set_analog_rangefinder_parameters()
|
||||
self.reboot_sitl()
|
||||
tstart = self.get_sim_time()
|
||||
|
||||
self.change_mode('LOITER')
|
||||
self.wait_ekf_happy()
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.armed():
|
||||
break
|
||||
|
@ -6909,7 +6906,7 @@ class AutoTestCopter(AutoTest):
|
|||
if self.get_sim_time_cached() - tstart > 10:
|
||||
break
|
||||
vel = self.get_body_frame_velocity()
|
||||
if vel.length() > 0.3:
|
||||
if vel.length() > 0.5:
|
||||
raise NotAchievedException("Moved too much (%s)" %
|
||||
(str(vel),))
|
||||
shove(None, None)
|
||||
|
@ -8337,7 +8334,7 @@ class AutoTestCopter(AutoTest):
|
|||
def verify_yaw(mav, m):
|
||||
if m.get_type() != 'ATTITUDE':
|
||||
return
|
||||
yawspeed_thresh_rads = math.radians(10)
|
||||
yawspeed_thresh_rads = math.radians(20)
|
||||
if m.yawspeed > yawspeed_thresh_rads:
|
||||
raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %
|
||||
(math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))
|
||||
|
|
|
@ -20,6 +20,8 @@ from common import AutoTestTimeoutException
|
|||
from common import NotAchievedException
|
||||
from common import PreconditionFailedException
|
||||
from common import WaitModeTimeout
|
||||
from common import Test
|
||||
|
||||
from pymavlink.rotmat import Vector3
|
||||
from pysim import vehicleinfo
|
||||
|
||||
|
@ -780,6 +782,7 @@ class AutoTestPlane(AutoTest):
|
|||
0,
|
||||
0)
|
||||
self.wait_airspeed(new_target_airspeed-0.5, new_target_airspeed+0.5)
|
||||
self.context_push()
|
||||
self.progress("Adding some wind, hoping groundspeed increases/decreases")
|
||||
self.set_parameters({
|
||||
"SIM_WIND_SPD": 7,
|
||||
|
@ -797,6 +800,7 @@ class AutoTestPlane(AutoTest):
|
|||
self.progress("groundspeed and airspeed should be different (have=%f want=%f)" % (delta, want_delta))
|
||||
if delta > want_delta:
|
||||
break
|
||||
self.context_pop()
|
||||
self.fly_home_land_and_disarm(timeout=240)
|
||||
|
||||
def fly_home_land_and_disarm(self, timeout=120):
|
||||
|
@ -994,7 +998,7 @@ class AutoTestPlane(AutoTest):
|
|||
self.context_collect("HEARTBEAT")
|
||||
self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950
|
||||
self.wait_mode('RTL') # long failsafe
|
||||
if (not self.get_mode_from_mode_mapping("CIRCLE") in
|
||||
if (self.get_mode_from_mode_mapping("CIRCLE") not in
|
||||
[x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]):
|
||||
raise NotAchievedException("Did not go via circle mode")
|
||||
self.progress("Ensure we've had our throttle squashed to 950")
|
||||
|
@ -1032,7 +1036,7 @@ class AutoTestPlane(AutoTest):
|
|||
self.context_collect("HEARTBEAT")
|
||||
self.set_parameter("SIM_RC_FAIL", 1) # no-pulses
|
||||
self.wait_mode('RTL') # long failsafe
|
||||
if (not self.get_mode_from_mode_mapping("CIRCLE") in
|
||||
if (self.get_mode_from_mode_mapping("CIRCLE") not in
|
||||
[x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]):
|
||||
raise NotAchievedException("Did not go via circle mode")
|
||||
self.do_timesync_roundtrip()
|
||||
|
@ -1169,29 +1173,28 @@ class AutoTestPlane(AutoTest):
|
|||
'''Ensure Long-Failsafe works on GCS loss'''
|
||||
self.start_subtest("Test Failsafe: RTL")
|
||||
self.load_sample_mission()
|
||||
self.set_parameter("RTL_AUTOLAND", 1)
|
||||
self.change_mode("AUTO")
|
||||
self.takeoff()
|
||||
self.set_parameters({
|
||||
"FS_GCS_ENABL": 1,
|
||||
"FS_LONG_ACTN": 1,
|
||||
"RTL_AUTOLAND": 1,
|
||||
"SYSID_MYGCS": self.mav.source_system,
|
||||
})
|
||||
self.takeoff()
|
||||
self.change_mode('LOITER')
|
||||
self.progress("Disconnecting GCS")
|
||||
self.set_heartbeat_rate(0)
|
||||
self.wait_mode("RTL", timeout=5)
|
||||
self.wait_mode("RTL", timeout=10)
|
||||
self.set_heartbeat_rate(self.speedup)
|
||||
self.end_subtest("Completed RTL Failsafe test")
|
||||
|
||||
self.start_subtest("Test Failsafe: FBWA Glide")
|
||||
self.set_parameters({
|
||||
"RTL_AUTOLAND": 1,
|
||||
"FS_LONG_ACTN": 2,
|
||||
})
|
||||
self.change_mode("AUTO")
|
||||
self.takeoff()
|
||||
self.change_mode('AUTO')
|
||||
self.progress("Disconnecting GCS")
|
||||
self.set_heartbeat_rate(0)
|
||||
self.wait_mode("FBWA", timeout=5)
|
||||
self.wait_mode("FBWA", timeout=10)
|
||||
self.set_heartbeat_rate(self.speedup)
|
||||
self.end_subtest("Completed FBWA Failsafe test")
|
||||
|
||||
|
@ -1844,6 +1847,7 @@ class AutoTestPlane(AutoTest):
|
|||
self.fly_home_land_and_disarm()
|
||||
|
||||
def deadreckoning_main(self, disable_airspeed_sensor=False):
|
||||
self.reboot_sitl()
|
||||
self.wait_ready_to_arm()
|
||||
self.gpi = None
|
||||
self.simstate = None
|
||||
|
@ -2414,11 +2418,6 @@ class AutoTestPlane(AutoTest):
|
|||
|
||||
self.load_mission('CMAC-soar.txt', strict=False)
|
||||
|
||||
self.set_current_waypoint(1)
|
||||
self.change_mode('AUTO')
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
|
||||
# Enable thermalling RC
|
||||
rc_chan = 0
|
||||
for i in range(8):
|
||||
|
@ -2432,15 +2431,22 @@ class AutoTestPlane(AutoTest):
|
|||
|
||||
self.set_rc_from_map({
|
||||
rc_chan: 1900,
|
||||
3: 1500, # Use trim airspeed.
|
||||
})
|
||||
|
||||
self.set_parameters({
|
||||
"SOAR_VSPEED": 0.55,
|
||||
"SOAR_MIN_THML_S": 25,
|
||||
})
|
||||
|
||||
self.set_current_waypoint(1)
|
||||
self.change_mode('AUTO')
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
|
||||
# Wait to detect thermal
|
||||
self.progress("Waiting for thermal")
|
||||
self.wait_mode('THERMAL', timeout=600)
|
||||
|
||||
self.set_parameter("SOAR_VSPEED", 0.6)
|
||||
|
||||
# Wait to climb to SOAR_ALT_MAX
|
||||
self.progress("Waiting for climb to max altitude")
|
||||
alt_max = self.get_parameter('SOAR_ALT_MAX')
|
||||
|
@ -4569,7 +4575,7 @@ class AutoTestPlane(AutoTest):
|
|||
self.AltResetBadGPS,
|
||||
self.AirspeedCal,
|
||||
self.MissionJumpTags,
|
||||
self.GCSFailsafe,
|
||||
Test(self.GCSFailsafe, speedup=8),
|
||||
self.SDCardWPTest,
|
||||
])
|
||||
return ret
|
||||
|
|
|
@ -33,7 +33,6 @@ import helicopter
|
|||
|
||||
import examples
|
||||
from pysim import util
|
||||
from pymavlink import mavutil
|
||||
from pymavlink.generator import mavtemplate
|
||||
|
||||
from common import Test
|
||||
|
@ -58,47 +57,6 @@ def buildlogs_path(path):
|
|||
return os.path.join(*bits)
|
||||
|
||||
|
||||
def get_default_params(atype, binary):
|
||||
"""Get default parameters."""
|
||||
# use rover simulator so SITL is not starved of input
|
||||
HOME = mavutil.location(40.071374969556928,
|
||||
-105.22978898137808,
|
||||
1583.702759,
|
||||
246)
|
||||
if "plane" in binary or "rover" in binary:
|
||||
frame = "rover"
|
||||
else:
|
||||
frame = "+"
|
||||
|
||||
home = "%f,%f,%u,%u" % (HOME.lat, HOME.lng, HOME.alt, HOME.heading)
|
||||
mavproxy_master = 'tcp:127.0.0.1:5760'
|
||||
sitl = util.start_SITL(binary,
|
||||
wipe=True,
|
||||
model=frame,
|
||||
home=home,
|
||||
speedup=10,
|
||||
unhide_parameters=True)
|
||||
mavproxy = util.start_MAVProxy_SITL(atype,
|
||||
master=mavproxy_master)
|
||||
print("Dumping defaults")
|
||||
idx = mavproxy.expect([r'Saved [0-9]+ parameters to (\S+)'])
|
||||
if idx == 0:
|
||||
# we need to restart it after eeprom erase
|
||||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sitl)
|
||||
sitl = util.start_SITL(binary, model=frame, home=home, speedup=10)
|
||||
mavproxy = util.start_MAVProxy_SITL(atype,
|
||||
master=mavproxy_master)
|
||||
mavproxy.expect(r'Saved [0-9]+ parameters to (\S+)')
|
||||
parmfile = mavproxy.match.group(1)
|
||||
dest = buildlogs_path('%s-defaults.parm' % atype)
|
||||
shutil.copy(parmfile, dest)
|
||||
util.pexpect_close(mavproxy)
|
||||
util.pexpect_close(sitl)
|
||||
print("Saved defaults for %s to %s" % (atype, dest))
|
||||
return True
|
||||
|
||||
|
||||
def build_all_filepath():
|
||||
"""Get build_all.sh path."""
|
||||
return util.reltopdir('Tools/scripts/build_all.sh')
|
||||
|
@ -484,6 +442,11 @@ def run_step(step):
|
|||
return util.build_replay(board='SITL')
|
||||
|
||||
if vehicle_binary is not None:
|
||||
try:
|
||||
binary = binary_path(step, debug=opts.debug)
|
||||
os.unlink(binary)
|
||||
except (FileNotFoundError, ValueError):
|
||||
pass
|
||||
if len(vehicle_binary.split(".")) == 1:
|
||||
return util.build_SITL(vehicle_binary, **build_opts)
|
||||
else:
|
||||
|
@ -495,9 +458,6 @@ def run_step(step):
|
|||
|
||||
binary = binary_path(step, debug=opts.debug)
|
||||
|
||||
if step.startswith("defaults"):
|
||||
vehicle = step[9:]
|
||||
return get_default_params(vehicle, binary)
|
||||
supplementary_binaries = []
|
||||
if step in suplementary_test_binary_map:
|
||||
for supplementary_test_binary in suplementary_test_binary_map[step]:
|
||||
|
@ -690,9 +650,10 @@ def write_fullresults():
|
|||
results.addglob("GPX track", '*.gpx')
|
||||
|
||||
# results common to all vehicles:
|
||||
vehicle_files = [('{vehicle} defaults', '{vehicle}-defaults.parm'),
|
||||
('{vehicle} core', '{vehicle}.core'),
|
||||
('{vehicle} ELF', '{vehicle}.elf'), ]
|
||||
vehicle_files = [
|
||||
('{vehicle} core', '{vehicle}.core'),
|
||||
('{vehicle} ELF', '{vehicle}.elf'),
|
||||
]
|
||||
vehicle_globs = [('{vehicle} log', '{vehicle}-*.BIN'), ]
|
||||
for vehicle in all_vehicles():
|
||||
subs = {'vehicle': vehicle}
|
||||
|
@ -1082,33 +1043,28 @@ if __name__ == "__main__":
|
|||
'run.examples',
|
||||
|
||||
'build.Plane',
|
||||
'defaults.Plane',
|
||||
'test.Plane',
|
||||
'test.QuadPlane',
|
||||
|
||||
'build.Rover',
|
||||
'defaults.Rover',
|
||||
'test.Rover',
|
||||
'test.BalanceBot',
|
||||
'test.Sailboat',
|
||||
|
||||
'build.Copter',
|
||||
'defaults.Copter',
|
||||
'test.Copter',
|
||||
|
||||
'build.Helicopter',
|
||||
'test.Helicopter',
|
||||
|
||||
'build.Tracker',
|
||||
'defaults.Tracker',
|
||||
'test.Tracker',
|
||||
|
||||
'build.Sub',
|
||||
'defaults.Sub',
|
||||
'test.Sub',
|
||||
|
||||
'build.Blimp',
|
||||
'defaults.Blimp',
|
||||
'test.Blimp',
|
||||
|
||||
'build.SITLPeriphGPS',
|
||||
'test.CAN',
|
||||
|
@ -1149,11 +1105,6 @@ if __name__ == "__main__":
|
|||
"drive.balancebot": "test.BalanceBot",
|
||||
"fly.CopterAVC": "test.Helicopter",
|
||||
"test.AntennaTracker": "test.Tracker",
|
||||
"defaults.ArduCopter": "defaults.Copter",
|
||||
"defaults.ArduPlane": "defaults.Plane",
|
||||
"defaults.ArduSub": "defaults.Sub",
|
||||
"defaults.APMrover2": "defaults.Rover",
|
||||
"defaults.AntennaTracker": "defaults.Tracker",
|
||||
"fly.ArduCopterTests1a": "test.CopterTests1a",
|
||||
"fly.ArduCopterTests1b": "test.CopterTests1b",
|
||||
"fly.ArduCopterTests1c": "test.CopterTests1c",
|
||||
|
|
|
@ -1984,7 +1984,8 @@ class AutoTest(ABC):
|
|||
self.progress("Rebooting SITL")
|
||||
self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force)
|
||||
self.do_heartbeats(force=True)
|
||||
self.assert_simstate_location_is_at_startup_location()
|
||||
if self.frame != 'sailboat': # sailboats drift with wind!
|
||||
self.assert_simstate_location_is_at_startup_location()
|
||||
|
||||
def reboot_sitl_mavproxy(self, required_bootcount=None):
|
||||
"""Reboot SITL instance using MAVProxy and wait for it to reconnect."""
|
||||
|
@ -2038,6 +2039,7 @@ class AutoTest(ABC):
|
|||
|
||||
def set_streamrate(self, streamrate, timeout=20, stream=mavutil.mavlink.MAV_DATA_STREAM_ALL):
|
||||
'''set MAV_DATA_STREAM_ALL; timeout is wallclock time'''
|
||||
self.do_timesync_roundtrip(timeout_in_wallclock=True)
|
||||
tstart = time.time()
|
||||
while True:
|
||||
if time.time() - tstart > timeout:
|
||||
|
@ -3363,17 +3365,19 @@ class AutoTest(ABC):
|
|||
self.progress("Received: %s" % str(m))
|
||||
if m is None:
|
||||
continue
|
||||
if m.tc1 == 0:
|
||||
self.progress("this is a timesync request, which we don't answer")
|
||||
continue
|
||||
if m.ts1 % 1000 != self.mav.source_system:
|
||||
self.progress("this isn't a response to our timesync (%s)" % (m.ts1 % 1000))
|
||||
continue
|
||||
if m.tc1 == 0:
|
||||
# this should also not happen:
|
||||
self.progress("this is a timesync request, which we don't answer")
|
||||
continue
|
||||
if int(m.ts1 / 1000) != self.timesync_number:
|
||||
self.progress("this isn't the one we just sent")
|
||||
continue
|
||||
if m.get_srcSystem() != self.mav.target_system:
|
||||
self.progress("response from system other than our target")
|
||||
self.progress("response from system other than our target (want=%u got=%u" %
|
||||
(self.mav.target_system, m.get_srcSystem()))
|
||||
continue
|
||||
# no component check ATM because we send broadcast...
|
||||
# if m.get_srcComponent() != self.mav.target_component:
|
||||
|
@ -4473,13 +4477,14 @@ class AutoTest(ABC):
|
|||
raise ValueError("count %u not handled" % count)
|
||||
self.progress("Rally content same")
|
||||
|
||||
def load_rally(self, filename):
|
||||
def load_rally_using_mavproxy(self, filename):
|
||||
"""Load rally points from a file to flight controller."""
|
||||
self.progress("Loading rally points (%s)" % filename)
|
||||
path = os.path.join(testdir, self.current_test_name_directory, filename)
|
||||
mavproxy = self.start_mavproxy()
|
||||
mavproxy.send('rally load %s\n' % path)
|
||||
mavproxy.expect("Loaded")
|
||||
self.delay_sim_time(10) # allow transfer to complete
|
||||
self.stop_mavproxy(mavproxy)
|
||||
|
||||
def load_sample_mission(self):
|
||||
|
@ -6385,10 +6390,13 @@ class AutoTest(ABC):
|
|||
**kwargs
|
||||
)
|
||||
|
||||
def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=30, **kwargs):
|
||||
def wait_altitude(self, altitude_min, altitude_max, relative=False, timeout=None, **kwargs):
|
||||
"""Wait for a given altitude range."""
|
||||
assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude."
|
||||
|
||||
if timeout is None:
|
||||
timeout = 30
|
||||
|
||||
def validator(value2, target2=None):
|
||||
if altitude_min <= value2 <= altitude_max:
|
||||
return True
|
||||
|
@ -6773,11 +6781,12 @@ class AutoTest(ABC):
|
|||
return Vector3(msg.vx, msg.vy, msg.vz)
|
||||
|
||||
"""Wait for a given speed vector."""
|
||||
def wait_speed_vector(self, speed_vector, accuracy=0.2, timeout=30, **kwargs):
|
||||
def wait_speed_vector(self, speed_vector, accuracy=0.3, timeout=30, **kwargs):
|
||||
def validator(value2, target2):
|
||||
return (math.fabs(value2.x - target2.x) <= accuracy and
|
||||
math.fabs(value2.y - target2.y) <= accuracy and
|
||||
math.fabs(value2.z - target2.z) <= accuracy)
|
||||
for (want, got) in (target2.x, value2.x), (target2.y, value2.y), (target2.z, value2.z):
|
||||
if want != float("nan") and (math.fabs(got - want) > accuracy):
|
||||
return False
|
||||
return True
|
||||
|
||||
self.wait_and_maintain(
|
||||
value_name="SpeedVector",
|
||||
|
@ -7211,7 +7220,7 @@ class AutoTest(ABC):
|
|||
|
||||
def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30):
|
||||
self.progress("Waiting for GPS health")
|
||||
tstart = self.get_sim_time_cached()
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
now = self.get_sim_time_cached()
|
||||
if now - tstart > timeout:
|
||||
|
@ -7652,6 +7661,10 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.mav.mav.set_send_callback(self.send_message_hook, self)
|
||||
self.mav.idle_hooks.append(self.idle_hook)
|
||||
|
||||
# we need to wait for a heartbeat here. If we don't then
|
||||
# self.mav.target_system will be zero because it hasn't
|
||||
# "locked on" to a target system yet.
|
||||
self.wait_heartbeat()
|
||||
self.set_streamrate(self.sitl_streamrate())
|
||||
|
||||
def show_test_timings_key_sorter(self, t):
|
||||
|
@ -7861,6 +7874,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
passed = False
|
||||
reset_needed = True
|
||||
|
||||
# if we haven't already reset ArduPilot because it's dead,
|
||||
# then ensure the vehicle was disarmed at the end of the test.
|
||||
# If it wasn't then the test is considered failed:
|
||||
if ardupilot_alive and self.armed() and not self.is_tracker():
|
||||
if ex is None:
|
||||
ex = ArmedAtEndOfTestException("Still armed at end of test")
|
||||
|
@ -7877,6 +7893,9 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.progress("Force-rebooting SITL")
|
||||
self.reboot_sitl() # that'll learn it
|
||||
passed = False
|
||||
elif not passed: # implicit reboot after a failed test:
|
||||
self.progress("Test failed but ArduPilot process alive; rebooting")
|
||||
self.reboot_sitl() # that'll learn it
|
||||
|
||||
if self._mavproxy is not None:
|
||||
self.progress("Stopping auto-started mavproxy")
|
||||
|
@ -8020,6 +8039,11 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.sup_prog.append(sup_prog_link)
|
||||
self.expect_list_add(sup_prog_link)
|
||||
|
||||
# mavlink will have disconnected here. Explicitly reconnect,
|
||||
# or the first packet we send will be lost:
|
||||
if self.mav is not None:
|
||||
self.mav.reconnect()
|
||||
|
||||
def get_suplementary_programs(self):
|
||||
return self.sup_prog
|
||||
|
||||
|
@ -8380,11 +8404,11 @@ Also, ignores heartbeats not from our target system'''
|
|||
start_loc = self.sitl_start_location()
|
||||
self.progress("SITL start loc: %s" % str(start_loc))
|
||||
delta = abs(orig_home.latitude * 1.0e-7 - start_loc.lat)
|
||||
if delta > 0.000001:
|
||||
if delta > 0.000006:
|
||||
raise ValueError("homes differ in lat got=%f vs want=%f delta=%f" %
|
||||
(orig_home.latitude * 1.0e-7, start_loc.lat, delta))
|
||||
delta = abs(orig_home.longitude * 1.0e-7 - start_loc.lng)
|
||||
if delta > 0.000001:
|
||||
if delta > 0.000006:
|
||||
raise ValueError("homes differ in lon got=%f vs want=%f delta=%f" %
|
||||
(orig_home.longitude * 1.0e-7, start_loc.lng, delta))
|
||||
if self.is_rover():
|
||||
|
@ -9575,6 +9599,8 @@ Also, ignores heartbeats not from our target system'''
|
|||
self.set_rc(interlock_channel, 1000)
|
||||
|
||||
self.start_subtest("Test all mode arming")
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
if self.arming_test_mission() is not None:
|
||||
self.load_mission(self.arming_test_mission())
|
||||
|
||||
|
@ -12986,6 +13012,7 @@ switch value'''
|
|||
(msg, m.alt, gpi_alt))
|
||||
introduced_error = 10 # in metres
|
||||
self.set_parameter("SIM_GPS2_ALT_OFS", introduced_error)
|
||||
self.do_timesync_roundtrip()
|
||||
m = self.assert_receive_message("GPS2_RAW")
|
||||
if abs((m.alt-introduced_error*1000) - gpi_alt) > 100:
|
||||
raise NotAchievedException("skewed Alt (%s) discrepancy; %d+%d vs %d" %
|
||||
|
@ -13004,9 +13031,11 @@ switch value'''
|
|||
if abs(new_gpi_alt2 - m.alt) > 100:
|
||||
raise NotAchievedException("Failover not detected")
|
||||
|
||||
def fetch_file_via_ftp(self, path):
|
||||
def fetch_file_via_ftp(self, path, timeout=20):
|
||||
'''returns the content of the FTP'able file at path'''
|
||||
self.progress("Retrieving (%s) using MAVProxy" % path)
|
||||
mavproxy = self.start_mavproxy()
|
||||
mavproxy.expect("Saved .* parameters to")
|
||||
ex = None
|
||||
tmpfile = tempfile.NamedTemporaryFile(mode='r', delete=False)
|
||||
try:
|
||||
|
@ -13015,9 +13044,18 @@ switch value'''
|
|||
mavproxy.send("ftp set debug 1\n") # so we get the "Terminated session" message
|
||||
mavproxy.send("ftp get %s %s\n" % (path, tmpfile.name))
|
||||
mavproxy.expect("Getting")
|
||||
self.delay_sim_time(2)
|
||||
mavproxy.send("ftp status\n")
|
||||
mavproxy.expect("No transfer in progress")
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
now = self.get_sim_time()
|
||||
if now - tstart > timeout:
|
||||
raise NotAchievedException("expected complete transfer")
|
||||
self.progress("Polling status")
|
||||
mavproxy.send("ftp status\n")
|
||||
try:
|
||||
mavproxy.expect("No transfer in progress", timeout=1)
|
||||
break
|
||||
except Exception:
|
||||
continue
|
||||
# terminate the connection, or it may still be in progress the next time an FTP is attempted:
|
||||
mavproxy.send("ftp cancel\n")
|
||||
mavproxy.expect("Terminated session")
|
||||
|
|
|
@ -1 +1,2 @@
|
|||
Q_OPTIONS 64
|
||||
ICE_RPM_THRESH 50 # idles at 70 (1% thrust)
|
||||
|
|
|
@ -161,7 +161,7 @@ class AutoTestHelicopter(AutoTestCopter):
|
|||
self.user_takeoff(alt_min=alt_min)
|
||||
else:
|
||||
self.set_rc(3, takeoff_throttle)
|
||||
self.wait_for_alt(alt_min=alt_min, timeout=timeout)
|
||||
self.wait_altitude(alt_min-1, alt_min+5, relative=True, timeout=timeout)
|
||||
self.hover()
|
||||
self.progress("TAKEOFF COMPLETE")
|
||||
|
||||
|
|
|
@ -393,6 +393,31 @@ class FakeMacOSXSpawn(object):
|
|||
return True
|
||||
|
||||
|
||||
class PSpawnStdPrettyPrinter(object):
|
||||
'''a fake filehandle-like object which prefixes a string to all lines
|
||||
before printing to stdout/stderr. To be used to pass to
|
||||
pexpect.spawn's logfile argument
|
||||
'''
|
||||
def __init__(self, output=sys.stdout, prefix="stdout"):
|
||||
self.output = output
|
||||
self.prefix = prefix
|
||||
self.buffer = ""
|
||||
|
||||
def close(self):
|
||||
self.print_prefixed_line(self.buffer)
|
||||
|
||||
def write(self, data):
|
||||
self.buffer += data
|
||||
for line in self.buffer.split("\n"):
|
||||
self.print_prefixed_line(line)
|
||||
|
||||
def print_prefixed_line(self, line):
|
||||
print("%s: %s" % (self.prefix, line), file=self.output)
|
||||
|
||||
def flush(self):
|
||||
pass
|
||||
|
||||
|
||||
def start_SITL(binary,
|
||||
valgrind=False,
|
||||
callgrind=False,
|
||||
|
@ -411,7 +436,8 @@ def start_SITL(binary,
|
|||
customisations=[],
|
||||
lldb=False,
|
||||
enable_fgview_output=False,
|
||||
supplementary=False):
|
||||
supplementary=False,
|
||||
stdout_prefix=None):
|
||||
|
||||
if model is None and not supplementary:
|
||||
raise ValueError("model must not be None")
|
||||
|
@ -513,6 +539,11 @@ def start_SITL(binary,
|
|||
|
||||
cmd.extend(customisations)
|
||||
|
||||
pexpect_logfile_prefix = stdout_prefix
|
||||
if pexpect_logfile_prefix is None:
|
||||
pexpect_logfile_prefix = os.path.basename(binary)
|
||||
pexpect_logfile = PSpawnStdPrettyPrinter(prefix=pexpect_logfile_prefix)
|
||||
|
||||
if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'):
|
||||
global windowID
|
||||
# on MacOS record the window IDs so we can close them later
|
||||
|
@ -550,7 +581,7 @@ def start_SITL(binary,
|
|||
# AP gets a redirect-stdout-to-filehandle option. So, in the
|
||||
# meantime, return a dummy:
|
||||
return pexpect.spawn("true", ["true"],
|
||||
logfile=sys.stdout,
|
||||
logfile=pexpect_logfile,
|
||||
encoding=ENCODING,
|
||||
timeout=5)
|
||||
else:
|
||||
|
@ -558,7 +589,7 @@ def start_SITL(binary,
|
|||
|
||||
first = cmd[0]
|
||||
rest = cmd[1:]
|
||||
child = pexpect.spawn(first, rest, logfile=sys.stdout, encoding=ENCODING, timeout=5)
|
||||
child = pexpect.spawn(first, rest, logfile=pexpect_logfile, encoding=ENCODING, timeout=5)
|
||||
pexpect_autoclose(child)
|
||||
# give time for parameters to properly setup
|
||||
time.sleep(3)
|
||||
|
|
|
@ -946,7 +946,7 @@ class AutoTestQuadPlane(AutoTest):
|
|||
self.wait_rpm(1, 6500, 7500, minimum_duration=30, timeout=40)
|
||||
self.progress("Setting min-throttle")
|
||||
self.set_rc(3, 1000)
|
||||
self.wait_rpm(1, 300, 400, minimum_duration=1)
|
||||
self.wait_rpm(1, 65, 75, minimum_duration=1)
|
||||
self.progress("Setting engine-start RC switch to LOW")
|
||||
self.set_rc(rc_engine_start_chan, 1000)
|
||||
self.wait_rpm(1, 0, 0, minimum_duration=1)
|
||||
|
|
|
@ -1290,7 +1290,8 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||
|
||||
def Rally(self):
|
||||
'''Test Rally Points'''
|
||||
self.load_rally("rover-test-rally.txt")
|
||||
self.load_rally_using_mavproxy("rover-test-rally.txt")
|
||||
self.assert_parameter_value('RALLY_TOTAL', 2)
|
||||
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
|
@ -6293,7 +6294,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
|||
self.DriveRTL,
|
||||
self.SmartRTL,
|
||||
self.DriveSquare,
|
||||
self.DriveMaxRCIN,
|
||||
self.DriveMission,
|
||||
# self.DriveBrake, # disabled due to frequent failures
|
||||
self.GetBanner,
|
||||
|
|
Loading…
Reference in New Issue