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
@@ -28,6 +30,7 @@ def __init__(self, mpstate):
         self.present = False
         self.enabled = False
         self.healthy = True
+        self.last_home_change = 0
 
     def gui_menu_items(self):
         ret = super(FenceModule, self).gui_menu_items()
@@ -36,14 +39,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 +58,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
 
@@ -69,6 +79,12 @@ def count(self):
         '''return number of waypoints'''
         return self.wploader.count()
 
+    def last_change(self):
+        base_class_last_change = super().last_change()
+        if self.last_home_change > base_class_last_change:
+            return self.last_home_change
+        return base_class_last_change
+
     def circles_of_type(self, t):
         '''return a list of Circle fences of a specific type - a single
         MISSION_ITEM'''
@@ -81,12 +97,28 @@ 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:
+                    home = self.module('wp').get_home()
+                    if home is None:
+                        no_home_error()
+                        return
+                else:
+                    no_home_error()
+                    return
+                p.x = home.x  # x (latitude)
+                p.y = home.y  # y (longitude)
+
             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'''
@@ -222,9 +254,15 @@ def handle_sys_status(self, m):
         elif self.enabled is True and self.healthy is False:
             self.console.set_status('Fence', 'FEN', row=0, fg='red')
 
+    def handle_home_position(self, m):
+        self.last_home_change = time.time()  # see self.last_change in mavwp.py
+
     def mavlink_packet(self, m):
-        if m.get_type() == 'SYS_STATUS' and self.message_is_from_primary_vehicle(m):
+        m_type = m.get_type()
+        if m_type == 'SYS_STATUS' and self.message_is_from_primary_vehicle(m):
             self.handle_sys_status(m)
+        elif m_type == 'HOME_POSITION':
+            self.handle_home_position(m)
         super(FenceModule, self).mavlink_packet(m)
 
     def apply_function_to_points(self, function):
@@ -341,6 +379,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 +636,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 +852,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>"]),