Ejemplo n.º 1
0
	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)
Ejemplo n.º 2
0
 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)
Ejemplo n.º 3
0
 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
Ejemplo n.º 4
0
    def cmd_snap_fence(self, args):
        '''snap fence to KML'''
        threshold = 10.0
        if len(args) > 0:
            threshold = float(args[0])
        fencemod = self.module('fence')
        loader = fencemod.fenceloader
        changed = False
        for i in range(0,loader.count()):
            fp = loader.point(i)
            lat = fp.lat
            lon = fp.lng
            best = None
            best_dist = (threshold+1)*3
            for (snap_lat,snap_lon) in self.snap_points:
                dist = mp_util.gps_distance(lat, lon, snap_lat, snap_lon)
                if dist < best_dist:
                    best_dist = dist
                    best = (snap_lat, snap_lon)
            if best is not None and best_dist <= threshold:
                if best[0] != lat or best[1] != lon:
                    loader.move(i, best[0], best[1])
                    print("Snapping fence point %u to %f %f" % (i, best[0], best[1]))
                    changed = True
            elif best is not None:
                if best_dist <= (threshold+1)*3:
                    print("Not snapping fence point %u dist %.1f" % (i, best_dist))

        if changed:
            fencemod.send_fence()
Ejemplo n.º 5
0
    def draw(self, img, pixmapper, bounds):
        '''draw a polygon on the image'''
        if self.hidden:
            return
        (x, y, w, h) = bounds
        spacing = 1000
        while True:
            start = mp_util.latlon_round((x, y), spacing)
            dist = mp_util.gps_distance(x, y, x + w, y + h)
            count = int(dist / spacing)
            if count < 2:
                spacing /= 10
            elif count > 50:
                spacing *= 10
            else:
                break

        for i in range(count * 2 + 2):
            pos1 = mp_util.gps_newpos(start[0], start[1], 90, i * spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 0, 3 * count * spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour,
                           self.linewidth)

            pos1 = mp_util.gps_newpos(start[0], start[1], 0, i * spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 90,
                                      3 * count * spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour,
                           self.linewidth)
Ejemplo n.º 6
0
 def cmd_snap_wp(self, args):
     '''snap waypoints to KML'''
     threshold = 10.0
     if len(args) > 0:
         threshold = float(args[0])
     wpmod = self.module('wp')
     wploader = wpmod.wploader
     changed = False
     for i in range(1,wploader.count()):
         w = wploader.wp(i)
         if not wploader.is_location_command(w.command):
             continue
         lat = w.x
         lon = w.y
         best = None
         best_dist = (threshold+1)*3
         for (snap_lat,snap_lon) in self.snap_points:
             dist = mp_util.gps_distance(lat, lon, snap_lat, snap_lon)
             if dist < best_dist:
                 best_dist = dist
                 best = (snap_lat, snap_lon)
         if best is not None and best_dist <= threshold:
             if w.x != best[0] or w.y != best[1]:
                 w.x = best[0]
                 w.y = best[1]
                 print("Snapping WP %u to %f %f" % (i, w.x, w.y))
                 wploader.set(w, i)
                 changed = True
         elif best is not None:
             if best_dist <= (threshold+1)*3:
                 print("Not snapping wp %u dist %.1f" % (i, best_dist))
     if changed:
         wpmod.send_all_waypoints()
Ejemplo n.º 7
0
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
Ejemplo n.º 8
0
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
Ejemplo n.º 9
0
 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)
Ejemplo n.º 10
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))
Ejemplo n.º 11
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))
Ejemplo n.º 12
0
 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)
Ejemplo n.º 13
0
 def wp_slope(self, args):
     '''show slope of waypoints'''
     if len(args) == 2:
         # specific waypoints
         wp1 = int(args[0])
         wp2 = int(args[1])
         w1 = self.wploader.wp(wp1)
         w2 = self.wploader.wp(wp2)
         delta_alt = w1.z - w2.z
         if delta_alt == 0:
             slope = "Level"
         else:
             delta_xy = mp_util.gps_distance(w1.x, w1.y, w2.x, w2.y)
             slope = "%.1f" % (delta_xy / delta_alt)
         print("wp%u -> wp%u %s" % (wp1, wp2, slope))
         return
     if len(args) != 0:
         print("Usage: wp slope WP1 WP2")
         return
     last_w = None
     for i in range(1, self.wploader.count()):
         w = self.wploader.wp(i)
         if w.command not in [
                 mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                 mavutil.mavlink.MAV_CMD_NAV_LAND
         ]:
             continue
         if last_w is not None:
             if last_w.frame != w.frame:
                 print("WARNING: frame change %u -> %u at %u" %
                       (last_w.frame, w.frame, i))
             delta_alt = last_w.z - w.z
             if delta_alt == 0:
                 slope = "Level"
             else:
                 delta_xy = mp_util.gps_distance(w.x, w.y, last_w.x,
                                                 last_w.y)
                 slope = "%.1f" % (delta_xy / delta_alt)
             print("WP%u: slope %s" % (i, slope))
         last_w = w
Ejemplo n.º 14
0
 def could_collide_hor(self, vpos, adsb_pkt):
     '''return true if vehicle could come within filter_dist_xy meters of adsb vehicle in timeout seconds'''
     margin = self.asterix_settings.filter_dist_xy
     timeout = self.asterix_settings.filter_time
     alat = adsb_pkt.lat * 1.0e-7
     alon = adsb_pkt.lon * 1.0e-7
     avel = adsb_pkt.hor_velocity * 0.01
     vvel = sqrt(vpos.vx**2 + vpos.vy**2)
     dist = mp_util.gps_distance(vpos.lat, vpos.lon, alat, alon)
     dist -= avel * timeout
     dist -= vvel * timeout
     if dist <= margin:
         return True
     return False
Ejemplo n.º 15
0
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 in [ "AUTO", "GUIDED", "LOITER", "RTL" ]:
            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()
Ejemplo n.º 16
0
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
Ejemplo n.º 17
0
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
Ejemplo n.º 18
0
 def draw(self, img, pixmapper, bounds):
     if self.hidden:
         return
     center_px = pixmapper(self.latlon)
     # figure out pixels per meter
     ref_pt = (self.latlon[0] + 1.0, self.latlon[1])
     dis = mp_util.gps_distance(self.latlon[0], self.latlon[1], ref_pt[0],
                                ref_pt[1])
     ref_px = pixmapper(ref_pt)
     dis_px = math.sqrt(float(center_px[1] - ref_px[1])**2.0)
     pixels_per_meter = dis_px / dis
     radius_px = int(self.radius * pixels_per_meter)
     if self.start_angle is not None:
         axes = (radius_px, radius_px)
         cv2.ellipse(img, center_px, axes, self.rotation, self.start_angle,
                     self.end_angle, self.color, self.linewidth)
         if self.add_radii:
             rimpoint1 = (
                 center_px[0] + int(radius_px * math.cos(
                     math.radians(self.rotation + self.start_angle))),
                 center_px[1] + int(radius_px * math.sin(
                     math.radians(self.rotation + self.start_angle))))
             rimpoint2 = (
                 center_px[0] + int(radius_px * math.cos(
                     math.radians(self.rotation + self.end_angle))),
                 center_px[1] + int(radius_px * math.sin(
                     math.radians(self.rotation + self.end_angle))))
             cv2.line(img, center_px, rimpoint1, self.color, self.linewidth)
             cv2.line(img, center_px, rimpoint2, self.color, self.linewidth)
     else:
         cv2.circle(img, center_px, radius_px, self.color, self.linewidth)
     if self.arrow:
         SlipArrow(self.key,
                   self.layer, (center_px[0] - radius_px, center_px[1]),
                   self.color,
                   self.linewidth,
                   0,
                   reverse=self.reverse).draw(img)
         SlipArrow(self.key,
                   self.layer, (center_px[0] + radius_px, center_px[1]),
                   self.color,
                   self.linewidth,
                   math.pi,
                   reverse=self.reverse).draw(img)
Ejemplo n.º 19
0
 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: %.8f %.8f (%s)' % (lat, lon,
                                                mp_util.latlon_to_grid(
                                                    (lat, lon)))
         if state.elevation:
             alt = self.ElevationMap.GetElevation(lat, lon)
             if alt is not None:
                 newtext += ' %.1fm %uft' % (alt, alt * 3.28084)
     state.mt.set_download(state.download)
     pending = 0
     if state.download:
         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: %.8f %.8f (%s %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]), mp_util.latlon_to_grid(self.click_pos))
     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: %.3fm %.3fnm Bearing %.1f' % (
             distance, distance * 0.000539957, bearing)
     if newtext != state.oldtext:
         self.position.Clear()
         self.position.WriteText(newtext)
         state.oldtext = newtext
Ejemplo n.º 20
0
 def update(self, pkt, tnow):
     dt = tnow - self.last_time
     if dt < 0 or dt > 10:
         self.last_time = tnow
         return
     if dt < 0.1:
         return
     self.last_time = tnow
     dist = mp_util.gps_distance(self.pkt.lat * 1e-7, self.pkt.lon * 1e-7,
                                 pkt.lat * 1e-7, pkt.lon * 1e-7)
     if dist > 0.01:
         heading = mp_util.gps_bearing(self.pkt.lat * 1e-7,
                                       self.pkt.lon * 1e-7, pkt.lat * 1e-7,
                                       pkt.lon * 1e-7)
         spd = dist / dt
         pkt.heading = int(heading * 100)
         new_vel = int(spd * 100)
         #print(pkt.ICAO_address, new_vel*0.01, pkt.hor_velocity*0.01)
         pkt.hor_velocity = new_vel
     if pkt.hor_velocity > 65535:
         pkt.hor_velocity = 65535
     self.pkt = pkt
Ejemplo n.º 21
0
 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
Ejemplo n.º 22
0
	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)
Ejemplo n.º 23
0
 def distance_from(self, lat, lon):
     '''get distance from a point'''
     lat1 = self.pkt['I105']['Lat']['val']
     lon1 = self.pkt['I105']['Lon']['val']
     return mp_util.gps_distance(lat1, lon1, lat, lon)
Ejemplo n.º 24
0
 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)
Ejemplo n.º 25
0
    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()
Ejemplo n.º 26
0
                      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,
Ejemplo n.º 27
0
    def cmd_wp_movemulti(self, args, latlon=None):
        '''handle wp move of multiple waypoints'''
        if len(args) < 3:
            print("usage: wp movemulti WPNUM WPSTART WPEND <rotation>")
            return
        idx = int(args[0])
        if idx < 1 or idx > self.wploader.count():
            print("Invalid wp number %u" % idx)
            return
        wpstart = int(args[1])
        if wpstart < 1 or wpstart > self.wploader.count():
            print("Invalid wp number %u" % wpstart)
            return
        wpend = int(args[2])
        if wpend < 1 or wpend > self.wploader.count():
            print("Invalid wp number %u" % wpend)
            return
        if idx < wpstart or idx > wpend:
            print("WPNUM must be between WPSTART and WPEND")
            return

        # optional rotation about center point
        if len(args) > 3:
            rotation = float(args[3])
        else:
            rotation = 0

        if latlon is None:
            latlon = self.mpstate.click_location
        if latlon is None:
            print("No map click position available")
            return
        wp = self.wploader.wp(idx)
        if not self.wploader.is_location_command(wp.command):
            print("WP must be a location command")
            return

        (lat, lon) = latlon
        distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
        bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon)

        for wpnum in range(wpstart, wpend + 1):
            wp = self.wploader.wp(wpnum)
            if not self.wploader.is_location_command(wp.command):
                continue
            (newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing,
                                                  distance)
            if wpnum != idx and rotation != 0:
                # add in rotation
                d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
                b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
                (newlat, newlon) = mp_util.gps_newpos(lat, lon, b2 + rotation,
                                                      d2)

            if (getattr(self.console, 'ElevationMap', None) is not None and
                    wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT
                    and self.settings.wpterrainadjust):
                alt1 = self.console.ElevationMap.GetElevation(newlat, newlon)
                alt2 = self.console.ElevationMap.GetElevation(wp.x, wp.y)
                if alt1 is not None and alt2 is not None:
                    wp.z += alt1 - alt2
            wp.x = newlat
            wp.y = newlon
            wp.target_system = self.target_system
            wp.target_component = self.target_component
            self.wploader.set(wp, wpnum)

        self.loading_waypoints = True
        self.loading_waypoint_lasttime = time.time()
        self.master.mav.mission_write_partial_list_send(
            self.target_system, self.target_component, wpstart, wpend + 1)
        print("Moved WPs %u:%u to %f, %f rotation=%.1f" %
              (wpstart, wpend, lat, lon, rotation))
Ejemplo n.º 28
0
	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)
Ejemplo n.º 29
0
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 in ["AUTO", "GUIDED", "LOITER", "RTL"]:
            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()