コード例 #1
0
ファイル: mp_tile.py プロジェクト: harking/MAVProxy
	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)
コード例 #2
0
ファイル: mp_slipmap.py プロジェクト: harking/MAVProxy
 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)
コード例 #3
0
ファイル: mp_slipmap.py プロジェクト: OxDuke/MatrixPilot
 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
コード例 #4
0
ファイル: console.py プロジェクト: kd0aij/MatrixPilot
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
コード例 #5
0
ファイル: mp_tile.py プロジェクト: harking/MAVProxy
	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))
コード例 #6
0
    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))
コード例 #7
0
ファイル: mp_slipmap.py プロジェクト: nethack11/MAVProxy
 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)
コード例 #8
0
ファイル: map.py プロジェクト: Inspirati/freeMixer
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()
コード例 #9
0
ファイル: map.py プロジェクト: ulise75/MAVProxy
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
コード例 #10
0
ファイル: map.py プロジェクト: krukhlis/MatrixPilot
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
コード例 #11
0
ファイル: mp_slipmap.py プロジェクト: harking/MAVProxy
 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)
コード例 #12
0
ファイル: mp_slipmap.py プロジェクト: nethack11/MAVProxy
 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
コード例 #13
0
ファイル: mp_tile.py プロジェクト: harking/MAVProxy
	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)
コード例 #14
0
ファイル: mp_tile.py プロジェクト: harking/MAVProxy
	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)
コード例 #15
0
ファイル: mp_tile.py プロジェクト: nethack11/MAVProxy
                      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,
コード例 #16
0
ファイル: map.py プロジェクト: krukhlis/MatrixPilot
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()
コード例 #17
0
ファイル: mp_tile.py プロジェクト: nethack11/MAVProxy
 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)