def size(self): '''return tile size as (width,height) in meters''' (lat1, lon1) = self.coord((0, 0)) (lat2, lon2) = self.coord((TILES_WIDTH, 0)) width = mp_util.gps_distance(lat1, lon1, lat2, lon2) (lat2, lon2) = self.coord((0, TILES_HEIGHT)) height = mp_util.gps_distance(lat1, lon1, lat2, lon2) return (width, height)
def mavflightview(filename): print("Loading %s ..." % filename) mlog = mavutil.mavlink_connection(filename) wp = mavwp.MAVWPLoader() if opts.mission is not None: wp.load(opts.mission) path = [] while True: m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT']) if m is None: break if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition(opts.condition): if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower(): continue lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 if lat != 0 or lng != 0: if mlog.flightmode in colourmap: point = (lat, lng, colourmap[mlog.flightmode]) else: point = (lat, lng) path.append(point) if m.get_type() == 'MISSION_ITEM': wp.set(m, m.seq) if len(path) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path) (lat, lon) = (bounds[0]+bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20): ground_width += 10 path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath', linewidth=2, colour=(255,0,180)) mission = wp.polygon() if len(mission) > 1: mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission', linewidth=2, colour=(255,255,255)) else: mission_obj = None if opts.imagefile: create_imagefile(opts.imagefile, (lat,lon), ground_width, path_obj, mission_obj) else: map = mp_slipmap.MPSlipMap(title=filename, service=opts.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon) map.add_object(path_obj) if mission_obj is not None: map.add_object(mission_obj)
def coord_to_pixel(self, lat, lon, width, ground_width, lat2, lon2): '''return pixel coordinate (px,py) for position (lat2,lon2) in an area image. Note that the results are relative to top,left and may be outside the image''' pixel_width = ground_width / float(width) if lat is None or lon is None or lat2 is None or lon2 is None: return (0, 0) dx = mp_util.gps_distance(lat, lon, lat, lon2) if lon2 < lon: dx = -dx dy = mp_util.gps_distance(lat, lon, lat2, lon) if lat2 > lat: dy = -dy dx /= pixel_width dy /= pixel_width return (int(dx), int(dy))
def closest_waypoint(self, latlon): '''find closest waypoint to a position''' (lat, lon) = latlon best_distance = -1 closest = -1 for i in range(self.module('wp').wploader.count()): w = self.module('wp').wploader.wp(i) distance = mp_util.gps_distance(lat, lon, w.x, w.y) if best_distance == -1 or distance < best_distance: best_distance = distance closest = i if best_distance < 20: return closest else: return -1
def mavflightview(filename): print("Loading %s ..." % filename) mlog = mavutil.mavlink_connection(filename) wp = mavwp.MAVWPLoader() if opts.mission is not None: wp.load(opts.mission) path = [] while True: m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT']) if m is None: break if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition( opts.condition): if opts.mode is not None and mlog.flightmode.lower( ) != opts.mode.lower(): continue lat = m.lat * 1.0e-7 lng = m.lon * 1.0e-7 if lat != 0 or lng != 0: if mlog.flightmode in colourmap: point = (lat, lng, colourmap[mlog.flightmode]) else: point = (lat, lng) path.append(point) if m.get_type() == 'MISSION_ITEM': wp.set(m, m.seq) if len(path) == 0: print("No points to plot") return bounds = mp_util.polygon_bounds(path) (lat, lon) = (bounds[0] + bounds[2], bounds[1]) (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50) ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3]) while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20 or mp_util.gps_distance( lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20): ground_width += 10 path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath', linewidth=2, colour=(255, 0, 180)) mission = wp.polygon() if len(mission) > 1: mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission', linewidth=2, colour=(255, 255, 255)) else: mission_obj = None if opts.imagefile: create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj, mission_obj) else: map = mp_slipmap.MPSlipMap(title=filename, service=opts.service, elevation=True, width=600, height=600, ground_width=ground_width, lat=lat, lon=lon) map.add_object(path_obj) if mission_obj is not None: map.add_object(mission_obj)
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if m.get_type() == "HEARTBEAT": from pymavlink import mavutil if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]: self.vehicle_type_name = 'plane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.vehicle_type_name = 'rover' elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER]: self.vehicle_type_name = 'copter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.vehicle_type_name = 'antenna' # this is the beginnings of allowing support for multiple vehicles # in the air at the same time vehicle = 'Vehicle%u' % m.get_srcSystem() if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos: self.create_vehicle_icon('Sim' + vehicle, 'green') self.mpstate.map.set_position('Sim' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw)) if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos: self.create_vehicle_icon('AHRS2' + vehicle, 'blue') self.mpstate.map.set_position('AHRS2' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw)) if m.get_type() == "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: self.create_vehicle_icon('GPS' + vehicle, 'blue') self.mpstate.map.set_position('GPS' + vehicle, (lat, lon), rotation=m.cog*0.01) if m.get_type() == "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.mpstate.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog*0.01) if m.get_type() == 'GLOBAL_POSITION_INT': (self.lat, self.lon, self.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01) if self.lat != 0 or self.lon != 0: self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True) self.mpstate.map.set_position('Pos' + vehicle, (self.lat, self.lon), rotation=self.heading) if m.get_type() == "NAV_CONTROLLER_OUTPUT": if self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL" ]: trajectory = [ (self.lat, self.lon), mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist) ] self.mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory', linewidth=2, colour=(255,0,180))) else: self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory')) # 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.fence_change_time != self.module('fence').fenceloader.last_change: self.display_fence() # if the rallypoints have changed, redisplay if self.rally_change_time != self.module('rally').rallyloader.last_change: self.rally_change_time = self.module('rally').rallyloader.last_change icon = self.mpstate.map.icon('rallypoint.png') self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints')) for i in range(self.module('rally').rallyloader.rally_count()): rp = self.module('rally').rallyloader.rally_point(i) popup = MPMenuSubMenu('Popup', items=[MPMenuItem('Rally Remove', returnkey='popupRallyRemove'), MPMenuItem('Rally Move', returnkey='popupRallyMove')]) self.mpstate.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.mpstate.map.add_object(mp_slipmap.SlipCircle('Rally Circ %u' % (i+1), 'RallyPoints', (rp.lat*1.0e-7, rp.lng*1.0e-7), abs(loiter_rad), (255,255,0), 2)) #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 != 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.mpstate.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.mpstate.map.check_events()
def distance(self, lat, lon): '''distance of this tile from a given lat/lon''' (tlat, tlon) = self.coord((TILES_WIDTH / 2, TILES_HEIGHT / 2)) return mp_util.gps_distance(lat, lon, tlat, tlon)
action='store_true', default=False, help="show debug info") (opts, args) = parser.parse_args() lat = opts.lat lon = opts.lon ground_width = opts.width if opts.boundary: boundary = mp_util.polygon_load(opts.boundary) bounds = mp_util.polygon_bounds(boundary) lat = bounds[0] + bounds[2] lon = bounds[1] ground_width = max( mp_util.gps_distance(lat, lon, lat, lon + bounds[3]), mp_util.gps_distance(lat, lon, lat - bounds[2], lon)) print lat, lon, ground_width mt = MPTile(debug=opts.debug, service=opts.service, tile_delay=opts.delay, max_zoom=opts.max_zoom) if opts.zoom is None: zooms = range(mt.min_zoom, mt.max_zoom + 1) else: zooms = [opts.zoom] for zoom in zooms: tlist = mt.area_to_tile_list(lat, lon, width=1024,