autotest: CI fixes for 4.4

This commit is contained in:
Peter Barker 2023-04-07 11:31:05 +10:00
parent b06ba1bacd
commit 379a905abd
10 changed files with 147 additions and 123 deletions

View File

@ -40,7 +40,7 @@ def check_log(logfile, progress=print, ekf2_only=False, ekf3_only=False, verbose
if not hasattr(m,'C'): if not hasattr(m,'C'):
continue continue
mtype = m.get_type() mtype = m.get_type()
if not mtype in counts: if mtype not in counts:
counts[mtype] = 0 counts[mtype] = 0
base_counts[mtype] = 0 base_counts[mtype] = 0
core = m.C core = m.C

View File

@ -134,7 +134,7 @@ class AutoTestCopter(AutoTest):
0, # param6 0, # param6
alt_min # param7 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, def takeoff(self,
alt_min=30, alt_min=30,
@ -154,17 +154,10 @@ class AutoTestCopter(AutoTest):
self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err) self.user_takeoff(alt_min=alt_min, timeout=timeout, max_err=max_err)
else: else:
self.set_rc(3, takeoff_throttle) 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.hover()
self.progress("TAKEOFF COMPLETE") 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): def land_and_disarm(self, timeout=60):
"""Land the quad.""" """Land the quad."""
self.progress("STARTING LANDING") self.progress("STARTING LANDING")
@ -176,7 +169,7 @@ class AutoTestCopter(AutoTest):
m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
alt = m.relative_alt / 1000.0 # mm -> m alt = m.relative_alt / 1000.0 # mm -> m
if alt > min_alt: 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_statustext("SIM Hit ground", timeout=timeout)
self.wait_disarmed() self.wait_disarmed()
@ -2086,7 +2079,7 @@ class AutoTestCopter(AutoTest):
self.progress("Regaining altitude") self.progress("Regaining altitude")
self.change_mode('ALT_HOLD') 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.progress("Flipping in pitch")
self.set_rc(2, 1700) self.set_rc(2, 1700)
@ -2426,7 +2419,7 @@ class AutoTestCopter(AutoTest):
raise NotAchievedException("AUTOTUNE gains not present in pilot testing") raise NotAchievedException("AUTOTUNE gains not present in pilot testing")
# land without changing mode # land without changing mode
self.set_rc(3, 1000) self.set_rc(3, 1000)
self.wait_for_alt(0) self.wait_altitude(-1, 5, relative=True)
self.wait_disarmed() self.wait_disarmed()
# Check gains are still there after disarm # Check gains are still there after disarm
if (rlld == self.get_parameter("ATC_RAT_RLL_D") or if (rlld == self.get_parameter("ATC_RAT_RLL_D") or
@ -2677,7 +2670,7 @@ class AutoTestCopter(AutoTest):
0, 0,
timeout=10, timeout=10,
want_result=mavutil.mavlink.MAV_RESULT_FAILED) 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.stop_sup_program(instance=0)
self.start_sup_program(instance=0) self.start_sup_program(instance=0)
self.context_stop_collecting('STATUSTEXT') self.context_stop_collecting('STATUSTEXT')
@ -3850,7 +3843,7 @@ class AutoTestCopter(AutoTest):
new_pos = self.mav.location() new_pos = self.mav.location()
delta = self.get_distance(target, new_pos) delta = self.get_distance(target, new_pos)
self.progress("Landed %f metres from target position" % delta) self.progress("Landed %f metres from target position" % delta)
max_delta = 1 max_delta = 1.5
if delta > max_delta: if delta > max_delta:
raise NotAchievedException("Did not land close enough to target position (%fm > %fm" % (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 # determine if we've successfully navigated to close to
# where we should be: # where we should be:
dist = math.sqrt(delta_ef.x * delta_ef.x + delta_ef.y * delta_ef.y) 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)) self.progress("dist=%f want <%f" % (dist, dist_max))
if dist < dist_max: if dist < dist_max:
# success! We've gotten within our target distance # success! We've gotten within our target distance
@ -4645,6 +4638,7 @@ class AutoTestCopter(AutoTest):
except Exception as e: except Exception as e:
self.print_exception_caught(e) self.print_exception_caught(e)
self.disarm_vehicle(force=True)
ex = e ex = e
self.context_pop() self.context_pop()
@ -6825,6 +6819,7 @@ class AutoTestCopter(AutoTest):
failed = False failed = False
wants = [] wants = []
gots = [] gots = []
epsilon = 20
while True: while True:
if self.get_sim_time_cached() - tstart > 30: if self.get_sim_time_cached() - tstart > 30:
raise AutoTestTimeoutException("Failed to get distances") raise AutoTestTimeoutException("Failed to get distances")
@ -6837,7 +6832,7 @@ class AutoTestCopter(AutoTest):
want = expected_distances_copy[m.orientation] want = expected_distances_copy[m.orientation]
wants.append(want) wants.append(want)
gots.append(got) gots.append(got)
if abs(want - got) > 5: if abs(want - got) > epsilon:
failed = True failed = True
del expected_distances_copy[m.orientation] del expected_distances_copy[m.orientation]
if failed: if failed:
@ -6856,9 +6851,11 @@ class AutoTestCopter(AutoTest):
}) })
self.set_analog_rangefinder_parameters() self.set_analog_rangefinder_parameters()
self.reboot_sitl() self.reboot_sitl()
tstart = self.get_sim_time()
self.change_mode('LOITER') self.change_mode('LOITER')
self.wait_ekf_happy() self.wait_ekf_happy()
tstart = self.get_sim_time()
while True: while True:
if self.armed(): if self.armed():
break break
@ -6909,7 +6906,7 @@ class AutoTestCopter(AutoTest):
if self.get_sim_time_cached() - tstart > 10: if self.get_sim_time_cached() - tstart > 10:
break break
vel = self.get_body_frame_velocity() vel = self.get_body_frame_velocity()
if vel.length() > 0.3: if vel.length() > 0.5:
raise NotAchievedException("Moved too much (%s)" % raise NotAchievedException("Moved too much (%s)" %
(str(vel),)) (str(vel),))
shove(None, None) shove(None, None)
@ -8337,7 +8334,7 @@ class AutoTestCopter(AutoTest):
def verify_yaw(mav, m): def verify_yaw(mav, m):
if m.get_type() != 'ATTITUDE': if m.get_type() != 'ATTITUDE':
return return
yawspeed_thresh_rads = math.radians(10) yawspeed_thresh_rads = math.radians(20)
if m.yawspeed > yawspeed_thresh_rads: if m.yawspeed > yawspeed_thresh_rads:
raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" % raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %
(math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame)) (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))

View File

@ -20,6 +20,8 @@ from common import AutoTestTimeoutException
from common import NotAchievedException from common import NotAchievedException
from common import PreconditionFailedException from common import PreconditionFailedException
from common import WaitModeTimeout from common import WaitModeTimeout
from common import Test
from pymavlink.rotmat import Vector3 from pymavlink.rotmat import Vector3
from pysim import vehicleinfo from pysim import vehicleinfo
@ -780,6 +782,7 @@ class AutoTestPlane(AutoTest):
0, 0,
0) 0)
self.wait_airspeed(new_target_airspeed-0.5, new_target_airspeed+0.5) 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.progress("Adding some wind, hoping groundspeed increases/decreases")
self.set_parameters({ self.set_parameters({
"SIM_WIND_SPD": 7, "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)) self.progress("groundspeed and airspeed should be different (have=%f want=%f)" % (delta, want_delta))
if delta > want_delta: if delta > want_delta:
break break
self.context_pop()
self.fly_home_land_and_disarm(timeout=240) self.fly_home_land_and_disarm(timeout=240)
def fly_home_land_and_disarm(self, timeout=120): def fly_home_land_and_disarm(self, timeout=120):
@ -994,7 +998,7 @@ class AutoTestPlane(AutoTest):
self.context_collect("HEARTBEAT") self.context_collect("HEARTBEAT")
self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950 self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950
self.wait_mode('RTL') # long failsafe 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")]): [x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]):
raise NotAchievedException("Did not go via circle mode") raise NotAchievedException("Did not go via circle mode")
self.progress("Ensure we've had our throttle squashed to 950") self.progress("Ensure we've had our throttle squashed to 950")
@ -1032,7 +1036,7 @@ class AutoTestPlane(AutoTest):
self.context_collect("HEARTBEAT") self.context_collect("HEARTBEAT")
self.set_parameter("SIM_RC_FAIL", 1) # no-pulses self.set_parameter("SIM_RC_FAIL", 1) # no-pulses
self.wait_mode('RTL') # long failsafe 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")]): [x.custom_mode for x in self.context_stop_collecting("HEARTBEAT")]):
raise NotAchievedException("Did not go via circle mode") raise NotAchievedException("Did not go via circle mode")
self.do_timesync_roundtrip() self.do_timesync_roundtrip()
@ -1169,29 +1173,28 @@ class AutoTestPlane(AutoTest):
'''Ensure Long-Failsafe works on GCS loss''' '''Ensure Long-Failsafe works on GCS loss'''
self.start_subtest("Test Failsafe: RTL") self.start_subtest("Test Failsafe: RTL")
self.load_sample_mission() self.load_sample_mission()
self.set_parameter("RTL_AUTOLAND", 1)
self.change_mode("AUTO")
self.takeoff()
self.set_parameters({ self.set_parameters({
"FS_GCS_ENABL": 1, "FS_GCS_ENABL": 1,
"FS_LONG_ACTN": 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.progress("Disconnecting GCS")
self.set_heartbeat_rate(0) self.set_heartbeat_rate(0)
self.wait_mode("RTL", timeout=5) self.wait_mode("RTL", timeout=10)
self.set_heartbeat_rate(self.speedup) self.set_heartbeat_rate(self.speedup)
self.end_subtest("Completed RTL Failsafe test") self.end_subtest("Completed RTL Failsafe test")
self.start_subtest("Test Failsafe: FBWA Glide") self.start_subtest("Test Failsafe: FBWA Glide")
self.set_parameters({ self.set_parameters({
"RTL_AUTOLAND": 1,
"FS_LONG_ACTN": 2, "FS_LONG_ACTN": 2,
}) })
self.change_mode("AUTO") self.change_mode('AUTO')
self.takeoff()
self.progress("Disconnecting GCS") self.progress("Disconnecting GCS")
self.set_heartbeat_rate(0) self.set_heartbeat_rate(0)
self.wait_mode("FBWA", timeout=5) self.wait_mode("FBWA", timeout=10)
self.set_heartbeat_rate(self.speedup) self.set_heartbeat_rate(self.speedup)
self.end_subtest("Completed FBWA Failsafe test") self.end_subtest("Completed FBWA Failsafe test")
@ -1844,6 +1847,7 @@ class AutoTestPlane(AutoTest):
self.fly_home_land_and_disarm() self.fly_home_land_and_disarm()
def deadreckoning_main(self, disable_airspeed_sensor=False): def deadreckoning_main(self, disable_airspeed_sensor=False):
self.reboot_sitl()
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.gpi = None self.gpi = None
self.simstate = None self.simstate = None
@ -2414,11 +2418,6 @@ class AutoTestPlane(AutoTest):
self.load_mission('CMAC-soar.txt', strict=False) 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 # Enable thermalling RC
rc_chan = 0 rc_chan = 0
for i in range(8): for i in range(8):
@ -2432,15 +2431,22 @@ class AutoTestPlane(AutoTest):
self.set_rc_from_map({ self.set_rc_from_map({
rc_chan: 1900, 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 # Wait to detect thermal
self.progress("Waiting for thermal") self.progress("Waiting for thermal")
self.wait_mode('THERMAL', timeout=600) self.wait_mode('THERMAL', timeout=600)
self.set_parameter("SOAR_VSPEED", 0.6)
# Wait to climb to SOAR_ALT_MAX # Wait to climb to SOAR_ALT_MAX
self.progress("Waiting for climb to max altitude") self.progress("Waiting for climb to max altitude")
alt_max = self.get_parameter('SOAR_ALT_MAX') alt_max = self.get_parameter('SOAR_ALT_MAX')
@ -4569,7 +4575,7 @@ class AutoTestPlane(AutoTest):
self.AltResetBadGPS, self.AltResetBadGPS,
self.AirspeedCal, self.AirspeedCal,
self.MissionJumpTags, self.MissionJumpTags,
self.GCSFailsafe, Test(self.GCSFailsafe, speedup=8),
self.SDCardWPTest, self.SDCardWPTest,
]) ])
return ret return ret

View File

@ -33,7 +33,6 @@ import helicopter
import examples import examples
from pysim import util from pysim import util
from pymavlink import mavutil
from pymavlink.generator import mavtemplate from pymavlink.generator import mavtemplate
from common import Test from common import Test
@ -58,47 +57,6 @@ def buildlogs_path(path):
return os.path.join(*bits) 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(): def build_all_filepath():
"""Get build_all.sh path.""" """Get build_all.sh path."""
return util.reltopdir('Tools/scripts/build_all.sh') return util.reltopdir('Tools/scripts/build_all.sh')
@ -484,6 +442,11 @@ def run_step(step):
return util.build_replay(board='SITL') return util.build_replay(board='SITL')
if vehicle_binary is not None: 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: if len(vehicle_binary.split(".")) == 1:
return util.build_SITL(vehicle_binary, **build_opts) return util.build_SITL(vehicle_binary, **build_opts)
else: else:
@ -495,9 +458,6 @@ def run_step(step):
binary = binary_path(step, debug=opts.debug) binary = binary_path(step, debug=opts.debug)
if step.startswith("defaults"):
vehicle = step[9:]
return get_default_params(vehicle, binary)
supplementary_binaries = [] supplementary_binaries = []
if step in suplementary_test_binary_map: if step in suplementary_test_binary_map:
for supplementary_test_binary in suplementary_test_binary_map[step]: for supplementary_test_binary in suplementary_test_binary_map[step]:
@ -690,9 +650,10 @@ def write_fullresults():
results.addglob("GPX track", '*.gpx') results.addglob("GPX track", '*.gpx')
# results common to all vehicles: # results common to all vehicles:
vehicle_files = [('{vehicle} defaults', '{vehicle}-defaults.parm'), vehicle_files = [
('{vehicle} core', '{vehicle}.core'), ('{vehicle} core', '{vehicle}.core'),
('{vehicle} ELF', '{vehicle}.elf'), ] ('{vehicle} ELF', '{vehicle}.elf'),
]
vehicle_globs = [('{vehicle} log', '{vehicle}-*.BIN'), ] vehicle_globs = [('{vehicle} log', '{vehicle}-*.BIN'), ]
for vehicle in all_vehicles(): for vehicle in all_vehicles():
subs = {'vehicle': vehicle} subs = {'vehicle': vehicle}
@ -1082,33 +1043,28 @@ if __name__ == "__main__":
'run.examples', 'run.examples',
'build.Plane', 'build.Plane',
'defaults.Plane',
'test.Plane', 'test.Plane',
'test.QuadPlane', 'test.QuadPlane',
'build.Rover', 'build.Rover',
'defaults.Rover',
'test.Rover', 'test.Rover',
'test.BalanceBot', 'test.BalanceBot',
'test.Sailboat', 'test.Sailboat',
'build.Copter', 'build.Copter',
'defaults.Copter',
'test.Copter', 'test.Copter',
'build.Helicopter', 'build.Helicopter',
'test.Helicopter', 'test.Helicopter',
'build.Tracker', 'build.Tracker',
'defaults.Tracker',
'test.Tracker', 'test.Tracker',
'build.Sub', 'build.Sub',
'defaults.Sub',
'test.Sub', 'test.Sub',
'build.Blimp', 'build.Blimp',
'defaults.Blimp', 'test.Blimp',
'build.SITLPeriphGPS', 'build.SITLPeriphGPS',
'test.CAN', 'test.CAN',
@ -1149,11 +1105,6 @@ if __name__ == "__main__":
"drive.balancebot": "test.BalanceBot", "drive.balancebot": "test.BalanceBot",
"fly.CopterAVC": "test.Helicopter", "fly.CopterAVC": "test.Helicopter",
"test.AntennaTracker": "test.Tracker", "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.ArduCopterTests1a": "test.CopterTests1a",
"fly.ArduCopterTests1b": "test.CopterTests1b", "fly.ArduCopterTests1b": "test.CopterTests1b",
"fly.ArduCopterTests1c": "test.CopterTests1c", "fly.ArduCopterTests1c": "test.CopterTests1c",

View File

@ -1984,6 +1984,7 @@ class AutoTest(ABC):
self.progress("Rebooting SITL") self.progress("Rebooting SITL")
self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force) self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force)
self.do_heartbeats(force=True) self.do_heartbeats(force=True)
if self.frame != 'sailboat': # sailboats drift with wind!
self.assert_simstate_location_is_at_startup_location() self.assert_simstate_location_is_at_startup_location()
def reboot_sitl_mavproxy(self, required_bootcount=None): def reboot_sitl_mavproxy(self, required_bootcount=None):
@ -2038,6 +2039,7 @@ class AutoTest(ABC):
def set_streamrate(self, streamrate, timeout=20, stream=mavutil.mavlink.MAV_DATA_STREAM_ALL): def set_streamrate(self, streamrate, timeout=20, stream=mavutil.mavlink.MAV_DATA_STREAM_ALL):
'''set MAV_DATA_STREAM_ALL; timeout is wallclock time''' '''set MAV_DATA_STREAM_ALL; timeout is wallclock time'''
self.do_timesync_roundtrip(timeout_in_wallclock=True)
tstart = time.time() tstart = time.time()
while True: while True:
if time.time() - tstart > timeout: if time.time() - tstart > timeout:
@ -3363,17 +3365,19 @@ class AutoTest(ABC):
self.progress("Received: %s" % str(m)) self.progress("Received: %s" % str(m))
if m is None: if m is None:
continue 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: if m.ts1 % 1000 != self.mav.source_system:
self.progress("this isn't a response to our timesync (%s)" % (m.ts1 % 1000)) self.progress("this isn't a response to our timesync (%s)" % (m.ts1 % 1000))
continue 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: if int(m.ts1 / 1000) != self.timesync_number:
self.progress("this isn't the one we just sent") self.progress("this isn't the one we just sent")
continue continue
if m.get_srcSystem() != self.mav.target_system: 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 continue
# no component check ATM because we send broadcast... # no component check ATM because we send broadcast...
# if m.get_srcComponent() != self.mav.target_component: # if m.get_srcComponent() != self.mav.target_component:
@ -4473,13 +4477,14 @@ class AutoTest(ABC):
raise ValueError("count %u not handled" % count) raise ValueError("count %u not handled" % count)
self.progress("Rally content same") 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.""" """Load rally points from a file to flight controller."""
self.progress("Loading rally points (%s)" % filename) self.progress("Loading rally points (%s)" % filename)
path = os.path.join(testdir, self.current_test_name_directory, filename) path = os.path.join(testdir, self.current_test_name_directory, filename)
mavproxy = self.start_mavproxy() mavproxy = self.start_mavproxy()
mavproxy.send('rally load %s\n' % path) mavproxy.send('rally load %s\n' % path)
mavproxy.expect("Loaded") mavproxy.expect("Loaded")
self.delay_sim_time(10) # allow transfer to complete
self.stop_mavproxy(mavproxy) self.stop_mavproxy(mavproxy)
def load_sample_mission(self): def load_sample_mission(self):
@ -6385,10 +6390,13 @@ class AutoTest(ABC):
**kwargs **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.""" """Wait for a given altitude range."""
assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude." assert altitude_min <= altitude_max, "Minimum altitude should be less than maximum altitude."
if timeout is None:
timeout = 30
def validator(value2, target2=None): def validator(value2, target2=None):
if altitude_min <= value2 <= altitude_max: if altitude_min <= value2 <= altitude_max:
return True return True
@ -6773,11 +6781,12 @@ class AutoTest(ABC):
return Vector3(msg.vx, msg.vy, msg.vz) return Vector3(msg.vx, msg.vy, msg.vz)
"""Wait for a given speed vector.""" """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): def validator(value2, target2):
return (math.fabs(value2.x - target2.x) <= accuracy and for (want, got) in (target2.x, value2.x), (target2.y, value2.y), (target2.z, value2.z):
math.fabs(value2.y - target2.y) <= accuracy and if want != float("nan") and (math.fabs(got - want) > accuracy):
math.fabs(value2.z - target2.z) <= accuracy) return False
return True
self.wait_and_maintain( self.wait_and_maintain(
value_name="SpeedVector", value_name="SpeedVector",
@ -7211,7 +7220,7 @@ class AutoTest(ABC):
def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30): def wait_gps_sys_status_not_present_or_enabled_and_healthy(self, timeout=30):
self.progress("Waiting for GPS health") self.progress("Waiting for GPS health")
tstart = self.get_sim_time_cached() tstart = self.get_sim_time()
while True: while True:
now = self.get_sim_time_cached() now = self.get_sim_time_cached()
if now - tstart > timeout: 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.mav.set_send_callback(self.send_message_hook, self)
self.mav.idle_hooks.append(self.idle_hook) 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()) self.set_streamrate(self.sitl_streamrate())
def show_test_timings_key_sorter(self, t): def show_test_timings_key_sorter(self, t):
@ -7861,6 +7874,9 @@ Also, ignores heartbeats not from our target system'''
passed = False passed = False
reset_needed = True 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 ardupilot_alive and self.armed() and not self.is_tracker():
if ex is None: if ex is None:
ex = ArmedAtEndOfTestException("Still armed at end of test") 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.progress("Force-rebooting SITL")
self.reboot_sitl() # that'll learn it self.reboot_sitl() # that'll learn it
passed = False 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: if self._mavproxy is not None:
self.progress("Stopping auto-started mavproxy") 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.sup_prog.append(sup_prog_link)
self.expect_list_add(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): def get_suplementary_programs(self):
return self.sup_prog return self.sup_prog
@ -8380,11 +8404,11 @@ Also, ignores heartbeats not from our target system'''
start_loc = self.sitl_start_location() start_loc = self.sitl_start_location()
self.progress("SITL start loc: %s" % str(start_loc)) self.progress("SITL start loc: %s" % str(start_loc))
delta = abs(orig_home.latitude * 1.0e-7 - start_loc.lat) 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" % raise ValueError("homes differ in lat got=%f vs want=%f delta=%f" %
(orig_home.latitude * 1.0e-7, start_loc.lat, delta)) (orig_home.latitude * 1.0e-7, start_loc.lat, delta))
delta = abs(orig_home.longitude * 1.0e-7 - start_loc.lng) 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" % raise ValueError("homes differ in lon got=%f vs want=%f delta=%f" %
(orig_home.longitude * 1.0e-7, start_loc.lng, delta)) (orig_home.longitude * 1.0e-7, start_loc.lng, delta))
if self.is_rover(): if self.is_rover():
@ -9575,6 +9599,8 @@ Also, ignores heartbeats not from our target system'''
self.set_rc(interlock_channel, 1000) self.set_rc(interlock_channel, 1000)
self.start_subtest("Test all mode arming") self.start_subtest("Test all mode arming")
self.wait_ready_to_arm()
if self.arming_test_mission() is not None: if self.arming_test_mission() is not None:
self.load_mission(self.arming_test_mission()) self.load_mission(self.arming_test_mission())
@ -12986,6 +13012,7 @@ switch value'''
(msg, m.alt, gpi_alt)) (msg, m.alt, gpi_alt))
introduced_error = 10 # in metres introduced_error = 10 # in metres
self.set_parameter("SIM_GPS2_ALT_OFS", introduced_error) self.set_parameter("SIM_GPS2_ALT_OFS", introduced_error)
self.do_timesync_roundtrip()
m = self.assert_receive_message("GPS2_RAW") m = self.assert_receive_message("GPS2_RAW")
if abs((m.alt-introduced_error*1000) - gpi_alt) > 100: if abs((m.alt-introduced_error*1000) - gpi_alt) > 100:
raise NotAchievedException("skewed Alt (%s) discrepancy; %d+%d vs %d" % raise NotAchievedException("skewed Alt (%s) discrepancy; %d+%d vs %d" %
@ -13004,9 +13031,11 @@ switch value'''
if abs(new_gpi_alt2 - m.alt) > 100: if abs(new_gpi_alt2 - m.alt) > 100:
raise NotAchievedException("Failover not detected") 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''' '''returns the content of the FTP'able file at path'''
self.progress("Retrieving (%s) using MAVProxy" % path)
mavproxy = self.start_mavproxy() mavproxy = self.start_mavproxy()
mavproxy.expect("Saved .* parameters to")
ex = None ex = None
tmpfile = tempfile.NamedTemporaryFile(mode='r', delete=False) tmpfile = tempfile.NamedTemporaryFile(mode='r', delete=False)
try: try:
@ -13015,9 +13044,18 @@ switch value'''
mavproxy.send("ftp set debug 1\n") # so we get the "Terminated session" message 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.send("ftp get %s %s\n" % (path, tmpfile.name))
mavproxy.expect("Getting") mavproxy.expect("Getting")
self.delay_sim_time(2) 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") mavproxy.send("ftp status\n")
mavproxy.expect("No transfer in progress") 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: # terminate the connection, or it may still be in progress the next time an FTP is attempted:
mavproxy.send("ftp cancel\n") mavproxy.send("ftp cancel\n")
mavproxy.expect("Terminated session") mavproxy.expect("Terminated session")

View File

@ -1 +1,2 @@
Q_OPTIONS 64 Q_OPTIONS 64
ICE_RPM_THRESH 50 # idles at 70 (1% thrust)

View File

@ -161,7 +161,7 @@ class AutoTestHelicopter(AutoTestCopter):
self.user_takeoff(alt_min=alt_min) self.user_takeoff(alt_min=alt_min)
else: else:
self.set_rc(3, takeoff_throttle) 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.hover()
self.progress("TAKEOFF COMPLETE") self.progress("TAKEOFF COMPLETE")

View File

@ -393,6 +393,31 @@ class FakeMacOSXSpawn(object):
return True 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, def start_SITL(binary,
valgrind=False, valgrind=False,
callgrind=False, callgrind=False,
@ -411,7 +436,8 @@ def start_SITL(binary,
customisations=[], customisations=[],
lldb=False, lldb=False,
enable_fgview_output=False, enable_fgview_output=False,
supplementary=False): supplementary=False,
stdout_prefix=None):
if model is None and not supplementary: if model is None and not supplementary:
raise ValueError("model must not be None") raise ValueError("model must not be None")
@ -513,6 +539,11 @@ def start_SITL(binary,
cmd.extend(customisations) 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'): if (gdb or lldb) and sys.platform == "darwin" and os.getenv('DISPLAY'):
global windowID global windowID
# on MacOS record the window IDs so we can close them later # 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 # AP gets a redirect-stdout-to-filehandle option. So, in the
# meantime, return a dummy: # meantime, return a dummy:
return pexpect.spawn("true", ["true"], return pexpect.spawn("true", ["true"],
logfile=sys.stdout, logfile=pexpect_logfile,
encoding=ENCODING, encoding=ENCODING,
timeout=5) timeout=5)
else: else:
@ -558,7 +589,7 @@ def start_SITL(binary,
first = cmd[0] first = cmd[0]
rest = cmd[1:] 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) pexpect_autoclose(child)
# give time for parameters to properly setup # give time for parameters to properly setup
time.sleep(3) time.sleep(3)

View File

@ -946,7 +946,7 @@ class AutoTestQuadPlane(AutoTest):
self.wait_rpm(1, 6500, 7500, minimum_duration=30, timeout=40) self.wait_rpm(1, 6500, 7500, minimum_duration=30, timeout=40)
self.progress("Setting min-throttle") self.progress("Setting min-throttle")
self.set_rc(3, 1000) 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.progress("Setting engine-start RC switch to LOW")
self.set_rc(rc_engine_start_chan, 1000) self.set_rc(rc_engine_start_chan, 1000)
self.wait_rpm(1, 0, 0, minimum_duration=1) self.wait_rpm(1, 0, 0, minimum_duration=1)

View File

@ -1290,7 +1290,8 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
def Rally(self): def Rally(self):
'''Test Rally Points''' '''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.wait_ready_to_arm()
self.arm_vehicle() self.arm_vehicle()
@ -6293,7 +6294,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
self.DriveRTL, self.DriveRTL,
self.SmartRTL, self.SmartRTL,
self.DriveSquare, self.DriveSquare,
self.DriveMaxRCIN,
self.DriveMission, self.DriveMission,
# self.DriveBrake, # disabled due to frequent failures # self.DriveBrake, # disabled due to frequent failures
self.GetBanner, self.GetBanner,