autotest: add test for auto-disabling min alt fence breaches on disar…

…ming

clean-up fence manipulation functions and add test for auto-enablement on copter
update tests to have some FENCE_ENABLE tests
add avoidance minimum and maximum altitude fence
add fence switch test while flying
add FenceAutoEnableDisableSwitch for auto mode 2
add more scenarios for plane fence auto-enable
validate fence rc switch behaviour
check fence autoenable by taking off in guided mode
more FENCE_AUTOENABLE tests

Tools/autotest/Generic_Missions/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

Tools/autotest/arducopter.py

@@ -1575,6 +1575,7 @@ def MaxAltFence(self):
             "FENCE_ENABLE": 1,
             "AVOID_ENABLE": 0,
             "FENCE_TYPE": 1,
+            "FENCE_ENABLE" : 1,
         })
 
         self.change_alt(10)
@@ -1610,6 +1611,7 @@ def MinAltFence(self):
         # enable fence, disable avoidance
         self.set_parameters({
             "AVOID_ENABLE": 0,
+            "FENCE_ENABLE" : 1,
             "FENCE_TYPE": 8,
             "FENCE_ALT_MIN": 20,
         })
@@ -1655,6 +1657,7 @@ def FenceFloorEnabledLanding(self):
         self.progress("Test Landing while fence floor enabled")
         self.set_parameters({
             "AVOID_ENABLE": 0,
+            "FENCE_ENABLE" : 1,
             "FENCE_TYPE": 15,
             "FENCE_ALT_MIN": 10,
             "FENCE_ALT_MAX": 20,
@@ -1675,16 +1678,100 @@ def FenceFloorEnabledLanding(self):
         self.set_rc(3, 1800)
 
         self.wait_mode('RTL', timeout=120)
+
         self.wait_landed_and_disarmed()
         self.assert_fence_enabled()
 
-        # Assert fence is not healthy
+        # Assert fence is not healthy since it was enabled manually
         self.assert_sensor_state(fence_bit, healthy=False)
 
         # Disable the fence using mavlink command to ensure cleaned up SITL state
         self.do_fence_disable()
         self.assert_fence_disabled()
 
+    def FenceFloorAutoDisableLanding(self):
+        """Ensures we can initiate and complete an RTL while the fence is enabled"""
+
+        fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+
+        self.progress("Test Landing while fence floor enabled")
+        self.set_parameters({
+            "AVOID_ENABLE": 0,
+            "FENCE_TYPE": 11,
+            "FENCE_ALT_MIN": 10,
+            "FENCE_ALT_MAX": 20,
+            "FENCE_AUTOENABLE" : 1,
+        })
+
+        self.change_mode("GUIDED")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.takeoff(alt_min=15, mode="GUIDED")
+
+        # Check fence is enabled
+        self.assert_fence_enabled()
+
+        # Change to RC controlled mode
+        self.change_mode('LOITER')
+
+        self.set_rc(3, 1800)
+
+        self.wait_mode('RTL', timeout=120)
+        # Assert fence is not healthy now that we are in RTL
+        self.assert_sensor_state(fence_bit, healthy=False)
+
+        self.wait_landed_and_disarmed(0)
+        # the breach should have cleared since we auto-disable the
+        # fence on landing
+        self.assert_fence_disabled()
+
+        # Assert fences have gone now that we have landed and disarmed
+        self.assert_sensor_state(fence_bit, present=True, enabled=False)
+
+    def FenceFloorAutoEnableOnArming(self):
+        """Ensures we can auto-enable fences on arming and still takeoff and land"""
+
+        fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+
+        self.set_parameters({
+            "AVOID_ENABLE": 0,
+            "FENCE_TYPE": 11,
+            "FENCE_ALT_MIN": 10,
+            "FENCE_ALT_MAX": 20,
+            "FENCE_AUTOENABLE" : 3,
+        })
+
+        self.change_mode("GUIDED")
+        # Check fence is not enabled
+        self.assert_fence_disabled()
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.takeoff(alt_min=15, mode="GUIDED")
+
+        # Check fence is enabled
+        self.assert_fence_enabled()
+
+        # Change to RC controlled mode
+        self.change_mode('LOITER')
+
+        self.set_rc(3, 1800)
+
+        self.wait_mode('RTL', timeout=120)
+        # Assert fence is not healthy now that we are in RTL
+        self.assert_sensor_state(fence_bit, healthy=False)
+
+        self.wait_landed_and_disarmed(0)
+        # the breach should have cleared since we auto-disable the
+        # fence on landing
+        self.assert_fence_disabled()
+
+        # Assert fences have gone now that we have landed and disarmed
+        self.assert_sensor_state(fence_bit, present=True, enabled=False)
+
+        # Disable the fence using mavlink command to ensure cleaned up SITL state
+        self.assert_fence_disabled()
+
     def GPSGlitchLoiter(self, timeout=30, max_distance=20):
         """fly_gps_glitch_loiter_test. Fly south east in loiter and test
         reaction to gps glitch."""
@@ -7375,6 +7462,54 @@ def AC_Avoidance_Fence(self):
         self.set_parameter("FENCE_ENABLE", 1)
         self.check_avoidance_corners()
 
+    def AvoidanceAltFence(self):
+        '''Test fence avoidance at minimum and maximum altitude'''
+        ex = None
+        try:
+            self.set_parameters({
+                "FENCE_ENABLE": 1,
+                "FENCE_TYPE": 9,   # min and max alt fence
+                "FENCE_ALT_MIN": 10,
+                "FENCE_ALT_MAX": 30,
+            })
+
+            self.change_mode('LOITER')
+            self.wait_ekf_happy()
+
+            tstart = self.get_sim_time()
+            self.takeoff(15, mode='LOITER')
+            self.progress("Increasing throttle, vehicle should stay below 30m")
+            self.set_rc(3, 1920)
+
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time_cached() - tstart > 20:
+                    break
+                alt = self.get_altitude(relative=True)
+                self.progress("Altitude %s" % alt)
+                if alt > 30:
+                    raise NotAchievedException("Breached maximum altitude (%s)" % (str(alt),))
+
+            self.progress("Decreasing, vehicle should stay above 10m")
+            self.set_rc(3, 1080)
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time_cached() - tstart > 20:
+                    break
+                alt = self.get_altitude(relative=True)
+                self.progress("Altitude %s" % alt)
+                if alt < 10:
+                    raise NotAchievedException("Breached minimum altitude (%s)" % (str(alt),))
+
+        except Exception as e:
+            self.progress("Caught exception: %s" %
+                          self.get_exception_stacktrace(e))
+            ex = e
+        self.land_and_disarm()
+        self.disarm_vehicle(force=True)
+        if ex is not None:
+            raise ex
+
     def global_position_int_for_location(self, loc, time_boot, heading=0):
         return self.mav.mav.global_position_int_encode(
             int(time_boot * 1000), # time_boot_ms
@@ -10096,6 +10231,7 @@ def tests1c(self):
              self.AC_Avoidance_Proximity_AVOID_ALT_MIN,
              self.AC_Avoidance_Fence,
              self.AC_Avoidance_Beacon,
+             self.AvoidanceAltFence,
              self.BaroWindCorrection,
              self.SetpointGlobalPos,
              self.ThrowDoubleDrop,
@@ -10114,6 +10250,8 @@ def tests1d(self):
              self.MaxAltFence,
              self.MinAltFence,
              self.FenceFloorEnabledLanding,
+             self.FenceFloorAutoDisableLanding,
+             self.FenceFloorAutoEnableOnArming,
              self.AutoTuneSwitch,
              self.GPSGlitchLoiter,
              self.GPSGlitchLoiter2,

Tools/autotest/arduplane.py

@@ -3711,6 +3711,176 @@ def FenceAltCeilFloor(self):
 
         self.fly_home_land_and_disarm(timeout=150)
 
+    def FenceMinAltAutoEnable(self):
+        '''Tests autoenablement of the alt min fence and fences on arming'''
+        self.set_parameters({
+            "FENCE_TYPE": 9,     # Set fence type to min alt and max alt
+            "FENCE_ACTION": 1,   # Set action to RTL
+            "FENCE_ALT_MIN": 25,
+            "FENCE_ALT_MAX": 100,
+            "FENCE_AUTOENABLE": 3,
+            "FENCE_ENABLE" : 0,
+            "RTL_AUTOLAND" : 2,
+        })
+
+        # check we can takeoff again
+        for i in [1, 2]:
+            # Grab Home Position
+            self.mav.recv_match(type='HOME_POSITION', blocking=True)
+            self.homeloc = self.mav.location()
+
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            # max alt fence should now be enabled
+            if i == 1:
+                self.assert_fence_enabled()
+
+            self.takeoff(alt=50, mode='TAKEOFF')
+            self.change_mode("FBWA")
+            self.set_rc(3, 1100)    # lower throttle
+
+            self.progress("Waiting for RTL")
+            tstart = self.get_sim_time()
+            mode = "RTL"
+            while not self.mode_is(mode, drain_mav=False):
+                self.mav.messages['HEARTBEAT'].custom_mode
+                self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
+                    self.mav.flightmode, mode, self.get_altitude(relative=True)))
+                if (self.get_sim_time_cached() > tstart + 120):
+                    raise WaitModeTimeout("Did not change mode")
+            self.progress("Got mode %s" % mode)
+            self.fly_home_land_and_disarm()
+            self.change_mode("FBWA")
+            self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL)
+            self.set_current_waypoint(0, check_afterwards=False)
+            self.set_rc(3, 1000)    # lower throttle
+
+    def FenceAutoEnableDisableSwitch(self):
+        '''Tests autoenablement of regular fences and manual disablement'''
+        self.set_parameters({
+            "FENCE_TYPE": 11,     # Set fence type to min alt
+            "FENCE_ACTION": 1,   # Set action to RTL
+            "FENCE_ALT_MIN": 50,
+            "FENCE_ALT_MAX": 100,
+            "FENCE_AUTOENABLE": 2,
+            "FENCE_OPTIONS" : 1,
+            "FENCE_ENABLE" : 1,
+            "FENCE_RADIUS" : 300,
+            "FENCE_RET_ALT" : 0,
+            "FENCE_RET_RALLY" : 0,
+            "FENCE_TOTAL" : 0,
+            "TKOFF_ALT" : 75,
+            "RC7_OPTION" : 11,   # AC_Fence uses Aux switch functionality
+        })
+        fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+        # Grab Home Position
+        self.mav.recv_match(type='HOME_POSITION', blocking=True)
+        self.homeloc = self.mav.location()
+        self.set_rc_from_map({7: 1000}) # Turn fence off with aux function
+
+        self.wait_ready_to_arm()
+        cruise_alt = 75
+        self.takeoff(cruise_alt, mode='TAKEOFF')
+
+        self.progress("Fly above ceiling and check there is no breach")
+        self.set_rc(3, 2000)
+        self.change_altitude(self.homeloc.alt + cruise_alt + 80)
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        self.progress("Got (%s)" % str(m))
+        if (not (m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence Ceiling breached")
+
+        self.progress("Return to cruise alt")
+        self.set_rc(3, 1500)
+        self.change_altitude(self.homeloc.alt + cruise_alt)
+
+        self.progress("Fly below floor and check for no breach")
+        self.change_altitude(self.homeloc.alt + 25)
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        self.progress("Got (%s)" % str(m))
+        if (not (m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence Ceiling breached")
+
+        self.progress("Fly above floor and check fence is not re-enabled")
+        self.set_rc(3, 2000)
+        self.change_altitude(self.homeloc.alt + 75)
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        self.progress("Got (%s)" % str(m))
+        if (m.onboard_control_sensors_enabled & fence_bit):
+            raise NotAchievedException("Fence Ceiling re-enabled")
+
+        self.progress("Return to cruise alt")
+        self.set_rc(3, 1500)
+        self.change_altitude(self.homeloc.alt + cruise_alt)
+        self.fly_home_land_and_disarm(timeout=250)
+
+    def FenceEnableDisableSwitch(self):
+        '''Tests enablement and disablement of fences on a switch'''
+        fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+
+        self.set_parameters({
+            "FENCE_TYPE": 4,     # Set fence type to polyfence
+            "FENCE_ACTION": 6,   # Set action to GUIDED
+            "FENCE_ALT_MIN": 10,
+            "FENCE_ENABLE" : 0,
+            "RC7_OPTION" : 11,   # AC_Fence uses Aux switch functionality
+        })
+
+        self.progress("Checking fence is not present before being configured")
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        self.progress("Got (%s)" % str(m))
+        if (m.onboard_control_sensors_enabled & fence_bit):
+            raise NotAchievedException("Fence enabled before being configured")
+
+        self.wait_ready_to_arm()
+        # takeoff at a lower altitude to avoid immediately breaching polyfence
+        self.takeoff(alt=25)
+        self.change_mode("FBWA")
+
+        self.load_fence("CMAC-fence.txt")
+
+        self.set_rc_from_map({
+            3: 1500,
+            7: 2000,
+        }) # Turn fence on with aux function
+
+        m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
+        self.progress("Got (%s)" % str(m))
+        if m is None:
+            raise NotAchievedException("Got FENCE_STATUS unexpectedly")
+
+        self.progress("Checking fence is initially OK")
+        self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE,
+                               present=True,
+                               enabled=True,
+                               healthy=True,
+                               verbose=False,
+                               timeout=30)
+
+        self.progress("Waiting for GUIDED")
+        tstart = self.get_sim_time()
+        mode = "GUIDED"
+        while not self.mode_is(mode, drain_mav=False):
+            self.mav.messages['HEARTBEAT'].custom_mode
+            self.progress("mav.flightmode=%s Want=%s Alt=%f" % (
+                self.mav.flightmode, mode, self.get_altitude(relative=True)))
+            if (self.get_sim_time_cached() > tstart + 120):
+                raise WaitModeTimeout("Did not change mode")
+        self.progress("Got mode %s" % mode)
+        # check we are in breach
+        self.assert_fence_enabled()
+
+        self.set_rc_from_map({
+            7: 1000,
+        }) # Turn fence off with aux function
+
+        # wait to no longer be in breach
+        self.delay_sim_time(5)
+        self.assert_fence_disabled()
+
+        self.fly_home_land_and_disarm(timeout=250)
+        self.do_fence_disable() # Ensure the fence is disabled after test
+
     def FenceBreachedChangeMode(self):
         '''Tests manual mode change after fence breach, as set with FENCE_OPTIONS'''
         """ Attempts to change mode while a fence is breached.
@@ -5380,6 +5550,9 @@ def tests(self):
             self.FenceRTLRally,
             self.FenceRetRally,
             self.FenceAltCeilFloor,
+            self.FenceMinAltAutoEnable,
+            self.FenceAutoEnableDisableSwitch,
+            self.FenceEnableDisableSwitch,
             self.FenceBreachedChangeMode,
             self.FenceNoFenceReturnPoint,
             self.FenceNoFenceReturnPointInclusion,