From d7f82eddf6c3ea0b5cf20f7f683e3852d6010659 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Feb 2024 13:58:28 +1100 Subject: [PATCH 1/4] fence: add option to move circle --- MAVProxy/modules/mavproxy_fence.py | 38 ++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/MAVProxy/modules/mavproxy_fence.py b/MAVProxy/modules/mavproxy_fence.py index 7ab9ac1971..49e86574a8 100644 --- a/MAVProxy/modules/mavproxy_fence.py +++ b/MAVProxy/modules/mavproxy_fence.py @@ -445,6 +445,9 @@ def removereturnpoint(self, seq): self.wploader.last_change = time.time() self.send_all_items() + 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 +465,40 @@ 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 is_circle_item(self, item): return item.command in [ mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, @@ -681,6 +718,7 @@ def commands(self): ret = super(FenceModule, self).commands() ret.update({ 'addcircle': (self.cmd_addcircle, ["", "RADIUS"]), + 'movecircle': (self.cmd_movecircle, []), 'addpoly': (self.cmd_addpoly, ["", "" "", ""]), 'movepolypoint': (self.cmd_movepolypoint, ["POLY_FIRSTPOINT", "POINT_OFFSET"]), 'addreturnpoint': (self.cmd_addreturnpoint, []), From e46f82e490ecba073738887d053bcd29cc925d5f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Feb 2024 13:58:38 +1100 Subject: [PATCH 2/4] map: add option to move circle --- MAVProxy/modules/mavproxy_map/__init__.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 152e529e30..d06e2a2e1c 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -33,6 +33,7 @@ def __init__(self, mpstate): self.moving_rally = None self.mission_list = None self.moving_polygon_point = None + self.moving_circle = None self.icon_counter = 0 self.circle_counter = 0 self.draw_line = None @@ -325,6 +326,13 @@ 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_remove_returnpoint(self, id): '''called when a returnpoint is right-clicked and remove is selected; removes the return point @@ -371,6 +379,7 @@ def display_polyfences_circles(self, circles, colour): lng *= 1e-7 items = [ MPMenuItem('Remove Circle', returnkey='popupPolyFenceRemoveCircle'), + MPMenuItem('Move Circle', returnkey='popupPolyFenceMoveCircle'), ] popup = MPMenuSubMenu('Popup', items) slipcircle = mp_slipmap.SlipCircle( @@ -596,6 +605,8 @@ 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 == 'popupPolyFenceRemoveReturnPoint': self.polyfence_remove_returnpoint(obj.selected[0].objkey) elif menuitem.returnkey == 'popupPolyFenceRemovePolygon': @@ -672,6 +683,17 @@ 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 + def click_updated(self): '''called when the click position has changed''' if self.map_settings.showclicktime == 0: From 657240a4b741c06b2f74871753bd6779763841f4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Feb 2024 19:18:03 +1100 Subject: [PATCH 3/4] fence: add options to change circle radii --- MAVProxy/modules/mavproxy_fence.py | 54 +++++++++++++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/MAVProxy/modules/mavproxy_fence.py b/MAVProxy/modules/mavproxy_fence.py index 49e86574a8..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,18 @@ 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])) @@ -499,6 +511,45 @@ def movecircle(self, seq): 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, @@ -719,6 +770,7 @@ def commands(self): 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, []), From 3832714a73f73bfd54189c47205402775327eba9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Feb 2024 19:18:11 +1100 Subject: [PATCH 4/4] map: add options to change circle radii --- MAVProxy/modules/mavproxy_map/__init__.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index d06e2a2e1c..05c011df15 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -34,6 +34,7 @@ def __init__(self, mpstate): 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 @@ -333,6 +334,12 @@ def polyfence_move_circle(self, id): (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 @@ -380,6 +387,7 @@ def display_polyfences_circles(self, circles, colour): 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( @@ -607,6 +615,8 @@ def handle_menu_event(self, obj): 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': @@ -694,6 +704,17 @@ def map_callback(self, obj): 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: