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 re_center(self, x, y, lat, lon):
'''re-center view for pixel x,y'''
state = self.state
(lat2,lon2) = self.coordinates(x, y)
distance = mp_util.gps_distance(lat2, lon2, lat, lon)
bearing = mp_util.gps_bearing(lat2, lon2, lat, lon)
(state.lat, state.lon) = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
def update_position(self):
'''update position text'''
state = self.state
pos = self.mouse_pos
newtext = ''
alt = 0
if pos is not None:
(lat,lon) = self.coordinates(pos.x, pos.y)
newtext += 'Cursor: %f %f' % (lat, lon)
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
newtext += ' %.1fm' % alt
pending = state.mt.tiles_pending()
if pending:
newtext += ' Map Downloading %u ' % pending
if alt == -1:
newtext += ' SRTM Downloading '
newtext += '\n'
if self.click_pos is not None:
newtext += 'Click: %f %f (%s %s)' % (self.click_pos[0], self.click_pos[1],
mp_util.degrees_to_dms(self.click_pos[0]),
mp_util.degrees_to_dms(self.click_pos[1]))
if self.last_click_pos is not None:
distance = mp_util.gps_distance(self.last_click_pos[0], self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
bearing = mp_util.gps_bearing(self.last_click_pos[0], self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
newtext += ' Distance: %.1fm Bearing %.1f' % (distance, bearing)
if newtext != state.oldtext:
self.position.Clear()
self.position.WriteText(newtext)
state.oldtext = newtext
def estimated_time_remaining(lat, lon, wpnum, speed):
"""estimate time remaining in mission in seconds"""
idx = wpnum
if wpnum >= mpstate.status.wploader.count():
return 0
distance = 0
done = set()
while idx < mpstate.status.wploader.count():
if idx in done:
break
done.add(idx)
w = mpstate.status.wploader.wp(idx)
if w.command == mavutil.mavlink.MAV_CMD_DO_JUMP:
idx = int(w.param1)
continue
idx += 1
if (w.x != 0 or w.y != 0) and w.command in [
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
mavutil.mavlink.MAV_CMD_NAV_LAND,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
]:
distance += mp_util.gps_distance(lat, lon, w.x, w.y)
lat = w.x
lon = w.y
if w.command == mavutil.mavlink.MAV_CMD_NAV_LAND:
break
return distance / speed
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) 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 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 re_center(self, x, y, lat, lon):
'''re-center view for pixel x,y'''
state = self.state
if lat is None or lon is None:
return
(lat2, lon2) = self.coordinates(x, y)
distance = mp_util.gps_distance(lat2, lon2, lat, lon)
bearing = mp_util.gps_bearing(lat2, lon2, lat, lon)
(state.lat, state.lon) = mp_util.gps_newpos(state.lat, state.lon,
bearing, distance)
def mavlink_packet(m):
'''handle an incoming mavlink packet'''
state = mpstate.map_state
if m.get_type() == "SIMSTATE":
if not mpstate.map_state.have_simstate:
mpstate.map_state.have_simstate = True
create_blueplane()
mpstate.map.set_position('blueplane', (m.lat, m.lng), rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and not mpstate.map_state.have_simstate:
(lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
if state.lat is not None and (mpstate.map_state.have_blueplane or
mp_util.gps_distance(lat, lon, state.lat, state.lon) > 10):
create_blueplane()
mpstate.map.set_position('blueplane', (lat, lon), rotation=m.cog*0.01)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if mpstate.master().flightmode == "AUTO":
trajectory = [ (state.lat, state.lon),
mp_util.gps_newpos(state.lat, state.lon, m.target_bearing, m.wp_dist) ]
mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory',
linewidth=2, colour=(255,0,180)))
else:
mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))
if m.get_type() == 'GLOBAL_POSITION_INT':
(state.lat, state.lon, state.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
else:
return
if state.lat != 0 or state.lon != 0:
mpstate.map.set_position('plane', (state.lat, state.lon), rotation=state.heading)
# if the waypoints have changed, redisplay
if state.wp_change_time != mpstate.status.wploader.last_change:
state.wp_change_time = mpstate.status.wploader.last_change
display_waypoints()
# if the fence has changed, redisplay
if state.fence_change_time != mpstate.status.fenceloader.last_change:
state.fence_change_time = mpstate.status.fenceloader.last_change
points = mpstate.status.fenceloader.polygon()
if len(points) > 1:
mpstate.map.add_object(mp_slipmap.SlipPolygon('fence', points, layer=1, linewidth=2, colour=(0,255,0)))
# check for any events from the map
mpstate.map.check_events()
def closest_waypoint(latlon):
'''find closest waypoint to a position'''
(lat, lon) = latlon
best_distance = -1
closest = -1
for i in range(mpstate.status.wploader.count()):
w = mpstate.status.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 closest_waypoint(latlon):
'''find closest waypoint to a position'''
(lat, lon) = latlon
best_distance = -1
closest = -1
for i in range(mpstate.status.wploader.count()):
w = mpstate.status.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 update_position(self):
'''update position text'''
state = self.state
pos = self.mouse_pos
self.position.Clear()
if pos is not None:
(lat,lon) = self.coordinates(pos.x, pos.y)
self.position.WriteText('Cursor: %f %f' % (lat, lon))
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
self.position.WriteText(' %.1fm' % alt)
pending = state.mt.tiles_pending()
if pending:
self.position.WriteText('Downloading %u ' % pending)
self.position.WriteText('\n')
if self.click_pos is not None:
self.position.WriteText('Click: %f %f' % (self.click_pos[0], self.click_pos[1]))
if self.last_click_pos is not None:
distance = mp_util.gps_distance(self.last_click_pos[0], self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
self.position.WriteText(' Distance: %.1fm' % distance)
def update_position(self):
'''update position text'''
state = self.state
pos = self.mouse_pos
newtext = ''
alt = 0
if pos is not None:
(lat, lon) = self.coordinates(pos.x, pos.y)
newtext += 'Cursor: %f %f' % (lat, lon)
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
newtext += ' %.1fm' % alt
pending = state.mt.tiles_pending()
if pending:
newtext += ' Map Downloading %u ' % pending
if alt == -1:
newtext += ' SRTM Downloading '
newtext += '\n'
if self.click_pos is not None:
newtext += 'Click: %f %f (%s %s)' % (
self.click_pos[0], self.click_pos[1],
mp_util.degrees_to_dms(self.click_pos[0]),
mp_util.degrees_to_dms(self.click_pos[1]))
if self.last_click_pos is not None:
distance = mp_util.gps_distance(self.last_click_pos[0],
self.last_click_pos[1],
self.click_pos[0],
self.click_pos[1])
bearing = mp_util.gps_bearing(self.last_click_pos[0],
self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
newtext += ' Distance: %.1fm Bearing %.1f' % (distance, bearing)
t1 = unicode(newtext, encoding='ascii', errors="replace")
if t1 != state.oldtext:
self.position.Clear()
self.position.WriteText(newtext)
state.oldtext = t1
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)
parser.add_option("--max-zoom", type='int', default=19, help="maximum tile zoom")
parser.add_option("--delay", type='float', default=1.0, help="tile download delay")
parser.add_option("--boundary", default=None, help="region boundary")
parser.add_option("--debug", 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, height=1024,
ground_width=ground_width, zoom=zoom)
print("zoom %u needs %u tiles" % (zoom, len(tlist)))
for tile in tlist:
mt.load_tile(tile)
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,
def mavlink_packet(m):
'''handle an incoming mavlink packet'''
state = mpstate.map_state
if m.get_type() == "SIMSTATE":
if not mpstate.map_state.have_simstate:
mpstate.map_state.have_simstate = True
create_blueplane()
mpstate.map.set_position('blueplane', (m.lat, m.lng),
rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and not mpstate.map_state.have_simstate:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if state.lat is not None and (
mpstate.map_state.have_blueplane
or mp_util.gps_distance(lat, lon, state.lat, state.lon) > 10):
create_blueplane()
mpstate.map.set_position('blueplane', (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if mpstate.master().flightmode == "AUTO":
trajectory = [(state.lat, state.lon),
mp_util.gps_newpos(state.lat, state.lon,
m.target_bearing, m.wp_dist)]
mpstate.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer='Trajectory',
linewidth=2,
colour=(255, 0, 180)))
else:
mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))
if m.get_type() == 'GLOBAL_POSITION_INT':
(state.lat, state.lon, state.heading) = (m.lat * 1.0e-7,
m.lon * 1.0e-7, m.hdg * 0.01)
else:
return
if state.lat != 0 or state.lon != 0:
mpstate.map.set_position('plane', (state.lat, state.lon),
rotation=state.heading)
# if the waypoints have changed, redisplay
if state.wp_change_time != mpstate.status.wploader.last_change:
state.wp_change_time = mpstate.status.wploader.last_change
display_waypoints()
# if the fence has changed, redisplay
if state.fence_change_time != mpstate.status.fenceloader.last_change:
state.fence_change_time = mpstate.status.fenceloader.last_change
points = mpstate.status.fenceloader.polygon()
if len(points) > 1:
mpstate.map.add_object(
mp_slipmap.SlipPolygon('fence',
points,
layer=1,
linewidth=2,
colour=(0, 255, 0)))
# check for any events from the map
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)
