Autotest: Correct the logic for fence based autotest functions

Adds corrections to enabling fence using aux function.
Correctly test fences statically. Only uploaded fences can be checked using a fence file, so we check those first. Then we add steps to check tin can, max and minm all set the fence as present, as expected.
Plane will support MAV_PROTOCOL_CAPABILITY_MISSION_FENCE, so we assert that it does support it.
To test ceiling and floor, leverage some existing functions for takeoff, change altitude and land. Check for respective breach.
Add a floor breach check to copter.

Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-fence.txt

@@ -1,6 +0,0 @@
--35.363720	149.163651
--35.358738	149.165070
--35.359295	149.154434
--35.372292	149.157135
--35.368290	149.166809
--35.358738	149.165070

Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/CMAC-mission.txt

@@ -1,10 +0,0 @@
-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

Tools/autotest/ArduPlane_Tests/FenceAltCeilFloor/flaps.txt

@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361279	149.164230	30.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361229	149.163025	80.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364563	149.163773	80.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364384	149.164795	80.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.358878	149.163760	80.000000	1
+6	0	0	177	2.000000	3.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+7	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.362915	149.162613	60.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.358867	149.164409	60.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359862	149.164697	55.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.360670	149.164857	39.889999	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.363041	149.165222	0.000000	1

Tools/autotest/arducopter.py

@@ -1102,7 +1102,7 @@ class AutoTestCopter(AutoTest):
     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_AUTOENABLE", 1) # fence
         self.set_parameter("FENCE_TYPE", 2) # circle
         fence_radius = 15
         self.set_parameter("FENCE_RADIUS", fence_radius)
@@ -1306,7 +1306,7 @@ class AutoTestCopter(AutoTest):
         """Hold loiter position."""
 
         # enable fence, disable avoidance
-        self.set_parameter("FENCE_ENABLE", 1)
+        self.set_parameter("FENCE_AUTOENABLE", 1)
         self.set_parameter("AVOID_ENABLE", 0)
         self.set_parameter("FENCE_TYPE", 1)
 
@@ -1335,6 +1335,42 @@ class AutoTestCopter(AutoTest):
 
         self.zero_throttle()
 
+    # fly_alt_fence_test - fly up until you hit the fence
+    def fly_alt_min_fence_test(self):
+        self.takeoff(50, mode="LOITER", timeout=120)
+        """Hold loiter position."""
+        self.mavproxy.send('switch 5\n')  # loiter mode
+        self.wait_mode('LOITER')
+
+        # enable fence, disable avoidance
+        self.set_parameter("FENCE_AUTOENABLE", 1)
+        self.set_parameter("AVOID_ENABLE", 0)
+        self.set_parameter("FENCE_TYPE", 8)
+
+        self.change_alt(10)
+
+        # first east
+        self.progress("turn east")
+        self.set_rc(4, 1580)
+        self.wait_heading(160)
+        self.set_rc(4, 1500)
+
+        # fly forward (east) at least 20m
+        self.set_rc(2, 1100)
+        self.wait_distance(20)
+
+        # stop flying forward and start flying based on input:
+        self.set_rc(2, 1500)
+        self.set_rc(3, 1200)
+
+        # wait for fence to trigger
+        self.wait_mode('RTL', timeout=120)
+
+        self.wait_rtl_complete()
+
+        self.zero_throttle()
+
+
     def fly_gps_glitch_loiter_test(self, timeout=30, max_distance=20):
         """fly_gps_glitch_loiter_test. Fly south east in loiter and test
         reaction to gps glitch."""
@@ -5032,7 +5068,7 @@ class AutoTestCopter(AutoTest):
         ex = None
         try:
             self.load_fence("copter-avoidance-fence.txt")
-            self.set_parameter("FENCE_ENABLE", 0)
+            self.set_parameter("FENCE_AUTOENABLE", 1)
             self.set_parameter("PRX_TYPE", 10)
             self.set_parameter("RC10_OPTION", 40) # proximity-enable
             self.reboot_sitl()
@@ -5135,7 +5171,7 @@ class AutoTestCopter(AutoTest):
         ex = None
         try:
             self.load_fence("copter-avoidance-fence.txt")
-            self.set_parameter("FENCE_ENABLE", 1)
+            self.set_parameter("FENCE_AUTOENABLE", 1)
             self.check_avoidance_corners()
         except Exception as e:
             self.print_exception_caught(e)
@@ -6479,6 +6515,10 @@ class AutoTestCopter(AutoTest):
              "Test Max Alt Fence",
              self.fly_alt_max_fence_test),  # 26s
 
+            ("MinAltFence",
+             "Test Max Alt Fence",
+             self.fly_alt_min_fence_test), #26s
+
             ("AutoTuneSwitch",
              "Fly AUTOTUNE on a switch",
              self.fly_autotune_switch),  # 105s

Tools/autotest/arduplane.py

@@ -969,16 +969,14 @@ class AutoTestPlane(AutoTest):
 
         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,
-                               enabled=False,
-                               healthy=True)
         self.set_rc_from_map({
             3: 1000,
             7: 2000,
-        })
+        }) # Turn fence on with aux function
+
+
+        self.mavproxy.expect("fence enabled")
+        self.delay_sim_time(1)  # This is needed else the SYS_STATUS may not have updated
 
         self.progress("Checking fence is initially OK")
         self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
@@ -1108,6 +1106,7 @@ class AutoTestPlane(AutoTest):
         ex = None
         try:
             self.progress("Checking for bizarre healthy-when-not-present-or-enabled")
+            self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present
             self.assert_fence_sys_status(False, False, True)
             self.load_fence("CMAC-fence.txt")
             m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
@@ -1149,6 +1148,19 @@ class AutoTestPlane(AutoTest):
             self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True)
             if self.get_parameter("FENCE_TOTAL") != 0:
                 raise NotAchievedException("Expected zero points remaining")
+            self.assert_fence_sys_status(False, False, True)
+            self.do_fence_disable()
+
+            # ensure that a fence is present if it is tin can, min alt or max alt
+            self.progress("Test other fence types (tin-can, min alt, max alt")
+            self.set_parameter("FENCE_TYPE", 1) # max alt
+            self.assert_fence_sys_status(True, False, True)
+            self.set_parameter("FENCE_TYPE", 8) # min alt
+            self.assert_fence_sys_status(True, False, True)
+            self.set_parameter("FENCE_TYPE", 2) # tin can
+            self.assert_fence_sys_status(True, False, True)
+
+
 
         except Exception as e:
             self.print_exception_caught(e)
@@ -1334,8 +1346,8 @@ class AutoTestPlane(AutoTest):
 
         self.change_mode('MANUAL')
 
-        self.progress("Asserting we don't support transfer of fence via mission item protocol")
-        self.assert_no_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE)
+        self.progress("Asserting we do support transfer of fence via mission item protocol")
+        self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE)
 
         # grab home position:
         self.mav.recv_match(type='HOME_POSITION', blocking=True)
@@ -2333,76 +2345,51 @@ class AutoTestPlane(AutoTest):
             raise ex
 
     def test_fence_alt_ceil_floor(self):
-        ex = None
-        target_system = 1
-        target_component = 1
-        try:
-            self.progress("Testing Fence Enable")
+        fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+        self.set_parameter("FENCE_TYPE", 9)     # Set fence type to max and min alt
+        self.set_parameter("FENCE_ACTION", 0)   # Set action to report
+        self.set_parameter("FENCE_ALT_MAX", 200)
+        self.set_parameter("FENCE_ALT_MIN", 100)
 
-            # 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
+        # Grab Home Position
+        self.mav.recv_match(type='HOME_POSITION', blocking=True)
+        self.homeloc = self.mav.location()
+
+        cruise_alt = 150
+        self.takeoff(cruise_alt)
+
+        self.do_fence_enable()
+
+        self.progress("Fly above ceiling and check for breach")
+        self.change_altitude(self.homeloc.alt + cruise_alt + 80)
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        print("%s" % str(m))
+        if ((m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence Ceiling did not breach")
+
+        self.progress("Return to cruise alt and check for breach clear")
+        self.change_altitude(self.homeloc.alt + cruise_alt)
+
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        print("%s" % str(m))
+        if (not (m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence breach did not clear")
+
+
+        self.progress("Fly below floor and check for breach")
+        self.change_altitude(self.homeloc.alt + cruise_alt - 80)
+
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        print("%s" % str(m))
+        if ((m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence Floor did not breach")
+
+        self.do_fence_disable()
+        
+        self.fly_home_land_and_disarm(timeout=150)
+
+
+        
 
     def tests(self):
         '''return list of all tests'''