diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d39eaa763a..429a370d81 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1354,14 +1354,7 @@ class AutoTestCopter(AutoTest): # Activate the floor fence # TODO this test should run without requiring this - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - 1, - 0, - 0, - 0, - 0, - 0, - 0) + self.do_fence_enable() # first east self.progress("turn east") diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 58d476c457..a65c36c381 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1038,27 +1038,6 @@ class AutoTestPlane(AutoTest): raise NotAchievedException("fence status incorrect; %s want=%u got=%u" % (name, want, got)) - def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - if value: - p1 = 1 - else: - p1 = 0 - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, - p1, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0, # param7 - want_result=want_result) - - def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(True, want_result=want_result) - - def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): - self.do_fence_en_or_dis_able(False, want_result=want_result) - def wait_circling_point_with_radius(self, loc, want_radius, epsilon=5.0, min_circle_time=5, timeout=120): on_radius_start_heading = None average_radius = 0.0 diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 5b2dd9d023..4f6f5078f0 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4619,6 +4619,27 @@ class AutoTest(ABC): self.mavproxy.expect("Loaded module relay") self.mavproxy.send("relay set %d %d\n" % (relay_num, on_off)) + def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + if value: + p1 = 1 + else: + p1 = 0 + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, + p1, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 0, # param7 + want_result=want_result) + + def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.do_fence_en_or_dis_able(True, want_result=want_result) + + def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED): + self.do_fence_en_or_dis_able(False, want_result=want_result) + ################################################# # WAIT UTILITIES #################################################