Add support for home-centered inclusion circle fence

Signed-off-by: Ryan Friedman <[email protected]>

Author: Ryanf55

MAVProxy/modules/mavproxy_fence.py

@@ -17,6 +17,7 @@
     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 +37,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 +56,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
 
@@ -81,12 +89,38 @@ def circles_of_type(self, t):
                 return []
             if p.command != t:
                 continue
+            if t == mavutil.mavlink.MAV_CMD_NAV_FENCE_HOME_CIRCLE_INCLUSION:
+                def no_home_error():
+                    print("Error - no home yet. Try again later.")
+
+                if self.module('wp') is not None:
+                    print("wp m")
+                    home = self.module('wp').get_home()
+                    print("gh")
+                    if home is not None:
+                        print("nh")
+                        # latlon = home.x, home.y)
+                        # print(latlon)
+                    else:
+                        no_home_error()
+                        return
+                else:
+                    no_home_error()
+                    return
+                print("initial", p)
+                print(home.x, home.y)
+                print(type(home.x))
+                p.x = home.x  # x (latitude)
+                p.y = home.y  # y (longitude)
+                print("after", p)
+
+            print(p)
             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)
+        return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) + 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'''
@@ -341,6 +375,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 +632,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 +848,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

@@ -558,6 +558,7 @@ def display_polyfences_circles(self, circles, colour):
     def display_polyfences_inclusion_circles(self):
         '''draws inclusion circles in the PolyFence layer with colour colour'''
         inclusions = self.module('fence').inclusion_circles()
+        print("display_polyfences_inclusion_circles", inclusions)
         self.display_polyfences_circles(inclusions, (0, 255, 0))
 
     def display_polyfences_exclusion_circles(self):