def mavlink_packet_distance_sensor(self, vehicle, m): heading = self.heading_for_vehicle.get(vehicle, 0) tlayer = "Distance sensor for %u.%u id=%u" % ( m.get_srcSystem(), m.get_srcComponent(), m.id) slipkey = '%s-POS%u' % (tlayer, m.orientation) if not m.get_srcSystem() in self.lat_lon_for_vehicle: return if m.current_distance == m.max_distance: self.foreach_map(lambda a_map: a_map.remove_object(slipkey)) return (lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()] mav_sensor_rotation_to_degrees = { mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 0, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 45, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 90, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 135, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 180, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 225, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 270, mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 315, } if m.orientation in mav_sensor_rotation_to_degrees: degrees = mav_sensor_rotation_to_degrees[m.orientation] else: # print("bad orientation (%u)" % m.orientation) return if m.current_distance >= m.max_distance: return p = mp_util.gps_newpos(lat, lon, heading + degrees, 0) # start angle/end angle are either side of the primary axis, # which is rotated to be North start_angle = -22.5 end_angle = 22.5 self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, p, m.current_distance / 100.0, (255, 127, 0), linewidth=3, start_angle=start_angle, end_angle=end_angle, rotation=(-90 + (heading + degrees)) % 360, )))
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if m.get_type() == "ADSB_VEHICLE": id = 'ADSB-' + str(m.ICAO_address) if id not in self.threat_vehicles.keys(): # check to see if the vehicle is in the dict # if not then add it self.threat_vehicles[id] = ADSBVehicle(id=id, state=m.to_dict()) for mp in self.module_matching('map*'): from modules.lib import mp_menu from modules.mavproxy_map import mp_slipmap self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem(name=id, returnkey=None) threat_radius = get_threat_radius(m) selected_icon = get_threat_icon(m, self.threat_vehicles[id].icon) if selected_icon is not None: # draw the vehicle on the map popup = mp_menu.MPMenuSubMenu('ADSB', items=[self.threat_vehicles[id].menu_item]) icon = mp.map.icon(selected_icon) mp.map.add_object(mp_slipmap.SlipIcon(id, (m.lat * 1e-7, m.lon * 1e-7), icon, layer=3, rotation=m.heading*0.01, follow=False, trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)), popup_menu=popup)) if threat_radius > 0: mp.map.add_object(mp_slipmap.SlipCircle(id+":circle", 3, (m.lat * 1e-7, m.lon * 1e-7), threat_radius, (0, 255, 255), linewidth=1)) else: # the vehicle is in the dict # update the dict entry self.threat_vehicles[id].update(m.to_dict(), self.get_time()) for mp in self.module_matching('map*'): # update the map ground_alt = mp.ElevationMap.GetElevation(m.lat*1e-7, m.lon*1e-7) alt_amsl = m.altitude * 0.001 if alt_amsl > 0: alt = int(alt_amsl - ground_alt) label = str(alt) + "m" else: label = None mp.map.set_position(id, (m.lat * 1e-7, m.lon * 1e-7), rotation=m.heading*0.01, label=label, colour=(0,250,250)) mp.map.set_position(id+":circle", (m.lat * 1e-7, m.lon * 1e-7))
def mavlink_packet_obstacle_distance(self, vehicle, m): heading = self.heading_for_vehicle.get(vehicle, 0) tlayer = "Obstacle Distance for %u.%u type=%u" % ( m.get_srcSystem(), m.get_srcComponent(), m.sensor_type) if m.get_srcSystem() in self.lat_lon_for_vehicle: color = (255, 0, 255) (lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()] self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipClearLayer(tlayer))) increment = m.increment_f if increment == 0: increment = float(m.increment) measurement_count = 0 for i in range(0, 72): if m.distances[i] != 65535: measurement_count += 1 fov = measurement_count * increment start_angle = -increment / 2.0 end_angle = increment / 2.0 rotation_start = -90 + heading for i in range(0, 72): slipkey = '%s-POS%u' % (tlayer, i) if m.distances[i] == m.max_distance + 1: # no measurement self.foreach_map( lambda a_map: a_map.remove_object(slipkey)) measurement_count += 1 continue distance = m.distances[i] / 100.0 # cm -> m circle = mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), distance, color, linewidth=3, start_angle=start_angle, end_angle=end_angle, rotation=(m.angle_offset + rotation_start + i * increment) % 360, ) if m.distances[i] < m.min_distance: measurement_count += 1 self.foreach_map(lambda a_map: a_map.add_object(circle)) slipkey = "%s-range" % tlayer if fov == 360: # perfect circle # add a circle for max-range self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), m.max_distance / 100.0, color, linewidth=1, ))) else: # add an arc for max-range self.foreach_map(lambda a_map: a_map.add_object( mp_slipmap.SlipCircle( slipkey, 3, (lat, lon), m.max_distance / 100.0, color, linewidth=1, start_angle=-fov / 2, end_angle=fov / 2, rotation=(m.angle_offset + -90 + (heading)) % 360, add_radii=True, )))
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' from modules.mavproxy_map import mp_slipmap mtype = m.get_type() sysid = m.get_srcSystem() if mtype == "HEARTBEAT": vname = 'plane' if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]: vname = 'plane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]: vname = 'rover' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: vname = 'sub' elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]: vname = 'boat' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER ]: vname = 'copter' elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]: vname = 'singlecopter' elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]: vname = 'heli' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: vname = 'antenna' self.vehicle_type_by_sysid[sysid] = vname if not sysid in self.vehicle_type_by_sysid: self.vehicle_type_by_sysid[sysid] = 'plane' self.vehicle_type_name = self.vehicle_type_by_sysid[sysid] # this is the beginnings of allowing support for multiple vehicles # in the air at the same time vehicle = 'Vehicle%u' % m.get_srcSystem() if mtype == "SIMSTATE" and self.map_settings.showsimpos: self.create_vehicle_icon('Sim' + vehicle, 'green') self.map.set_position('Sim' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "AHRS2" and self.map_settings.showahrs2pos: self.create_vehicle_icon('AHRS2' + vehicle, 'blue') self.map.set_position('AHRS2' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "AHRS3" and self.map_settings.showahrs3pos: self.create_vehicle_icon('AHRS3' + vehicle, 'orange') self.map.set_position('AHRS3' + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)) elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: if m.vel > 300 or 'ATTITUDE' not in self.master.messages: cog = m.cog * 0.01 else: cog = math.degrees(self.master.messages['ATTITUDE'].yaw) self.create_vehicle_icon('GPS' + vehicle, 'blue') self.map.set_position('GPS' + vehicle, (lat, lon), rotation=cog) elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos: (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7) if lat != 0 or lon != 0: self.create_vehicle_icon('GPS2' + vehicle, 'green') self.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog * 0.01) elif mtype == 'GLOBAL_POSITION_INT' and self.map_settings.showahrspos: (lat, lon, heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01) self.lat_lon[m.get_srcSystem()] = (lat, lon) if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3: self.have_global_position = True self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True) if len(self.vehicle_type_by_sysid) > 1: label = str(sysid) else: label = None self.map.set_position('Pos' + vehicle, (lat, lon), rotation=heading, label=label, colour=(255, 255, 255)) self.map.set_follow_object('Pos' + vehicle, self.is_primary_vehicle(m)) elif mtype == 'HOME_POSITION': (lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7) icon = self.map.icon('home.png') self.map.add_object( mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon), icon, layer=3, rotation=0, follow=False)) elif mtype == "NAV_CONTROLLER_OUTPUT": tlayer = 'Trajectory%u' % m.get_srcSystem() if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ] and m.get_srcSystem() in self.lat_lon): (lat, lon) = self.lat_lon[m.get_srcSystem()] trajectory = [(lat, lon), mp_util.gps_newpos(lat, lon, m.target_bearing, m.wp_dist)] self.map.add_object( mp_slipmap.SlipPolygon('trajectory', trajectory, layer=tlayer, linewidth=2, colour=(255, 0, 180))) self.trajectory_layers.add(tlayer) else: if tlayer in self.trajectory_layers: self.map.add_object(mp_slipmap.SlipClearLayer(tlayer)) self.trajectory_layers.remove(tlayer) elif mtype == "POSITION_TARGET_GLOBAL_INT": # FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL? if not m.get_srcSystem() in self.lat_lon: return tlayer = 'PostionTarget%u' % m.get_srcSystem() (lat, lon) = self.lat_lon[m.get_srcSystem()] if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND", "FOLLOW" ]): lat_float = m.lat_int * 1e-7 lon_float = m.lon_int * 1e-7 vec = [(lat_float, lon_float), (lat, lon)] self.map.add_object( mp_slipmap.SlipPolygon('position_target', vec, layer=tlayer, linewidth=2, colour=(0, 255, 0))) else: self.map.add_object(mp_slipmap.SlipClearLayer(tlayer)) if not self.is_primary_vehicle(m): # the rest should only be done for the primary vehicle return # if the waypoints have changed, redisplay last_wp_change = self.module('wp').wploader.last_change if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1: self.wp_change_time = last_wp_change self.display_waypoints() #this may have affected the landing lines from the rally points: self.rally_change_time = time.time() # if the fence has changed, redisplay if (self.module('fence') and self.fence_change_time != self.module('fence').fenceloader.last_change): self.display_fence() # if the rallypoints have changed, redisplay if (self.module('rally') and self.rally_change_time != self.module('rally').last_change()): self.rally_change_time = self.module('rally').last_change() icon = self.map.icon('rallypoint.png') self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints')) for i in range(self.module('rally').rally_count()): rp = self.module('rally').rally_point(i) popup = MPMenuSubMenu( 'Popup', items=[ MPMenuItem('Rally Remove', returnkey='popupRallyRemove'), MPMenuItem('Rally Move', returnkey='popupRallyMove') ]) self.map.add_object( mp_slipmap.SlipIcon('Rally %u' % (i + 1), (rp.lat * 1.0e-7, rp.lng * 1.0e-7), icon, layer='RallyPoints', rotation=0, follow=False, popup_menu=popup)) loiter_rad = self.get_mav_param('WP_LOITER_RAD') if self.map_settings.rallycircle: self.map.add_object( mp_slipmap.SlipCircle( 'Rally Circ %u' % (i + 1), 'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7), loiter_rad, (255, 255, 0), 2, arrow=self.map_settings.showdirection)) #draw a line between rally point and nearest landing point nearest_land_wp = None nearest_distance = 10000000.0 for j in range(self.module('wp').wploader.count()): w = self.module('wp').wploader.wp(j) if (w.command == 21): #if landing waypoint #get distance between rally point and this waypoint dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) if (dis < nearest_distance): nearest_land_wp = w nearest_distance = dis if nearest_land_wp is not None: points = [] #tangential approach? if self.get_mav_param('LAND_BREAK_PATH') == 0: theta = math.degrees( math.atan(loiter_rad / nearest_distance)) tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad)) ral_bearing = mp_util.gps_bearing( nearest_land_wp.x, nearest_land_wp.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7) points.append( mp_util.gps_newpos(nearest_land_wp.x, nearest_land_wp.y, ral_bearing + theta, tan_dis)) else: #not tangential approach points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7)) points.append((nearest_land_wp.x, nearest_land_wp.y)) self.map.add_object( mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1), points, 'RallyPoints', (255, 255, 0), 2)) # check for any events from the map self.map.check_events()
def display_waypoints(self): '''display the waypoints''' from modules.mavproxy_map import mp_slipmap self.mission_list = self.module('wp').wploader.view_list() polygons = self.module('wp').wploader.polygon_list() self.map.add_object(mp_slipmap.SlipClearLayer('Mission')) for i in range(len(polygons)): p = polygons[i] if len(p) > 1: items = [ MPMenuItem('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, layer='Mission', linewidth=2, colour=(255, 255, 255), arrow=self.map_settings.showdirection, popup_menu=popup)) labeled_wps = {} self.map.add_object(mp_slipmap.SlipClearLayer('LoiterCircles')) for i in range(len(self.mission_list)): next_list = self.mission_list[i] for j in range(len(next_list)): #label already printed for this wp? if (next_list[j] not in labeled_wps): label = self.label_for_waypoint(next_list[j]) colour = self.colour_for_wp(next_list[j]) self.map.add_object( mp_slipmap.SlipLabel('miss_cmd %u/%u' % (i, j), polygons[i][j], label, 'Mission', colour=colour)) if (self.map_settings.loitercircle and self.module('wp').wploader.wp_is_loiter( next_list[j])): wp = self.module('wp').wploader.wp(next_list[j]) if wp.command != mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param3 != 0: # wp radius and direction is defined by the mission loiter_rad = wp.param3 elif wp.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param2 != 0: # wp radius and direction is defined by the mission loiter_rad = wp.param2 else: # wp radius and direction is defined by the parameter loiter_rad = self.get_mav_param('WP_LOITER_RAD') self.map.add_object( mp_slipmap.SlipCircle( 'Loiter Circle %u' % (next_list[j] + 1), 'LoiterCircles', polygons[i][j], loiter_rad, (255, 255, 255), 2, arrow=self.map_settings.showdirection)) labeled_wps[next_list[j]] = (i, j)
def cmd_map(self, args): '''map commands''' from modules.mavproxy_map import mp_slipmap if len(args) < 1: print("usage: map <icon|set>") elif args[0] == "icon": if len(args) < 3: print("Usage: map icon <lat> <lon> <icon>") else: lat = args[1] lon = args[2] flag = 'flag.png' if len(args) > 3: flag = args[3] + '.png' icon = self.map.icon(flag) self.map.add_object( mp_slipmap.SlipIcon('icon - %s [%u]' % (str(flag), self.icon_counter), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False)) self.icon_counter += 1 elif args[0] == "circle": if len(args) < 4: # map circle -27.70533373 153.23404844 5 red print("Usage: map circle <lat> <lon> <radius> <colour>") else: lat = args[1] lon = args[2] radius = args[3] colour = 'red' if len(args) > 4: colour = args[4] if colour == "red": colour = (255, 0, 0) elif colour == "green": colour = (0, 255, 0) elif colour == "blue": colour = (0, 0, 255) else: colour = eval(colour) circle = mp_slipmap.SlipCircle( "circle %u" % self.circle_counter, 3, (float(lat), float(lon)), float(radius), colour, linewidth=1, ) self.map.add_object(circle) self.circle_counter += 1 elif args[0] == "set": self.map_settings.command(args[1:]) self.map.add_object( mp_slipmap.SlipBrightness(self.map_settings.brightness)) elif args[0] == "sethome": self.cmd_set_home(args) elif args[0] == "sethomepos": self.cmd_set_homepos(args) elif args[0] == "setorigin": self.cmd_set_origin(args) elif args[0] == "setoriginpos": self.cmd_set_originpos(args) elif args[0] == "zoom": self.cmd_zoom(args) elif args[0] == "center": self.cmd_center(args) elif args[0] == "follow": self.cmd_follow(args) elif args[0] == "clear": self.cmd_clear(args) else: print("usage: map <icon|set>")