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)
def area_to_tile_list(self, lat, lon, width, height, ground_width, zoom=None): '''return a list of TileInfoScaled objects needed for an area of land, with ground_width in meters, and width/height in pixels. lat/lon is the top left corner. If unspecified, the zoom is automatically chosen to avoid having to grow the tiles ''' pixel_width = ground_width / float(width) ground_height = ground_width * (height/(float(width))) top_right = mp_util.gps_newpos(lat, lon, 90, ground_width) bottom_left = mp_util.gps_newpos(lat, lon, 180, ground_height) bottom_right = mp_util.gps_newpos(bottom_left[0], bottom_left[1], 90, ground_width) # choose a zoom level if not provided if zoom is None: zooms = range(self.min_zoom, self.max_zoom+1) else: zooms = [zoom] for zoom in zooms: tile_min = self.coord_to_tile(lat, lon, zoom) (twidth,theight) = tile_min.size() tile_pixel_width = twidth / float(TILES_WIDTH) scale = pixel_width / tile_pixel_width if scale >= 1.0: break scaled_tile_width = int(TILES_WIDTH / scale) scaled_tile_height = int(TILES_HEIGHT / scale) # work out the bottom right tile tile_max = self.coord_to_tile(bottom_right[0], bottom_right[1], zoom) ofsx = int(tile_min.offsetx / scale) ofsy = int(tile_min.offsety / scale) srcy = ofsy dsty = 0 ret = [] # place the tiles for y in range(tile_min.y, tile_max.y+1): srcx = ofsx dstx = 0 for x in range(tile_min.x, tile_max.x+1): if dstx < width and dsty < height: ret.append(TileInfoScaled((x,y), zoom, scale, (srcx,srcy), (dstx,dsty))) dstx += scaled_tile_width-srcx srcx = 0 dsty += scaled_tile_height-srcy srcy = 0 return ret
def cmd_wp_move_rel_home(self, args, latlon=None):
'''handle wp move to a point relative to home by dist/bearing'''
if len(args) < 3:
print("usage: wp moverelhome WPNUM dist bearing")
return
idx = int(args[0])
if idx < 1 or idx > self.wploader.count():
print("Invalid wp number %u" % idx)
return
dist = float(args[1])
bearing = float(args[2])
home = self.get_home()
if home is None:
print("Need home")
return
wp = self.wploader.wp(idx)
if not self.wploader.is_location_command(wp.command):
print("Not a nav command")
return
(newlat, newlon) = mp_util.gps_newpos(home.x, home.y, bearing, dist)
wp.x = newlat
wp.y = newlon
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, idx)
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system, self.target_component, idx, idx + 1)
print("Moved WP %u %.1fm bearing %.1f from home" %
(idx, dist, bearing))
def on_mouse(self, event):
'''handle mouse events'''
state = self.state
pos = event.GetPosition()
if event.Leaving():
self.mouse_pos = None
else:
self.mouse_pos = pos
self.update_position()
if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
# send any event with a mouse button to the parent
latlon = self.coordinates(pos.x, pos.y)
selected = self.selected_objects(pos)
state.event_queue.put(SlipMouseEvent(latlon, event, selected))
if event.LeftDown():
self.mouse_down = pos
self.last_click_pos = self.click_pos
self.click_pos = self.coordinates(pos.x, pos.y)
if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
# drag map to new position
newpos = pos
dx = (self.mouse_down.x - newpos.x)
dy = -(self.mouse_down.y - newpos.y)
pdist = math.sqrt(dx**2 + dy**2)
if pdist > state.drag_step:
bearing = math.degrees(math.atan2(dx, dy))
distance = (state.ground_width/float(state.width)) * pdist
newlatlon = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
(state.lat, state.lon) = newlatlon
self.mouse_down = newpos
self.redraw_map()
def on_mouse(self, event):
'''handle mouse events'''
state = self.state
pos = event.GetPosition()
if event.Leaving():
self.mouse_pos = None
else:
self.mouse_pos = pos
self.update_position()
if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
# send any event with a mouse button to the parent
latlon = self.coordinates(pos.x, pos.y)
selected = self.selected_objects(pos)
state.event_queue.put(SlipMouseEvent(latlon, event, selected))
if event.LeftDown():
self.mouse_down = pos
self.last_click_pos = self.click_pos
self.click_pos = self.coordinates(pos.x, pos.y)
if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
# drag map to new position
newpos = pos
dx = (self.mouse_down.x - newpos.x)
dy = -(self.mouse_down.y - newpos.y)
pdist = math.sqrt(dx**2 + dy**2)
if pdist > state.drag_step:
bearing = math.degrees(math.atan2(dx, dy))
distance = (state.ground_width / float(state.width)) * pdist
newlatlon = mp_util.gps_newpos(state.lat, state.lon, bearing,
distance)
(state.lat, state.lon) = newlatlon
self.mouse_down = newpos
self.redraw_map()
def update_target(self, time_boot_ms):
'''update target on map'''
if not self.mpstate.map:
# don't draw if no map
return
if not 'HOME_POSITION' in self.master.messages:
return
home_position = self.master.messages['HOME_POSITION']
now = time_boot_ms * 1.0e-3
dt = now - self.last_update
if dt < 0:
dt = 0
self.last_update = now
self.circle_dist += dt * self.follow_settings.speed
# assume a circle for now
circumference = math.pi * self.follow_settings.radius * 2
rotations = math.fmod(self.circle_dist, circumference) / circumference
angle = math.pi * 2 * rotations
self.target_pos = mp_util.gps_newpos(home_position.latitude * 1.0e-7,
home_position.longitude * 1.0e-7,
math.degrees(angle),
self.follow_settings.radius)
icon = self.mpstate.map.icon('camera-small-red.png')
(lat, lon) = (self.target_pos[0], self.target_pos[1])
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('followtest', (lat, lon),
icon,
layer='FollowTest',
rotation=0,
follow=False))
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 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 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()
def mavlink_packet_distance_sensor(self, vehicle, m):
heading = self.heading_for_vehicle.get(vehicle, 0)
tlayer = "Distance sensor for %u.%u id=%u" % (
m.get_srcSystem(), m.get_srcComponent(), m.id)
slipkey = '%s-POS%u' % (tlayer, m.orientation)
if not m.get_srcSystem() in self.lat_lon_for_vehicle:
return
if m.current_distance == m.max_distance:
self.foreach_map(lambda a_map: a_map.remove_object(slipkey))
return
(lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()]
mav_sensor_rotation_to_degrees = {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 0,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 45,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 90,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 135,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 180,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 225,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 270,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 315,
}
if m.orientation in mav_sensor_rotation_to_degrees:
degrees = mav_sensor_rotation_to_degrees[m.orientation]
else:
# print("bad orientation (%u)" % m.orientation)
return
if m.current_distance >= m.max_distance:
return
p = mp_util.gps_newpos(lat, lon, heading + degrees, 0)
# start angle/end angle are either side of the primary axis,
# which is rotated to be North
start_angle = -22.5
end_angle = 22.5
self.foreach_map(lambda a_map: a_map.add_object(
mp_slipmap.SlipCircle(
slipkey,
3,
p,
m.current_distance / 100.0,
(255, 127, 0),
linewidth=3,
start_angle=start_angle,
end_angle=end_angle,
rotation=(-90 + (heading + degrees)) % 360,
)))
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))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
state = self
mtype = m.get_type()
if mtype == 'GLOBAL_POSITION_INT':
state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
state.hdg = m.hdg*scale_hdg
agl = state.elevation_model.GetElevation(state.lat, state.lon)
if agl is not None:
state.height = m.relative_alt*scale_relative_alt + state.home_height - agl
elif mtype == 'ATTITUDE':
state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
elif mtype in ['GPS_RAW', 'GPS_RAW_INT']:
if self.module('wp').wploader.count() > 0:
home = self.module('wp').wploader.wp(0).x, self.module('wp').wploader.wp(0).y
else:
home = [self.master.field('HOME', c)*scale_latlon for c in ['lat', 'lon']]
old = state.home_height # TODO TMP
agl = state.elevation_model.GetElevation(*home)
if agl is None:
return
state.home_height = agl
# TODO TMP
if state.home_height != old:
# tridge said to get home pos from wploader,
# but this is not the same as from master() below...!!
# using master() gives the right coordinates
# (i.e. matches GLOBAL_POSITION_INT coords, and $IMHOME in sim_arduplane.sh)
# and wploader is a bit off
print('home height changed from',old,'to',state.home_height)
elif mtype == 'SERVO_OUTPUT_RAW':
for (axis, attr) in [('ROLL', 'mount_roll'), ('TILT', 'mount_pitch'), ('PAN', 'mount_yaw')]:
channel = int(self.get_mav_param('MNT_RC_IN_{0}'.format(axis), 0))
if self.get_mav_param('MNT_STAB_{0}'.format(axis), 0) and channel:
# enabled stabilisation on this axis
# TODO just guessing that RC_IN_ROLL gives the servo number, but no idea if this is really the case
servo = 'servo{0}_raw'.format(channel)
centidegrees = self.scale_rc(getattr(m, servo),
self.get_mav_param('MNT_ANGMIN_{0}'.format(axis[:3])),
self.get_mav_param('MNT_ANGMAX_{0}'.format(axis[:3])),
param='RC{0}'.format(channel))
setattr(state, attr, centidegrees*0.01)
#state.mount_roll = min(max(-state.roll,-45),45)#TODO TMP
#state.mount_yaw = min(max(-state.yaw,-45),45)#TODO TMP
#state.mount_pitch = min(max(-state.pitch,-45),45)#TODO TMP
else:
return
if self.mpstate.map: # if the map module is loaded, redraw polygon
# get rid of the old polygon
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('CameraView'))
# camera view polygon determined by projecting corner pixels of the image onto the ground
pixel_positions = [cuav_util.pixel_position(px[0],px[1], state.height, state.pitch+state.mount_pitch, state.roll+state.mount_roll, state.yaw+state.mount_yaw, state.camera_params) for px in [(0,0), (state.camera_params.xresolution,0), (state.camera_params.xresolution,state.camera_params.yresolution), (0,state.camera_params.yresolution)]]
if any(pixel_position is None for pixel_position in pixel_positions):
# at least one of the pixels is not on the ground
# so it doesn't make sense to try to draw the polygon
return
gps_positions = [mp_util.gps_newpos(state.lat, state.lon, math.degrees(math.atan2(*pixel_position)), math.hypot(*pixel_position)) for pixel_position in pixel_positions]
# draw new polygon
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('cameraview', gps_positions+[gps_positions[0]], # append first element to close polygon
layer='CameraView', linewidth=2, colour=state.col))
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()
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()
def move(self, bearing, distance):
'''move position by bearing and distance'''
lat = self.pkt['I105']['Lat']['val']
lon = self.pkt['I105']['Lon']['val']
(lat, lon) = mp_util.gps_newpos(lat, lon, bearing, distance)
self.setpos(lat, lon)
def on_mouse(self, event):
'''handle mouse events'''
state = self.state
pos = event.GetPosition()
if event.Leaving():
self.mouse_pos = None
else:
self.mouse_pos = pos
self.update_position()
if hasattr(event, 'ButtonIsDown'):
any_button_down = event.ButtonIsDown(wx.MOUSE_BTN_ANY)
left_button_down = event.ButtonIsDown(wx.MOUSE_BTN_LEFT)
right_button_down = event.ButtonIsDown(wx.MOUSE_BTN_RIGHT)
else:
left_button_down = event.leftIsDown
right_button_down = event.rightIsDown
any_button_down = left_button_down or right_button_down
if any_button_down or event.ButtonUp():
# send any event with a mouse button to the parent
latlon = self.coordinates(pos.x, pos.y)
selected = self.selected_objects(pos)
state.event_queue.put(SlipMouseEvent(latlon, event, selected))
if event.RightDown():
state.popup_object = None
state.popup_latlon = None
if len(selected) > 0:
obj = state.layers[selected[0].layer][selected[0].objkey]
if obj.popup_menu is not None:
state.popup_object = obj
state.popup_latlon = latlon
self.show_popup(obj, pos)
state.popup_started = True
if not state.popup_started and state.default_popup is not None:
state.popup_latlon = latlon
self.show_default_popup(pos)
state.popup_started = True
if not right_button_down:
state.popup_started = False
if event.LeftDown() or event.RightDown():
self.mouse_down = pos
self.last_click_pos = self.click_pos
self.click_pos = self.coordinates(pos.x, pos.y)
if event.Dragging() and left_button_down:
# drag map to new position
newpos = pos
if self.mouse_down and newpos:
dx = (self.mouse_down.x - newpos.x)
dy = -(self.mouse_down.y - newpos.y)
pdist = math.sqrt(dx**2 + dy**2)
if pdist > state.drag_step:
bearing = math.degrees(math.atan2(dx, dy))
distance = (state.ground_width /
float(state.width)) * pdist
newlatlon = mp_util.gps_newpos(state.lat, state.lon,
bearing, distance)
(state.lat, state.lon) = newlatlon
self.mouse_down = newpos
self.redraw_map()
def area_to_tile_list(self,
lat,
lon,
width,
height,
ground_width,
zoom=None):
'''return a list of TileInfoScaled objects needed for
an area of land, with ground_width in meters, and
width/height in pixels.
lat/lon is the top left corner. If unspecified, the
zoom is automatically chosen to avoid having to grow
the tiles
'''
pixel_width = ground_width / float(width)
ground_height = ground_width * (height / (float(width)))
top_right = mp_util.gps_newpos(lat, lon, 90, ground_width)
bottom_left = mp_util.gps_newpos(lat, lon, 180, ground_height)
bottom_right = mp_util.gps_newpos(bottom_left[0], bottom_left[1], 90,
ground_width)
# choose a zoom level if not provided
if zoom is None:
zooms = range(self.min_zoom, self.max_zoom + 1)
else:
zooms = [zoom]
for zoom in zooms:
tile_min = self.coord_to_tile(lat, lon, zoom)
(twidth, theight) = tile_min.size()
tile_pixel_width = twidth / float(TILES_WIDTH)
scale = pixel_width / tile_pixel_width
if scale >= 1.0:
break
scaled_tile_width = int(TILES_WIDTH / scale)
scaled_tile_height = int(TILES_HEIGHT / scale)
# work out the bottom right tile
tile_max = self.coord_to_tile(bottom_right[0], bottom_right[1], zoom)
ofsx = int(tile_min.offsetx / scale)
ofsy = int(tile_min.offsety / scale)
srcy = ofsy
dsty = 0
ret = []
# place the tiles
for y in range(tile_min.y, tile_max.y + 1):
srcx = ofsx
dstx = 0
for x in range(tile_min.x, tile_max.x + 1):
if dstx < width and dsty < height:
ret.append(
TileInfoScaled((x, y), zoom, scale, (srcx, srcy),
(dstx, dsty), self.service))
dstx += scaled_tile_width - srcx
srcx = 0
dsty += scaled_tile_height - srcy
srcy = 0
return ret
max_zoom=opts.max_zoom,
elevation=opts.elevation,
tile_delay=opts.delay)
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
sm.add_object(SlipPolygon('boundary', boundary, layer=1, linewidth=2, colour=(0,255,0)))
if opts.thumbnail:
thumb = cv.LoadImage(opts.thumbnail)
sm.add_object(SlipThumbnail('thumb', (opts.lat,opts.lon), layer=1, img=thumb, border_width=2, border_colour=(255,0,0)))
if opts.icon:
icon = cv.LoadImage(opts.icon)
sm.add_object(SlipIcon('icon', (opts.lat,opts.lon), icon, layer=3, rotation=90, follow=True))
sm.set_position('icon', mp_util.gps_newpos(opts.lat,opts.lon, 180, 100), rotation=45)
sm.add_object(SlipInfoImage('detail', icon))
sm.add_object(SlipInfoText('detail text', 'test text'))
for flag in opts.flag:
(lat,lon) = flag.split(',')
icon = sm.icon('flag.png')
sm.add_object(SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
while sm.is_alive():
while sm.event_count() > 0:
obj = sm.get_event()
if isinstance(obj, SlipMouseEvent):
print("Mouse event at %s (X/Y=%u/%u) for %u objects" % (obj.latlon,
obj.event.X, obj.event.Y,
len(obj.selected)))
wp = mavwp.MAVWPLoader()
wp.load(file)
boundary = wp.polygon()
sm.add_object(SlipPolygon('mission-%s' % file, boundary, layer=1, linewidth=1, colour=(255,255,255)))
if opts.grid:
sm.add_object(SlipGrid('grid', layer=3, linewidth=1, colour=(255,255,0)))
if opts.thumbnail:
thumb = cv2.imread(opts.thumbnail)
sm.add_object(SlipThumbnail('thumb', (opts.lat,opts.lon), layer=1, img=thumb, border_width=2, border_colour=(255,0,0)))
if opts.icon:
icon = cv2.imread(opts.icon)
sm.add_object(SlipIcon('icon', (opts.lat,opts.lon), icon, layer=3, rotation=90, follow=True))
sm.set_position('icon', mp_util.gps_newpos(opts.lat,opts.lon, 180, 100), rotation=45)
sm.add_object(SlipInfoImage('detail', icon))
sm.add_object(SlipInfoText('detail text', 'test text'))
for flag in opts.flag:
(lat,lon) = flag.split(',')
icon = sm.icon('flag.png')
sm.add_object(SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
while sm.is_alive():
while not sm.event_queue_empty():
obj = sm.get_event()
if not opts.verbose:
continue
if isinstance(obj, SlipMouseEvent):
print("Mouse event at %s (X/Y=%u/%u) for %u objects" % (obj.latlon,
SlipThumbnail('thumb', (opts.lat, opts.lon),
layer=1,
img=thumb,
border_width=2,
border_colour=(255, 0, 0)))
if opts.icon:
icon = cv.LoadImage(opts.icon)
sm.add_object(
SlipIcon('icon', (opts.lat, opts.lon),
icon,
layer=3,
rotation=90,
follow=True))
sm.set_position('icon',
mp_util.gps_newpos(opts.lat, opts.lon, 180, 100),
rotation=45)
sm.add_object(SlipInfoImage('detail', icon))
sm.add_object(SlipInfoText('detail text', 'test text'))
for flag in opts.flag:
(lat, lon) = flag.split(',')
icon = sm.icon('flag.png')
sm.add_object(
SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
while sm.is_alive():
