Tools: autotest: add tests for Plane fence

Tools/autotest/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/arduplane.py

@@ -16,6 +16,8 @@ from common import AutoTestTimeoutException
 from common import NotAchievedException
 from common import PreconditionFailedException
 
+from MAVProxy.modules.lib import mp_util
+
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
 SITL_START_LOCATION = mavutil.location(-35.362938, 149.165085, 585, 354)
@@ -846,6 +848,245 @@ class AutoTestPlane(AutoTest):
         if ex is not None:
             raise ex
 
+    def assert_fence_sys_status(self, present, enabled, health):
+        self.delay_sim_time(1)
+        self.drain_mav_unparsed()
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True, timeout=1)
+        if m is None:
+            raise NotAchievedException("Did not receive SYS_STATUS")
+        tests = [ ( "present", present, m.onboard_control_sensors_present ),
+                  ( "enabled", enabled, m.onboard_control_sensors_enabled ),
+                  ( "health", health, m.onboard_control_sensors_health ),
+                  ]
+        bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
+        for test in tests:
+            (name, want, field) = test
+            got = (field & bit) != 0
+            if want != got:
+                raise NotAchievedException("fence status incorrect; %s want=%u got=%u" %
+                                           (name, want, got))
+
+    def do_fence_en_or_dis_able(self, value, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
+        if value:
+            p1 = 1
+        else:
+            p1 = 0
+        self.run_cmd(mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE,
+                     p1, # param1
+                     0, # param2
+                     0, # param3
+                     0, # param4
+                     0, # param5
+                     0, # param6
+                     0, # param7
+                     want_result=want_result)
+
+    def do_fence_enable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
+        self.do_fence_en_or_dis_able(True, want_result=want_result)
+
+    def do_fence_disable(self, want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
+        self.do_fence_en_or_dis_able(False, want_result=want_result)
+
+    def wait_circling_point_with_radius(self, loc, want_radius, epsilon=2.0, min_circle_time=5):
+        on_radius_start_heading = None
+        average_radius = 0.0
+        circle_time_start = 0
+        done_time = False
+        done_angle = False
+        while True:
+            here = self.mav.location()
+            got_radius = self.get_distance(loc, here)
+            average_radius = 0.95*average_radius + 0.05*got_radius
+            now = self.get_sim_time()
+            on_radius = abs(got_radius - want_radius) < epsilon
+            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+            heading = m.heading
+            on_string = "off"
+            got_angle = ""
+            if on_radius_start_heading is not None:
+                got_angle = "%0.2f" % abs(on_radius_start_heading - heading) # FIXME
+                on_string = "on"
+
+            want_angle = 180 # we don't actually get this (angle-substraction issue.  But we get enough...
+            self.progress("wait-circling: got-r=%0.2f want-r=%f avg-r=%f %s want-a=%0.1f got-a=%s" %
+                          (got_radius, want_radius, average_radius, on_string, want_angle, got_angle))
+            if on_radius:
+                if on_radius_start_heading is None:
+                    on_radius_start_heading = heading
+                    average_radius = got_radius
+                    circle_time_start = self.get_sim_time()
+                    continue
+                if abs(on_radius_start_heading - heading) > want_angle: # FIXME
+                    done_angle = True
+                if self.get_sim_time() - circle_time_start > min_circle_time:
+                    done_time = True
+                if done_time and done_angle:
+                    return
+                continue
+            if on_radius_start_heading is not None:
+                average_radius = 0.0
+            on_radius_start_heading = None
+            circle_time_start = 0
+
+    def test_fence_static(self):
+        ex = None
+        try:
+            self.progress("Checking for bizarre healthy-when-not-present-or-enabled")
+            self.assert_fence_sys_status(False, False, True)
+            fence_filepath = os.path.join(self.mission_directory(),
+                                          "CMAC-fence.txt")
+            self.mavproxy.send("fence load %s\n" % fence_filepath)
+            self.mavproxy.expect("Loaded 6 geo-fence")
+            m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
+            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.assert_fence_sys_status(True, False, True)
+            self.mavproxy.send('fence enable\n')
+            self.mavproxy.expect("fence enabled")
+            self.assert_fence_sys_status(True, True, True)
+            m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
+            if m is None:
+                raise NotAchievedException("Did not get FENCE_STATUS");
+            if m.breach_status:
+                raise NotAchievedException("Breached fence unexpectedly (%u)" %
+                                           (m.breach_status))
+            self.mavproxy.send('fence disable\n')
+            self.mavproxy.expect("fence disabled")
+            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.assert_fence_sys_status(True, False, True)
+            self.mavproxy.send("fence clear\n")
+            self.mavproxy.expect("fence removed")
+            if self.get_parameter("FENCE_TOTAL") != 0:
+                raise NotAchievedException("Expected zero points remaining")
+            self.assert_fence_sys_status(False, False, True)
+            self.progress("Trying to enable fence with no points")
+            self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED)
+
+            # test a rather unfortunate behaviour:
+            self.progress("Killing a live fence with fence-clear")
+            self.mavproxy.send("fence load %s\n" % fence_filepath)
+            self.mavproxy.expect("Loaded 6 geo-fence")
+            self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
+            self.do_fence_enable()
+            self.assert_fence_sys_status(True, True, True)
+            self.mavproxy.send("fence clear\n")
+            self.mavproxy.expect("fence removed")
+            if self.get_parameter("FENCE_TOTAL") != 0:
+                raise NotAchievedException("Expected zero points remaining")
+            self.assert_fence_sys_status(False, False, True)
+
+        except Exception as e:
+            self.progress("Exception caught:")
+            self.progress(self.get_exception_stacktrace(e))
+            ex = e
+        self.mavproxy.send('fence clear\n')
+        if ex is not None:
+            raise ex
+
+    def test_fence_breach_circle_at(self, loc):
+        ex = None
+        try:
+            fence_filepath = os.path.join(self.mission_directory(),
+                                          "CMAC-fence.txt")
+            self.mavproxy.send("fence load %s\n" % fence_filepath)
+            self.mavproxy.expect("Loaded 6 geo-fence")
+            want_radius = 100
+            # when ArduPlane is fixed, remove this fudge factor
+            REALLY_BAD_FUDGE_FACTOR = 1.16
+            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.do_fence_enable()
+            self.assert_fence_sys_status(True, True, True)
+
+            self.takeoff(alt=45, alt_max=300)
+
+            tstart = self.get_sim_time()
+            while True:
+                if self.get_sim_time() - tstart > 30:
+                    raise NotAchievedException("Did not breach fence")
+                m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2)
+                if m is None:
+                    raise NotAchievedException("Did not get FENCE_STATUS");
+                if m.breach_status == 0:
+                    continue
+
+                # we've breached; check our state;
+                if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY:
+                    raise NotAchievedException("Unexpected breach type %u" %
+                                               (m.breach_type,))
+                if m.breach_count == 0:
+                    raise NotAchievedException("Unexpected breach count %u" %
+                                               (m.breach_count,))
+                self.assert_fence_sys_status(True, True, False)
+                break
+
+            self.wait_circling_point_with_radius(loc, expected_radius)
+
+            self.disarm_vehicle(force=True)
+            self.reboot_sitl()
+
+        except Exception as e:
+            self.progress("Exception caught:")
+            self.progress(self.get_exception_stacktrace(e))
+            ex = e
+        self.mavproxy.send('fence clear\n')
+        if ex is not None:
+            raise ex
+
+    def test_fence_rtl(self):
+        self.progress("Testing FENCE_ACTION_RTL no rally point")
+        self.test_fence_breach_circle_at(self.home_position_as_mav_location())
+
+    def location_offset_ne(self, location, north, east):
+        print("old: %f %f" % (location.lat, location.lng))
+        (lat, lng) = mp_util.gps_offset(location.lat, location.lng, east, north)
+        location.lat = lat
+        location.lng = lng
+        print("new: %f %f" % (location.lat, location.lng))
+
+    def test_fence_rtl_rally(self):
+        ex = None
+        target_system = 1
+        target_component = 1
+        try:
+            self.progress("Testing FENCE_ACTION_RTL with rally point")
+
+            self.wait_ready_to_arm()
+            loc = self.home_position_as_mav_location()
+            self.location_offset_ne(loc, 50, -50)
+
+            self.set_parameter("RALLY_TOTAL", 1)
+            self.mav.mav.rally_point_send(target_system,
+                                          target_component,
+                                          0, # sequence number
+                                          1, # total count
+                                          int(loc.lat * 1e7),
+                                          int(loc.lng * 1e7),
+                                          15,
+                                          0, # "break" alt?!
+                                          0, # "land dir"
+                                          0) # flags
+            self.delay_sim_time(1)
+            self.mavproxy.send("rally list\n")
+            self.test_fence_breach_circle_at(loc)
+        except Exception as e:
+            self.progress("Exception caught:")
+            self.progress(self.get_exception_stacktrace(e))
+            ex = e
+        self.mavproxy.send('rally clear\n')
+        if ex is not None:
+            raise ex
+
     def test_parachute(self):
         self.set_rc(9, 1000)
         self.set_parameter("CHUTE_ENABLED", 1)
@@ -1045,6 +1286,18 @@ class AutoTestPlane(AutoTest):
              "Test RangeFinder Basic Functionality",
              self.test_rangefinder),
 
+            ("FenceStatic",
+             "Test Basic Fence Functionality",
+             self.test_fence_static),
+
+            ("FenceRTL",
+             "Test Fence RTL",
+             self.test_fence_rtl),
+
+            ("FenceRTLRally",
+             "Test Fence RTL Rally",
+             self.test_fence_rtl_rally),
+
             ("LogDownLoad",
              "Log download",
              lambda: self.log_download(

Tools/autotest/common.py

@@ -13,6 +13,8 @@ import pexpect
 import fnmatch
 import operator
 
+from MAVProxy.modules.lib import mp_util
+
 from pymavlink import mavwp, mavutil, DFReader
 from pysim import util, vehicleinfo
 
@@ -1170,16 +1172,35 @@ class AutoTest(ABC):
     #################################################
     # UTILITIES
     #################################################
+    @staticmethod
+    def longitude_scale(lat):
+        ret = math.cos(lat * (math.radians(1)));
+        print("scale=%f" % ret)
+        return ret
+
     @staticmethod
     def get_distance(loc1, loc2):
         """Get ground distance between two locations."""
+        return AutoTest.get_distance_accurate(loc1, loc2)
         dlat = loc2.lat - loc1.lat
         try:
             dlong = loc2.lng - loc1.lng
         except AttributeError:
             dlong = loc2.lon - loc1.lon
 
-        return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5
+        return math.sqrt((dlat*dlat) + (dlong*dlong)*AutoTest.longitude_scale(loc2.lat)) * 1.113195e5
+
+    @staticmethod
+    def get_distance_accurate(loc1, loc2):
+        """Get ground distance between two locations."""
+        try:
+            lon1 = loc1.lng
+            lon2 = loc2.lng
+        except AttributeError:
+            lon1 = loc1.lon
+            lon2 = loc2.lon
+
+        return mp_util.gps_distance(loc1.lat, lon1, loc2.lat, lon2)
 
     @staticmethod
     def get_latlon_attr(loc, attrs):
@@ -1984,6 +2005,10 @@ class AutoTest(ABC):
                                    blocking=True)
         return self.get_distance_int(m, here)
 
+    def home_position_as_mav_location(self):
+        m = self.poll_home_position()
+        return mavutil.location(m.latitude*1.0e-7, m.longitude*1.0e-7, m.altitude*1.0e-3, 0)
+
     def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
         tstart = self.get_sim_time()
         while True: