diff --git a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt new file mode 100644 index 0000000000..a6169e4e3c --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt @@ -0,0 +1,6 @@ +-35.363720 149.163651 +-35.358738 149.165070 +-35.359295 149.154434 +-35.372292 149.157135 +-35.368290 149.166809 +-35.358738 149.165070 diff --git a/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt new file mode 100644 index 0000000000..ec0225131c --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt @@ -0,0 +1,10 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.362938 149.165086 585.090027 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.359628 149.164405 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360380 149.157558 60.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364289 149.156969 100.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 140.000000 1 +6 0 0 189 0.000000 0.000000 0.000000 0.000000 -35.369329 149.158092 0.000000 1 +7 0 3 31 0.000000 -75.000000 0.000000 1.000000 -35.367154 149.164864 100.000000 1 +8 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363200 149.165233 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index ad7a31cc09..f8c693de1b 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -967,8 +967,9 @@ class AutoTestPlane(AutoTest): self.load_fence("CMAC-fence.txt") - self.set_parameter("FENCE_CHANNEL", 7) - self.set_parameter("FENCE_ACTION", 4) + self.set_parameter("RC7_OPTION", 11) # AC_Fence uses Aux switch functionality + self.set_parameter("FENCE_ACTION", 4) # Fence action Brake + self.set_parameter("FENCE_ENABLE", 1) self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, present=True, @@ -1113,9 +1114,9 @@ class AutoTestPlane(AutoTest): if m is not None: raise NotAchievedException("Got FENCE_STATUS unexpectedly") self.drain_mav_unparsed() - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) # report only - self.assert_fence_sys_status(False, False, True) - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) # report only + self.set_parameter("FENCE_ACTION", 0) # report only + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 1) # RTL self.assert_fence_sys_status(True, False, True) self.do_fence_enable() self.assert_fence_sys_status(True, True, True) @@ -1127,9 +1128,9 @@ class AutoTestPlane(AutoTest): (m.breach_status)) self.do_fence_disable() self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE) - self.assert_fence_sys_status(False, False, True) - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + self.set_parameter("FENCE_ACTION", 1) + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 0) self.assert_fence_sys_status(True, False, True) self.clear_fence() if self.get_parameter("FENCE_TOTAL") != 0: @@ -1166,7 +1167,7 @@ class AutoTestPlane(AutoTest): expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius self.set_parameter("RTL_RADIUS", want_radius) self.set_parameter("NAVL1_LIM_BANK", 60) - self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL) + self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 self.do_fence_enable() self.assert_fence_sys_status(True, True, True) @@ -2331,6 +2332,78 @@ class AutoTestPlane(AutoTest): if ex is not None: raise ex + def test_fence_alt_ceil_floor(self): + ex = None + target_system = 1 + target_component = 1 + try: + self.progress("Testing Fence Enable") + + # Load Fence + self.load_fence("CMAC-fence.txt") + if self.get_parameter("FENCE_TOTAL") == 0: + raise NotAchievedException("Expected fence to be present") + + # Load Mission + self.load_mission("CMAC-mission.txt") + + # Set up fence parameters + self.set_parameter("FENCE_AUTOENABLE", 1) # Enable/Disable on Takeoff/Landing + self.set_parameter("FENCE_ACTION", 1) # RTL + self.set_parameter("FENCE_TYPE", 13) # Set Fence Type to Alt Max/Min and Polygon + self.set_parameter("FENCE_ALT_MIN", 80) + self.set_parameter("FENCE_ALT_MAX", 120) + + # Now we can arm the vehicle and kick off the flight + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode("AUTO") + + # On takeoff complete, check fence is enabled + self.progress("Looking for takeoff complete") + self.mavproxy.expect("APM: Takeoff complete at [0-9]+\.[0-9]+m") + #self.progress("Looking for fence enable") + #self.mavproxy.expect("fence enabled") + + # Fly until we breach fence floor where we enter RTL + self.progress("Looking for fence breach") + self.mavproxy.expect("fence breach") + + # As we RTL, we should re-enter fence zone. Wait until we're inside + self.progress("Waiting for return to altitude") + self.wait_altitude(altitude_min=100, altitude_max=110, timeout=30, relative=True) + + # Skip the waypoint outside fence, continue mission + self.progress("Skipping to next waypoint") + self.mav.waypoint_set_current_send(4) + self.change_mode("AUTO") + self.drain_mav() + + # Fly until we breach fence ceiling where we enter RTL + self.mavproxy.expect("fence breach") + + # As we RTL, we should re-enter fence zone. Wait until we're inside + self.wait_altitude(altitude_min=100, altitude_max=110, timeout=60, relative=True) + + # Skip the waypoint outside fence, continue mission + self.progress("Skipping to next waypoint") + self.mav.waypoint_set_current_send(6) + self.change_mode("AUTO") + self.drain_mav() + + # Continue through to landing sequence, which disables the fence + self.progress("Continuing mission to land") + self.mavproxy.expect("APM: Fence disabled \(auto disable\)") + + # Once landed, wait till disarmed + self.wait_disarmed(timeout=120) + except Exception as e: + self.progress("Exception caught:") + self.progress(self.get_exception_stacktrace(e)) + ex = e + if ex is not None: + raise ex + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -2400,6 +2473,10 @@ class AutoTestPlane(AutoTest): "Test Fence RTL Rally", self.test_fence_rtl_rally), + ("FenceAltCeilFloor", + "Tests the fence ceiling and floor", + self.test_fence_alt_ceil_floor), + ("ADSB", "Test ADSB", self.test_adsb),