Add option to change circle radius (and move them)

MAVProxy/modules/mavproxy_fence.py

@@ -288,7 +288,7 @@ def cmd_addcircle(self, args):
         if not self.check_have_list():
             return
         if len(args) < 2:
-            print("Need 2 arguments")
+            print("Usage: fence setcircle inclusion|exclusion RADIUS")
             return
         t = args[0]
         radius = float(args[1])
@@ -445,6 +445,21 @@ def removereturnpoint(self, seq):
         self.wploader.last_change = time.time()
         self.send_all_items()
 
+    def cmd_setcircleradius(self, args):
+        if len(args) < 1:
+            print("fence setcircleradius INDEX RADIUS")
+            return
+
+        if len(args) < 2:
+            # this one will use the click position:
+            self.setcircleradius(int(args[0]))
+            return
+
+        self.setcircleradius(int(args[0]), radius=float(args[1]))
+
+    def cmd_movecircle(self, args):
+        self.movecircle(int(args[0]))
+
     def removecircle(self, seq):
         '''remove circle at offset seq'''
         if not self.check_have_list():
@@ -462,6 +477,79 @@ def removecircle(self, seq):
         self.wploader.last_change = time.time()
         self.send_all_items()
 
+    def movecircle(self, seq):
+        '''moves circle at polygon_start_seqence to map click point
+        '''
+        if not self.check_have_list():
+            return
+
+        item = self.wploader.item(seq)
+        if item is None:
+            print("No item %s" % str(seq))
+            return
+
+        if not self.is_circle_item(item):
+            print("Item %u is not a circle" % seq)
+            return
+
+        latlon = self.mpstate.click_location
+        if latlon is None:
+            print("No map click position available")
+            return
+
+        moving_item = self.wploader.item(seq)
+        moving_item.x = latlon[0]
+        moving_item.y = latlon[1]
+        if moving_item.get_type() == "MISSION_ITEM_INT":
+            moving_item.x *= 1e7
+            moving_item.y *= 1e7
+            moving_item.x = int(moving_item.x)
+            moving_item.y = int(moving_item.y)
+
+        self.wploader.set(moving_item, moving_item.seq)
+        self.wploader.last_change = time.time()
+
+        self.send_single_waypoint(moving_item.seq)
+
+    def setcircleradius(self, seq, radius=None):
+        '''change radius of circle at seq to radius
+        '''
+        if not self.check_have_list():
+            return
+
+        item = self.wploader.item(seq)
+        if item is None:
+            print("No item %s" % str(seq))
+            return
+
+        if not self.is_circle_item(item):
+            print("Item %u is not a circle" % seq)
+            return
+
+        if radius is None:
+            # calculate radius from clock position:
+            latlon = self.mpstate.click_location
+            if latlon is None:
+                print("No click position available")
+                return
+            item_x = item.x
+            item_y = item.y
+            if item.get_type() == 'MISSION_ITEM_INT':
+                item_x *= 1e-7
+                item_y *= 1e-7
+            radius = mavextra.distance_lat_lon(latlon[0], latlon[1], item_x, item_y)
+        elif radius <= 0:
+            print("radius must be positive")
+            return
+
+        changing_item = self.wploader.item(seq)
+        changing_item.param1 = radius
+
+        self.wploader.set(changing_item, changing_item.seq)
+        self.wploader.last_change = time.time()
+
+        self.send_single_waypoint(changing_item.seq)
+
     def is_circle_item(self, item):
         return item.command in [
             mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION,
@@ -681,6 +769,8 @@ def commands(self):
         ret = super(FenceModule, self).commands()
         ret.update({
             'addcircle': (self.cmd_addcircle, ["<inclusion|inc|exclusion|exc>", "RADIUS"]),
+            'movecircle': (self.cmd_movecircle, []),
+            'setcircleradius': (self.cmd_setcircleradius, ["seq radius"]),
             'addpoly': (self.cmd_addpoly, ["<inclusion|inc|exclusion|exc>", "<radius>" "<pointcount>", "<rotation>"]),
             'movepolypoint': (self.cmd_movepolypoint, ["POLY_FIRSTPOINT", "POINT_OFFSET"]),
             'addreturnpoint': (self.cmd_addreturnpoint, []),

MAVProxy/modules/mavproxy_map/__init__.py

@@ -33,6 +33,8 @@ def __init__(self, mpstate):
         self.moving_rally = None
         self.mission_list = None
         self.moving_polygon_point = None
+        self.moving_circle = None
+        self.setting_circle_radius = None
         self.icon_counter = 0
         self.circle_counter = 0
         self.draw_line = None
@@ -325,6 +327,19 @@ def polyfence_remove_circle(self, id):
         (seq, type) = id.split(":")
         self.module('fence').removecircle(int(seq))
 
+    def polyfence_move_circle(self, id):
+        '''called when a fence is right-clicked and move circle is selected; start
+        moving the circle
+        '''
+        (seq, t) = id.split(":")
+        self.moving_circle = int(seq)
+
+    def polyfence_set_circle_radius(self, id):
+        '''called when a fence is right-clicked and change-circle-radius is selected; next click sets the radius
+        '''
+        (seq, t) = id.split(":")
+        self.setting_circle_radius = int(seq)
+
     def polyfence_remove_returnpoint(self, id):
         '''called when a returnpoint is right-clicked and remove is selected;
         removes the return point
@@ -371,6 +386,8 @@ def display_polyfences_circles(self, circles, colour):
                 lng *= 1e-7
             items = [
                 MPMenuItem('Remove Circle', returnkey='popupPolyFenceRemoveCircle'),
+                MPMenuItem('Move Circle', returnkey='popupPolyFenceMoveCircle'),
+                MPMenuItem('Set Circle Radius w/click', returnkey='popupPolyFenceSetCircleRadius'),
             ]
             popup = MPMenuSubMenu('Popup', items)
             slipcircle = mp_slipmap.SlipCircle(
@@ -596,6 +613,10 @@ def handle_menu_event(self, obj):
             self.move_fencepoint(obj.selected[0].objkey, obj.selected[0].extra_info)
         elif menuitem.returnkey == 'popupPolyFenceRemoveCircle':
             self.polyfence_remove_circle(obj.selected[0].objkey)
+        elif menuitem.returnkey == 'popupPolyFenceMoveCircle':
+            self.polyfence_move_circle(obj.selected[0].objkey)
+        elif menuitem.returnkey == 'popupPolyFenceSetCircleRadius':
+            self.polyfence_set_circle_radius(obj.selected[0].objkey)
         elif menuitem.returnkey == 'popupPolyFenceRemoveReturnPoint':
             self.polyfence_remove_returnpoint(obj.selected[0].objkey)
         elif menuitem.returnkey == 'popupPolyFenceRemovePolygon':
@@ -672,6 +693,28 @@ def map_callback(self, obj):
                 time.time() - self.mpstate.click_time > 0.1):
                 self.mpstate.click(obj.latlon)
 
+        if obj.event.leftIsDown and self.moving_circle is not None:
+            self.mpstate.click(obj.latlon)
+            seq = self.moving_circle
+            self.mpstate.functions.process_stdin("fence movecircle %u" % int(seq))
+            self.moving_circle = None
+            return
+        if obj.event.rightIsDown and self.moving_circle is not None:
+            print("Cancelled circle move")
+            self.moving_circle = None
+            return
+
+        if obj.event.leftIsDown and self.setting_circle_radius is not None:
+            self.mpstate.click(obj.latlon)
+            seq = self.setting_circle_radius
+            self.mpstate.functions.process_stdin("fence setcircleradius %u" % int(seq))
+            self.setting_circle_radius = None
+            return
+        if obj.event.rightIsDown and self.setting_circle_radius is not None:
+            print("Cancelled circle move")
+            self.setting_circle_radius = None
+            return
+
     def click_updated(self):
         '''called when the click position has changed'''
         if self.map_settings.showclicktime == 0: