diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index c1448ce5a7..83842cf075 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -3140,6 +3140,56 @@ class AutoTestCopter(AutoTest): 0) # button mask self.do_RTL() + def check_avoidance_corners(self): + self.takeoff(10, mode="LOITER") + self.set_rc(2, 1400) + west_loc = mavutil.location(-35.363007, + 149.164911, + 0, + 0) + self.wait_location(west_loc, accuracy=6) + north_loc = mavutil.location(-35.362908, + 149.165051, + 0, + 0) + self.reach_heading_manual(0); + self.wait_location(north_loc, accuracy=6) + self.reach_heading_manual(90); + east_loc = mavutil.location(-35.363013, + 149.165194, + 0, + 0) + self.wait_location(east_loc, accuracy=6) + self.reach_heading_manual(225); + self.wait_location(west_loc, accuracy=6) + self.set_rc(2, 1500) + self.do_RTL() + + def fly_proximity_avoidance_test(self): + self.context_push() + ex = None + try: + avoid_filepath = os.path.join(self.mission_directory(), + "copter-avoidance-fence.txt") + self.mavproxy.send("fence load %s\n" % avoid_filepath) + self.mavproxy.expect("Loaded 5 geo-fence") + self.set_parameter("FENCE_ENABLE", 0) + self.set_parameter("PRX_TYPE", 10) + self.set_parameter("RC10_OPTION", 40) # proximity-enable + self.reboot_sitl() + self.progress("Enabling proximity") + self.set_rc(10, 2000) + self.check_avoidance_corners() + except Exception as e: + self.progress("Caught exception: %s" % str(e)) + ex = e + self.context_pop() + self.mavproxy.send("fence clear\n") + self.disarm_vehicle(force=True) + self.reboot_sitl() + if ex is not None: + raise ex + def tests(self): '''return list of all tests''' ret = super(AutoTestCopter, self).tests() @@ -3207,6 +3257,10 @@ class AutoTestCopter(AutoTest): "Fly stability patch", lambda: self.fly_stability_patch(30)), + ("AC_Avoidance_Proximity", + "Test proximity avoidance slide behaviour", + self.fly_proximity_avoidance_test), + ("HorizontalFence", "Test horizontal fence", self.fly_fence_test), diff --git a/Tools/autotest/copter-avoidance-fence.txt b/Tools/autotest/copter-avoidance-fence.txt new file mode 100644 index 0000000000..a43ea3baa6 --- /dev/null +++ b/Tools/autotest/copter-avoidance-fence.txt @@ -0,0 +1,5 @@ +-35.362919 149.165039 +-35.362892 149.165054 +-35.363014 149.164886 +-35.363018 149.165207 +-35.362892 149.165054