diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 07bf77ea94..ce3aeb9dd0 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3812,6 +3812,22 @@ function''' self.fly_home_land_and_disarm(timeout=180) + def MidAirDisarmDisallowed(self): + self.takeoff(50) + disarmed = False + try: + self.disarm_vehicle() + disarmed = True + except ValueError as e: + self.progress("Got %s" % repr(e)) + if "Expected MAV_RESULT_ACCEPTED got MAV_RESULT_FAILED" not in str(e): + raise e + if disarmed: + raise NotAchievedException("Disarmed when we shouldn't have") + # should still be able to force-disarm: + self.disarm_vehicle(force=True) + self.reboot_sitl() + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -4090,6 +4106,11 @@ function''' ("HIGH_LATENCY2", "Set sending of HIGH_LATENCY2", self.HIGH_LATENCY2), + + ("MidAirDisarmDisallowed", + "Ensure mid-air disarm is not possible", + self.MidAirDisarmDisallowed), + ]) return ret diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 3c8cd87bdc..40becfeb70 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -338,10 +338,10 @@ class AutoTestQuadPlane(AutoTest): (self.landed_state_name(landed_state), self.landed_state_name(m.landed_state))) - def wait_extended_sys_state(self, vtol_state, landed_state): + def wait_extended_sys_state(self, vtol_state, landed_state, timeout=10): tstart = self.get_sim_time() while True: - if self.get_sim_time() - tstart > 10: + if self.get_sim_time() - tstart > timeout: raise NotAchievedException("Did not achieve vol/landed states") self.progress("Waiting for MAV_VTOL_STATE=%s MAV_LANDED_STATE=%s" % (self.vtol_state_name(vtol_state), @@ -867,6 +867,57 @@ class AutoTestQuadPlane(AutoTest): self.change_mode('AUTO') self.wait_disarmed(timeout=300) + def MidAirDisarmDisallowed(self): + self.start_subtest("Basic arm in qloiter") + self.set_parameter("FLIGHT_OPTIONS", 0) + self.change_mode('QLOITER') + self.wait_ready_to_arm() + self.arm_vehicle() + self.disarm_vehicle() + + self.context_push() + self.start_subtest("Ensure disarming in q-modes on ground works") + self.set_parameter("FLIGHT_OPTIONS", 1 << 11) + self.arm_vehicle() + self.disarm_vehicle() # should be OK as we're not flying yet + self.context_pop() + + self.start_subtest("Ensure no disarming mid-air") + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(5, 50, relative=True) + self.set_rc(3, 1000) + disarmed = False + try: + self.disarm_vehicle() + disarmed = True + except ValueError as e: + self.progress("Got %s" % repr(e)) + if "Expected MAV_RESULT_ACCEPTED got MAV_RESULT_FAILED" not in str(e): + raise e + if disarmed: + raise NotAchievedException("Disarmed when we shouldn't have") + + self.change_mode('QLAND') + self.wait_disarmed() + + self.start_subtest("Check we can disarm after a short period on the ground") + self.takeoff(5, 'QHOVER') + self.change_mode('QLAND') + try: + self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 10) + self.wait_extended_sys_state( + landed_state=mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, + vtol_state=mavutil.mavlink.MAV_VTOL_STATE_MC, + timeout=60 + ) + except Exception: + self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, 0) + raise + + self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -1) + self.disarm_vehicle() + def tests(self): '''return list of all tests''' @@ -923,6 +974,10 @@ class AutoTestQuadPlane(AutoTest): "Test ICE Engine Mission support", self.ICEngineMission), + ("MidAirDisarmDisallowed", + "Check disarm behaviour in Q-mode", + self.MidAirDisarmDisallowed), + ("LogUpload", "Log upload", self.log_upload),