diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 3fb79965f5..4e1936775c 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -259,7 +259,6 @@ def display_waypoints(self): items = [MPMenuItem('WP Set', returnkey='popupMissionSet'), MPMenuItem('WP Remove', returnkey='popupMissionRemove'), MPMenuItem('WP Move', returnkey='popupMissionMove'), - MPMenuItem('Remove NoFly', returnkey='popupMissionRemoveNoFly'), ] popup = MPMenuSubMenu('Popup', items) self.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p, @@ -375,71 +374,6 @@ def remove_mission(self, key, selection_index): idx = self.selection_index_to_idx(key, selection_index) self.mpstate.functions.process_stdin('wp remove %u' % idx) - def validate_nofly(self): - seq_start = None - wploader = self.module('wp').wploader - for x in range(0,wploader.count()): - tmp = wploader.wp(x) - if tmp.seq != x: - print("Indexing error %u" % x) - return False - if tmp.command != mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: - if seq_start is not None: - print("Invalid sequence starting at %u" % x) - return False - continue - if seq_start is None: - seq_start = tmp - - if int(tmp.param1) != int(seq_start.param1): - print("Invalid sequence starting at %u" % seq_start.seq) - return False - - if x - seq_start.seq == tmp.param1-1: - # good sequence - seq_start = None - - if seq_start is not None: - print("Short nofly polygon list") - return False - - return True - - def remove_mission_nofly(self, key, selection_index): - '''remove a mission nofly polygon''' - if not self.validate_nofly(): - print("NoFly invalid") - return - print("NoFly valid") - - idx = self.selection_index_to_idx(key, selection_index) - wploader = self.module('wp').wploader - - if idx < 0 or idx >= wploader.count(): - print("Invalid wp number %u" % idx) - return - wp = wploader.wp(idx) - if wp.command != mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION: - print("Not an exclusion point (%u)" % idx) - return - - # we know the list is valid. Search for the start of the sequence to delete - tmp_idx = idx - while tmp_idx > 0: - tmp = wploader.wp(tmp_idx-1) - if (tmp.command != wp.command or - tmp.param1 != wp.param1): - break - tmp_idx -= 1 - - start_idx_to_delete = idx - ((idx-tmp_idx)%int(wp.param1)) - for i in range(int(start_idx_to_delete+wp.param1)-1,start_idx_to_delete-1,-1): - # remove in reverse order as wploader.remove re-indexes - print("Removing at %u" % i) - deadun = wploader.wp(i) - wploader.remove(deadun) - self.module('wp').send_all_waypoints() - def remove_fencepoint(self, key, selection_index): '''remove a fence point''' self.mpstate.functions.process_stdin('fence remove %u' % (selection_index+1)) @@ -470,8 +404,6 @@ def handle_menu_event(self, obj): self.move_rally(obj.selected[0].objkey) elif menuitem.returnkey == 'popupMissionSet': self.set_mission(obj.selected[0].objkey, obj.selected[0].extra_info) - elif menuitem.returnkey == 'popupMissionRemoveNoFly': - self.remove_mission_nofly(obj.selected[0].objkey, obj.selected[0].extra_info) elif menuitem.returnkey == 'popupMissionRemove': self.remove_mission(obj.selected[0].objkey, obj.selected[0].extra_info) elif menuitem.returnkey == 'popupMissionMove': diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index 7626e62314..d1bd911074 100644 --- a/MAVProxy/modules/mavproxy_wp.py +++ b/MAVProxy/modules/mavproxy_wp.py @@ -84,7 +84,6 @@ def __init__(self, mpstate): default=100)), MPMenuItem('Undo', 'Undo', '# wp undo'), MPMenuItem('Loop', 'Loop', '# wp loop'), - MPMenuItem('Add NoFly', 'Loop', '# wp noflyadd'), MPMenuItem( 'Add Takeoff', 'Add Takeoff', '# wp add_takeoff ', handler=MPMenuCallTextDialog( @@ -821,8 +820,6 @@ def cmd_wp(self, args): self.set_home_location() elif args[0] == "loop": self.wp_loop() - elif args[0] == "noflyadd": - self.nofly_add() elif args[0] == "status": self.wp_status() elif args[0] == "slope": @@ -1141,32 +1138,6 @@ def wp_loop(self): self.master.waypoint_count_send(self.wploader.count()) print("Closed loop on mission") - def nofly_add(self): - '''add a square flight exclusion zone''' - latlon = self.mpstate.click_location - if latlon is None: - print("No position chosen") - return - loader = self.wploader - (center_lat, center_lon) = latlon - points = [] - points.append(mp_util.gps_offset(center_lat, center_lon, -25, 25)) - points.append(mp_util.gps_offset(center_lat, center_lon, 25, 25)) - points.append(mp_util.gps_offset(center_lat, center_lon, 25, -25)) - points.append(mp_util.gps_offset(center_lat, center_lon, -25, -25)) - start_idx = loader.count() - for p in points: - wp = mavutil.mavlink.MAVLink_mission_item_message( - 0, 0, 0, 0, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0, 1, 4, 0, 0, 0, p[0], p[1], 0 - ) - loader.add(wp) - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - start_idx, start_idx+4) - print("Added nofly zone") - def wp_add_takeoff(self, args): '''add a takeoff as first mission item''' latlon = self.mpstate.click_location