diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f6d3868278..a5f1abe00c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -555,6 +555,81 @@ class AutoTestCopter(AutoTest): self.progress("Armed") return + def wait_distance_from_home(self, distance_min, distance_max, timeout=10): + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > timeout: + raise NotAchievedException("Did not achieve distance from home") + distance = self.distance_to_home(use_cached_home=True) + self.progress("Distance from home: now=%f %f= distance_min and distance <= distance_max: + return + + # fly_fence_test - fly east until you hit the horizontal circular fence + avoid_behave_slide = 0 + def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide): + using_mode = "LOITER" # must be something which adjusts velocity! + self.change_mode(using_mode) + self.set_parameter("FENCE_ENABLE", 1) # fence + self.set_parameter("FENCE_TYPE", 2) # circle + fence_radius = 15 + self.set_parameter("FENCE_RADIUS", fence_radius) + fence_margin = 3 + self.set_parameter("FENCE_MARGIN", fence_margin) + self.set_parameter("AVOID_ENABLE", 1) + self.set_parameter("AVOID_BEHAVE", avoid_behave) + self.set_parameter("RC10_OPTION", 40) # avoid-enable + self.wait_ready_to_arm() + self.set_rc(10, 2000) + home_distance = self.distance_to_home(use_cached_home=True) + if home_distance > 5: + raise PreconditionFailedException("Expected to be within 5m of home") + self.zero_throttle() + self.arm_vehicle() + self.set_rc(3, 1700) + self.wait_altitude(10, 100, relative=True) + self.set_rc(3, 1500) + self.set_rc(2, 1400) + self.wait_distance_from_home(12, 20) + tstart = self.get_sim_time() + push_time = 70 # push against barrier for 60 seconds + failed_max = False + failed_min = False + while True: + if self.get_sim_time() - tstart > push_time: + self.progress("Push time up") + break + # make sure we don't RTL: + if not self.mode_is(using_mode): + raise NotAchievedException("Changed mode away from %s" % using_mode) + distance = self.distance_to_home(use_cached_home=True) + inner_radius = fence_radius - fence_margin + want_min = inner_radius - 1 # allow 1m either way + want_max = inner_radius + 1 # allow 1m either way + self.progress("Push: distance=%f %f want_max: + if failed_max is False: + self.progress("Failed max") + failed_max = True + if failed_min and failed_max: + raise NotAchievedException("Failed both min and max checks. Clever") + if failed_min: + raise NotAchievedException("Failed min") + if failed_max: + raise NotAchievedException("Failed max") + self.set_rc(2, 1500) + self.do_RTL() + + def fly_fence_avoid_test(self, timeout=180): + self.fly_fence_avoid_test_radius_check(avoid_behave=1, timeout=timeout) + self.fly_fence_avoid_test_radius_check(avoid_behave=0, timeout=timeout) + # fly_fence_test - fly east until you hit the horizontal circular fence def fly_fence_test(self, timeout=180): # enable fence, disable avoidance @@ -3331,6 +3406,10 @@ class AutoTestCopter(AutoTest): "Test horizontal fence", self.fly_fence_test), + ("HorizontalAvoidFence", + "Test horizontal Avoidance fence", + self.fly_fence_avoid_test), + ("MaxAltFence", "Test Max Alt Fence", self.fly_alt_max_fence_test),