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,