aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: autotest: add basic tests for AP_AdvancedFailsafe

Browse Source
This commit is contained in:
Peter Barker 2019-09-14 14:40:06 +10:00 committed by Andrew Tridgell
parent 2f60b230cd
commit bba019abc2
2 changed files with 138 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/autotest/arduplane.py
View File

@ -1418,6 +1418,10 @@ class AutoTestPlane(AutoTest):
"Test ADSB",
self.test_adsb),
("AdvancedFailsafe",
"Test Advanced Failsafe",
self.test_advanced_failsafe),
("LogDownLoad",
"Log download",
lambda: self.log_download(

173
Tools/autotest/common.py
View File

@ -689,6 +689,9 @@ class AutoTest(ABC):
self.mavproxy.send('rally load %s\n' % path)
self.mavproxy.expect("Loaded")
def load_sample_mission(self):
self.load_mission(self.sample_mission_filename())
def load_mission(self, filename):
"""Load a mission from a file to flight controller."""
self.progress("Loading mission (%s)" % filename)
@ -1178,20 +1181,18 @@ class AutoTest(ABC):
z)
self.run_cmd_get_ack(command, want_result, timeout)
def run_cmd(self,
command,
p1,
p2,
p3,
p4,
p5,
p6,
p7,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
target_sysid=None,
target_compid=None,
timeout=10,
quiet=False):
def send_cmd(self,
command,
p1,
p2,
p3,
p4,
p5,
p6,
p7,
target_sysid=None,
target_compid=None,
):
"""Send a MAVLink command long."""
if target_sysid is None:
target_sysid = self.sysid_thismav()
@ -1208,6 +1209,32 @@ class AutoTest(ABC):
p5,
p6,
p7)
def run_cmd(self,
command,
p1,
p2,
p3,
p4,
p5,
p6,
p7,
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
target_sysid=None,
target_compid=None,
timeout=10,
quiet=False):
self.send_cmd(command,
p1,
p2,
p3,
p4,
p5,
p6,
p7,
target_sysid=target_sysid,
target_compid=target_compid,
)
self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet)
def run_cmd_get_ack(self, command, want_result, timeout, quiet=False):
@ -1327,32 +1354,41 @@ class AutoTest(ABC):
self.mavproxy.send('mode %s\n' % mode)
self.progress("Got mode %s" % mode)
def capable(self, capability):
return self.get_autopilot_capabilities() & capability
def assert_capability(self, capability):
if not self.capable(capability):
name = mavutil.mavlink.enums["MAV_PROTOCOL_CAPABILITY"][capability].name
raise NotAchievedException("AutoPilot does not have capbility %s" % (name,))
def assert_no_capability(self, capability):
if self.capable(capability):
name = mavutil.mavlink.enums["MAV_PROTOCOL_CAPABILITY"][capability].name
raise NotAchievedException("AutoPilot has feature %s (when it shouln't)" % (name,))
def get_autopilot_capabilities(self):
# Cannot use run_cmd otherwise the respond is lost during the wait for ACK
self.mav.mav.command_long_send(1,
1,
mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
0, # confirmation
1, # 1: Request autopilot version
0,
0,
0,
0,
0,
0)
m = self.mav.recv_match(type='AUTOPILOT_VERSION',
blocking=True,
timeout=10)
if m is None:
raise NotAchievedException("Did not get AUTOPILOT_VERSION")
return m.capabilities
def do_get_autopilot_capabilities(self):
tstart = self.get_sim_time()
while self.get_sim_time_cached() - tstart < 10:
# Cannot use run_cmd otherwise the respond is lost during the wait for ACK
self.mav.mav.command_long_send(1,
1,
mavutil.mavlink.MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
0, # confirmation
1, # 1: Request autopilot version
0,
0,
0,
0,
0,
0)
m = self.mav.recv_match(type='AUTOPILOT_VERSION',
blocking=True,
timeout=10)
if m is None:
continue
if not (m.capabilities & mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT):
# all AP vehicles support PARAM_FLOAT
raise ValueError("Vehicle did not report itself as supporting PARAM_FLOAT")
self.progress("AUTOPILOT_VERSION received")
return
raise AutoTestTimeoutException("No AUTOPILOT_VERSION received")
self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT)
def get_mode_from_mode_mapping(self, mode):
"""Validate and return the mode number from a string or int."""
@ -3142,6 +3178,65 @@ switch value'''
if m is None:
raise PreconditionFailedException("Did not start to get PID_TUNING message")
def sample_mission_filename(self):
return "flaps.txt"
def test_advanced_failsafe(self):
self.context_push()
ex = None
try:
self.drain_mav()
self.assert_no_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION)
self.set_parameter("AFS_ENABLE", 1)
self.drain_mav()
self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION)
self.set_parameter("AFS_TERM_ACTION", 42)
self.load_sample_mission()
self.change_mode("AUTO") # must go to auto for AFS to latch on
self.change_mode("MANUAL")
self.start_subtest("RC Failure")
self.set_parameter("AFS_RC_FAIL_TIME", 1)
self.set_parameter("SIM_RC_FAIL", 1)
self.wait_statustext("Terminating due to RC failure")
self.set_parameter("AFS_RC_FAIL_TIME", 0)
self.set_parameter("SIM_RC_FAIL", 0)
self.set_parameter("AFS_TERMINATE", 0)
if not self.is_plane():
# plane requires a polygon fence...
self.start_subtest("Altitude Limit breach")
self.set_parameter("AFS_AMSL_LIMIT", 100)
self.mavproxy.send("fence enable\n")
self.wait_statustext("Terminating due to fence breach")
self.set_parameter("AFS_AMSL_LIMIT", 0)
self.set_parameter("AFS_TERMINATE", 0)
self.mavproxy.send("fence disable\n")
self.start_subtest("GPS Failure")
self.set_parameter("AFS_MAX_GPS_LOSS", 1)
self.set_parameter("SIM_GPS_DISABLE", 1)
self.wait_statustext("AFS State: GPS_LOSS")
self.set_parameter("SIM_GPS_DISABLE", 0)
self.set_parameter("AFS_MAX_GPS_LOSS", 0)
self.set_parameter("AFS_TERMINATE", 0)
self.send_cmd(mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION,
1, # terminate
0,
0,
0,
0,
0,
0,
)
self.wait_statustext("Terminating due to GCS request")
except Exception as e:
ex = e
self.mavproxy.send("fence disable\n")
if ex is not None:
raise ex
def tests(self):
return [
("PIDTuning",