diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 9837bfeb6a..b7acf5d91a 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -597,14 +597,81 @@ class AutoTestCopter(AutoTest): self.progress("RTL after stab patch") self.do_RTL() + def debug_arming_issue(self): + while True: + self.send_mavlink_arm_command() + m = self.mav.recv_match(blocking=True, timeout=1) + if m is None: + continue + if m.get_type() in ["STATUSTEXT", "COMMAND_ACK"]: + print("Got: %s" % str(m)) + if self.mav.motors_armed(): + self.progress("Armed") + return + # fly_fence_test - fly east until you hit the horizontal circular fence def fly_fence_test(self, timeout=180): - self.takeoff(10, mode="LOITER") - # enable fence, disable avoidance self.set_parameter("FENCE_ENABLE", 1) self.set_parameter("AVOID_ENABLE", 0) + self.change_mode("LOITER") + self.wait_ready_to_arm() + + # fence requires home to be set: + m = self.poll_home_position() + if m is None: + raise NotAchievedException("Did not receive HOME_POSITION") + self.progress("home: %s" % str(m)) + + self.start_subtest("ensure we can't arm if ouside fence") + fence = os.path.join(self.mission_directory(), + "fence-in-middle-of-nowhere.txt") + self.mavproxy.send('fence load %s\n' % fence) + self.mavproxy.expect("Loaded 6 geo-fence") + self.delay_sim_time(5) # let fence check run so it loads-from-eeprom + seen_statustext = False + seen_command_ack = False + + tstart = self.get_sim_time_cached() + while True: + self.send_mavlink_arm_command() + if seen_command_ack and seen_statustext: + break + if self.get_sim_time_cached() - tstart > 5: + raise NotAchievedException("Did not see failure-to-arm messages (statustext=%s command_ack=%s" % (seen_statustext, seen_command_ack)) + m = self.mav.recv_match(blocking=True, timeout=1) + if m is None: + continue + if m.get_type() == "STATUSTEXT": + if "vehicle outside fence" in m.text: + self.progress("Got: %s" % str(m)) + seen_statustext = True + elif "PreArm" in m.text: + self.progress("Got: %s" % str(m)) + raise NotAchievedException("Unexpected prearm failure (%s)" % m.text) + + if m.get_type() == "COMMAND_ACK": + print("Got: %s" % str(m)) + if m.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + if m.result != 4: + raise NotAchievedException("command-ack says we didn't fail to arm") + self.progress("Got: %s" % str(m)) + seen_command_ack = True + if self.mav.motors_armed(): + raise NotAchievedException("Armed when we shouldn't have") + + self.progress("Failed to arm outside fence (good!)") + self.drain_mav() + self.mavproxy.send('fence clear\n') + self.delay_sim_time(2) + self.mavproxy.send('fence list\n') + self.mavproxy.expect("No geo-fence points") + + self.start_subtest("Check breach-fence behaviour") + self.set_parameter("FENCE_TYPE", 2) + self.takeoff(10, mode="LOITER") + # first east self.progress("turn east") self.set_rc(4, 1580) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 41a1c0d801..252b34063b 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -769,6 +769,22 @@ class AutoTest(ABC): """Return true if vehicle is armed and safetyoff""" return self.mav.motors_armed() + def send_mavlink_arm_command(self): + target_sysid = 1 + target_compid = 1 + self.mav.mav.command_long_send( + target_sysid, + target_compid, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 1, # confirmation + 1, # ARM + 0, + 0, + 0, + 0, + 0, + 0) + def arm_vehicle(self, timeout=20): """Arm vehicle with mavlink arm message.""" self.progress("Arm motors with MAVLink cmd") diff --git a/Tools/autotest/fence-in-middle-of-nowhere.txt b/Tools/autotest/fence-in-middle-of-nowhere.txt new file mode 100644 index 0000000000..ef442dd546 --- /dev/null +++ b/Tools/autotest/fence-in-middle-of-nowhere.txt @@ -0,0 +1,6 @@ +-35.361359 149.162872 +-35.362522 149.161926 +-35.362072 149.164459 +-35.360195 149.163742 +-35.360554 149.161285 +-35.362522 149.161926