diff --git a/MAVProxy/modules/mavproxy_fence.py b/MAVProxy/modules/mavproxy_fence.py index 7ab9ac1971..08f4b53663 100644 --- a/MAVProxy/modules/mavproxy_fence.py +++ b/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, ["", "RADIUS"]), + 'movecircle': (self.cmd_movecircle, []), + 'setcircleradius': (self.cmd_setcircleradius, ["seq radius"]), 'addpoly': (self.cmd_addpoly, ["", "" "", ""]), 'movepolypoint': (self.cmd_movepolypoint, ["POLY_FIRSTPOINT", "POINT_OFFSET"]), 'addreturnpoint': (self.cmd_addreturnpoint, []), diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 152e529e30..05c011df15 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/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: