mavproxy_fence: Support MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>

Author: Ryanf55

MAVProxy/modules/mavproxy_fence.py

@@ -17,6 +17,8 @@
     from MAVProxy.modules.lib.mp_menu import MPMenuCallTextDialog
     from MAVProxy.modules.lib.mp_menu import MPMenuItem
 
+DEFAULT_CIRCLE_SIZE = 500
+
 
 class FenceModule(mission_item_protocol.MissionItemProtocolModule):
     '''uses common MISSION_ITEM protocol base class to provide fence
@@ -36,14 +38,14 @@ def gui_menu_items(self):
                 'Add Inclusion Circle', 'Add Inclusion Circle', '# fence addcircle inc ',
                 handler=MPMenuCallTextDialog(
                     title='Radius (m)',
-                    default=500
+                    default=DEFAULT_CIRCLE_SIZE
                 )
             ),
             MPMenuItem(
                 'Add Exclusion Circle', 'Add Exclusion Circle', '# fence addcircle exc ',
                 handler=MPMenuCallTextDialog(
                     title='Radius (m)',
-                    default=500
+                    default=DEFAULT_CIRCLE_SIZE
                 )
             ),
             MPMenuItem(
@@ -55,6 +57,13 @@ def gui_menu_items(self):
             MPMenuItem(
                 'Add Return Point', 'Add Return Point', '# fence addreturnpoint',
             ),
+            MPMenuItem(
+                'Add Home Centered Inclusion Circle', 'Add Home Inclusion Circle', '# fence addhomecircle ',
+                handler=MPMenuCallTextDialog(
+                    title='Radius (m)',
+                    default=DEFAULT_CIRCLE_SIZE
+                )
+            ),
         ])
         return ret
 
@@ -82,12 +91,17 @@ def circles_of_type(self, t):
             if p.command != t:
                 continue
             ret.append(p)
+
         return ret
 
     def inclusion_circles(self):
         '''return a list of Circle inclusion fences - a single MISSION_ITEM each'''
         return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION)
 
+    def home_inclusion_circles(self):
+        '''return a list of around-home Circle inclusion fences - a single MISSION_ITEM each'''
+        return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION)
+
     def exclusion_circles(self):
         '''return a list of Circle exclusion fences - a single MISSION_ITEM each'''
         return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION)
@@ -341,6 +355,35 @@ def cmd_addcircle(self, args):
         self.append(m)
         self.send_all_items()
 
+    def cmd_addhomecircle(self, args):
+        '''adds a home-centered circle to the map with given radius'''
+        if not self.check_have_list():
+            return
+        if len(args) < 1:
+            print("Usage: fence addhomecircle RADIUS")
+            return
+        radius = float(args[0])
+
+        m = mavutil.mavlink.MAVLink_mission_item_int_message(
+            self.target_system,
+            self.target_component,
+            0,  # seq
+            mavutil.mavlink.MAV_FRAME_GLOBAL,  # frame
+            mavutil.mavlink.MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION,  # command
+            0,    # current
+            0,    # autocontinue
+            radius, # param1,
+            0.0,  # param2,
+            0.0,  # param3
+            0.0,  # param4
+            0,  # x (latitude), ignored
+            0,  # y (longitude), ignored
+            0,  # z (altitude)
+            self.mav_mission_type(),
+        )
+        self.append(m)
+        self.send_all_items()
+
     def cmd_addreturnpoint(self, args):
         '''adds a returnpoint at the map click location'''
         if not self.check_have_list():
@@ -569,6 +612,7 @@ def is_circle_item(self, item):
         return item.command in [
             mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
             mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION,
+            mavutil.mavlink.MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION,
         ]
 
     def find_polygon_point(self, polygon_start_seq, item_offset):
@@ -784,6 +828,7 @@ def commands(self):
         ret = super(FenceModule, self).commands()
         ret.update({
             'addcircle': (self.cmd_addcircle, ["<inclusion|inc|exclusion|exc>", "RADIUS"]),
+            'addhomecircle': (self.cmd_addhomecircle, ["RADIUS"]),
             'movecircle': (self.cmd_movecircle, []),
             'setcircleradius': (self.cmd_setcircleradius, ["seq radius"]),
             'addpoly': (self.cmd_addpoly, ["<inclusion|inc|exclusion|exc>", "<radius>" "<pointcount>", "<rotation>"]),

MAVProxy/modules/mavproxy_map/__init__.py

@@ -73,6 +73,8 @@ def __init__(self, mpstate):
             ('contour_grid_spacing', float, 30.0),
             ('contour_grid_extent', float, 20000.0),
         ])
+        self.home_pos = ()
+        self.last_rendered_home_pos = ()
 
         service = 'MicrosoftHyb'
         if 'MAP_SERVICE' in os.environ:
@@ -552,11 +554,14 @@ def polyfence_move_polygon_point(self, id, extra):
     def display_polyfences_circles(self, circles, colour):
         '''draws circles in the PolyFence layer with colour colour'''
         for circle in circles:
-            lat = circle.x
-            lng = circle.y
-            if circle.get_type() == 'MISSION_ITEM_INT':
-                lat *= 1e-7
-                lng *= 1e-7
+            if circle.command == mavutil.mavlink.MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION:
+                (lat, lng) = self.home_pos
+            else:
+                lat = circle.x
+                lng = circle.y
+                if circle.get_type() == 'MISSION_ITEM_INT':
+                    lat *= 1e-7
+                    lng *= 1e-7
             items = [
                 MPMenuItem('Remove Circle', returnkey='popupPolyFenceRemoveCircle'),
                 MPMenuItem('Move Circle', returnkey='popupPolyFenceMoveCircle'),
@@ -578,6 +583,11 @@ def display_polyfences_inclusion_circles(self):
         inclusions = self.module('fence').inclusion_circles()
         self.display_polyfences_circles(inclusions, (0, 255, 0))
 
+    def display_polyfences_home_inclusion_circles(self):
+        '''draws around-inclusion circles in the PolyFence layer'''
+        inclusions = self.module('fence').home_inclusion_circles()
+        self.display_polyfences_circles(inclusions, (0, 255, 96))
+
     def display_polyfences_exclusion_circles(self):
         '''draws exclusion circles in the PolyFence layer with colour colour'''
         exclusions = self.module('fence').exclusion_circles()
@@ -652,6 +662,7 @@ def display_polyfences(self):
         '''draws PolyFence items in the PolyFence layer'''
         self.map.add_object(mp_slipmap.SlipClearLayer('PolyFence'))
         self.display_polyfences_inclusion_circles()
+        self.display_polyfences_home_inclusion_circles()
         self.display_polyfences_exclusion_circles()
         self.display_polyfences_inclusion_polygons()
         self.display_polyfences_exclusion_polygons()
@@ -1251,11 +1262,11 @@ def mavlink_packet(self, m):
                 self.map.set_follow_object('Pos' + vehicle, self.message_is_from_primary_vehicle(m))
 
         elif mtype == 'HOME_POSITION':
-            (lat, lon) = (m.latitude*1.0e-7, m.longitude*1.0e-7)
+            self.home_pos = (m.latitude*1.0e-7, m.longitude*1.0e-7)
             icon = self.map.icon('home.png')
             self.map.add_object(mp_slipmap.SlipIcon(
                 'HOME_POSITION',
-                (lat, lon),
+                self.home_pos,
                 icon,
                 layer=3,
                 rotation=0,
@@ -1337,8 +1348,10 @@ def check_redisplay_fencepoints(self):
             else:
                 # old fence module
                 last_change = fence_module.fenceloader.last_change
-            if self.fence_change_time != last_change:
+            if (self.fence_change_time != last_change or
+                    self.home_pos != self.last_rendered_home_pos):
                 self.fence_change_time = last_change
+                self.last_rendered_home_pos = self.home_pos
                 self.display_fence()
 
     def check_redisplay_rallypoints(self):