def geofence_draw_callback(self, points):
'''callback from drawing waypoints'''
if len(points) != self.Geofence0["numV"]:
print("Insufficient points in polygon, try drawing polygon again")
print("(Expected %d points, Received %d points)" %
(self.Geofence0["numV"], len(points)))
return
for pts in points:
self.Geofence0['Vertices'].append(pts)
# Adjust fence id to not leave empty spaces
if self.Geofence0['id'] > len(self.fenceList) + len(
self.drawnFenceList):
self.Geofence0['id'] = len(self.fenceList) + len(
self.drawnFenceList)
name = "DraftFence" + str(self.Geofence0['id'])
# Check that the geofence polygon is nice
add_geofence = True
if geofence_check_niceness:
points = self.fence_to_polygon(self.Geofence0["Vertices"])
# Reverse points if not counterclockwise
if not counterclockwise_edges(points):
points.reverse()
self.Geofence0["Vertices"].reverse()
add_geofence = nice_polygon_2D(points, 0.01)
# Add the geofence to the draft list if it is a nice polygon
if add_geofence:
if self.Geofence0['id'] not in self.drawnFenceIds:
self.drawnFenceList.append(self.Geofence0)
else:
self.drawnFenceList[self.drawnFenceIds.index(
self.Geofence0['id'])] = self.Geofence0
self.mpstate.map.remove_object(name)
print("Geofence %d added (draft, confirm with 'geofence upload')" %
(self.Geofence0['id']))
else:
print(
"Geofence not added - Polygon does not meet niceness criteria (must be counterclockwise)"
)
return
# Draw the draft geofence (but do not upload it yet)
points = self.Geofence0['Vertices'][:]
points.append(points[0])
if self.Geofence0['type'] == 0:
gcf = (255, 255, 150)
else:
gcf = (255, 200, 200)
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon(name,
points,
layer=2,
linewidth=2,
colour=gcf))
def display_waypoints(wploader, map):
'''display the waypoints'''
mission_list = wploader.view_list()
polygons = wploader.polygon_list()
map.add_object(mp_slipmap.SlipClearLayer('Mission'))
for k in range(len(polygons)):
p = polygons[k]
if len(p) > 1:
map.add_object(
mp_slipmap.SlipPolygon('mission %u' % k,
p,
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
labeled_wps = {}
for i in range(len(mission_list)):
next_list = mission_list[i]
for j in range(len(next_list)):
#label already printed for this wp?
if (next_list[j] not in labeled_wps):
map.add_object(
mp_slipmap.SlipLabel('miss_cmd %u/%u' % (i, j),
polygons[i][j],
str(next_list[j]),
'Mission',
colour=(0, 255, 255)))
labeled_wps[next_list[j]] = (i, j)
def loadkml(self, filename):
'''Load a kml from file and put it on the map'''
#Open the zip file
nodes = self.readkmz(filename)
#go through each object in the kml...
for n in nodes:
point = self.readObject(n)
#and place any polygons on the map
if self.mpstate.map is not None and point[0] == 'Polygon':
#print "Adding " + point[1]
newcolour = (random.randint(0, 255), 0, random.randint(0, 255))
curpoly = mp_slipmap.SlipPolygon(point[1], point[2],
layer=2, linewidth=2, colour=newcolour)
self.mpstate.map.add_object(curpoly)
self.allayers.append(curpoly)
self.curlayers.append(point[1])
#and points - barrell image and text
if self.mpstate.map is not None and point[0] == 'Point':
#print "Adding " + point[1]
icon = self.mpstate.map.icon('barrell.png')
curpoint = mp_slipmap.SlipIcon(point[1], latlon = (point[2][0][0], point[2][0][1]), layer=3, img=icon, rotation=0, follow=False)
curtext = mp_slipmap.SlipLabel(point[1], point = (point[2][0][0], point[2][0][1]), layer=4, label=point[1], colour=(0,255,255))
self.mpstate.map.add_object(curpoint)
self.mpstate.map.add_object(curtext)
self.allayers.append(curpoint)
self.alltextlayers.append(curtext)
self.curlayers.append(point[1])
self.curtextlayers.append(point[1])
self.menu_needs_refreshing = True
def display_waypoints(self):
'''display the waypoints'''
self.mission_list = self.module('wp').wploader.view_list()
polygons = self.module('wp').wploader.polygon_list()
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission'))
for i in range(len(polygons)):
p = polygons[i]
if len(p) > 1:
popup = MPMenuSubMenu('Popup',
items=[MPMenuItem('Set', returnkey='popupMissionSet'),
MPMenuItem('WP Remove', returnkey='popupMissionRemove'),
MPMenuItem('WP Move', returnkey='popupMissionMove')])
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p,
layer='Mission', linewidth=2, colour=(255,255,255),
popup_menu=popup))
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
labeled_wps = {}
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('LoiterCircles'))
for i in range(len(self.mission_list)):
next_list = self.mission_list[i]
for j in range(len(next_list)):
#label already printed for this wp?
if (next_list[j] not in labeled_wps):
self.mpstate.map.add_object(mp_slipmap.SlipLabel(
'miss_cmd %u/%u' % (i,j), polygons[i][j], str(next_list[j]), 'Mission', colour=(0,255,255)))
if (self.map_settings.loitercircle and
self.module('wp').wploader.wp_is_loiter(next_list[j])):
self.mpstate.map.add_object(mp_slipmap.SlipCircle('Loiter Circle %u' % (next_list[j] + 1), 'LoiterCircles', polygons[i][j], abs(loiter_rad), (255, 255, 255), 2))
labeled_wps[next_list[j]] = (i,j)
def drawing_update(self):
'''update line drawing'''
if self.draw_callback is None:
return
self.draw_line.append(self.click_position)
if len(self.draw_line) > 1:
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('drawing', self.draw_line,
layer='Drawing', linewidth=2, colour=(128,128,255)))
def drawing_update(self):
'''update line drawing'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
if self.draw_callback is None:
return
self.draw_line.append(self.click_position)
if len(self.draw_line) > 1:
self.map.add_object(mp_slipmap.SlipPolygon('drawing', self.draw_line,
layer='Drawing', linewidth=2, colour=(128,128,255)))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
state = self
if m.get_type() == 'GLOBAL_POSITION_INT':
state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
state.hdg = m.hdg*scale_hdg
state.height = m.relative_alt*scale_relative_alt + state.home_height - state.elevation_model.GetElevation(state.lat, state.lon)
elif m.get_type() == 'ATTITUDE':
state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
elif m.get_type() 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
state.home_height = state.elevation_model.GetElevation(*home)
# 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 m.get_type() == '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 display_waypoints():
'''display the waypoints'''
polygons = mpstate.wp_state.wploader.polygon_list()
mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission'))
for i in range(len(polygons)):
p = polygons[i]
if len(p) > 1:
mpstate.map.add_object(mp_slipmap.SlipPolygon('mission%u' % i, p,
layer='Mission', linewidth=2, colour=(255,255,255)))
def set_boundary(self, boundary):
'''set a polygon search boundary'''
if not cuav_util.polygon_complete(boundary):
raise RuntimeError('invalid boundary passed to mosaic')
self.boundary = boundary[:]
self.slipmap.add_object(
mp_slipmap.SlipPolygon('boundary',
self.boundary,
layer=1,
linewidth=2,
colour=(0, 0, 255)))
def display_fence(self):
'''display the fence'''
self.fence_change_time = self.module('fence').fenceloader.last_change
points = self.module('fence').fenceloader.polygon()
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Fence'))
if len(points) > 1:
popup = MPMenuSubMenu('Popup',
items=[MPMenuItem('FencePoint Remove', returnkey='popupFenceRemove'),
MPMenuItem('FencePoint Move', returnkey='popupFenceMove')])
self.mpstate.map.add_object(mp_slipmap.SlipPolygon('Fence', points, layer=1,
linewidth=2, colour=(0,255,0), popup_menu=popup))
def Send_fence(self,fence):
'''send fence points from fenceloader'''
target_system = 2;
target_component = 0;
self.master.mav.command_long_send(target_system,target_component,
mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE,0,
0,fence["id"],fence["type"],fence["numV"],
fence["floor"],fence["roof"],0);
fence_sent = False;
while(not fence_sent):
msg = None
while(msg == None):
msg = self.master.recv_msg();
if(msg.get_type() == "FENCE_FETCH_POINT"):
numV = fence["numV"]
lat = fence["Vertices"][msg.idx][0]
lon = fence["Vertices"][msg.idx][1]
self.master.mav.fence_point_send(2,0,msg.idx,numV,lat,lon)
elif(msg.get_type() == "COMMAND_ACK" ):
if msg.result == 1:
fence_sent = True;
print ("Geofence sent")
else:
self.Send_fence(fence);
fence_sent = True;
points = fence["Vertices"][:]
points.append(points[0])
from MAVProxy.modules.mavproxy_map import mp_slipmap
name = 'Fence'+str(fence["id"])
if(fence["type"] == 0):
gcf = (0,255,0)
else:
gcf = (255,0,0)
self.mpstate.map.add_object(mp_slipmap.SlipPolygon(name, points, layer=1,
linewidth=2, colour=gcf))
return
def display_waypoints(self):
'''display the waypoints'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
self.mission_list = self.module('wp').wploader.view_list()
polygons = self.module('wp').wploader.polygon_list()
self.map.add_object(mp_slipmap.SlipClearLayer('Mission'))
for i in range(len(polygons)):
p = polygons[i]
if len(p) > 1:
items = [MPMenuItem('Set', returnkey='popupMissionSet'),
MPMenuItem('WP Remove', returnkey='popupMissionRemove'),
MPMenuItem('WP Move', returnkey='popupMissionMove'),
MPMenuItem('Remove NoFly', returnkey='popupMissionRemoveNoFly'),
]
popup = MPMenuSubMenu('Popup', items)
self.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p,
layer='Mission', linewidth=2, colour=(255,255,255),
arrow = self.map_settings.showdirection, popup_menu=popup))
labeled_wps = {}
self.map.add_object(mp_slipmap.SlipClearLayer('LoiterCircles'))
if not self.map_settings.showwpnum:
return
for i in range(len(self.mission_list)):
next_list = self.mission_list[i]
for j in range(len(next_list)):
#label already printed for this wp?
if (next_list[j] not in labeled_wps):
label = self.label_for_waypoint(next_list[j])
colour = self.colour_for_wp(next_list[j])
self.map.add_object(mp_slipmap.SlipLabel(
'miss_cmd %u/%u' % (i,j), polygons[i][j], label, 'Mission', colour=colour))
if (self.map_settings.loitercircle and
self.module('wp').wploader.wp_is_loiter(next_list[j])):
wp = self.module('wp').wploader.wp(next_list[j])
if wp.command != mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param3 != 0:
# wp radius and direction is defined by the mission
loiter_rad = wp.param3
elif wp.command == mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT and wp.param2 != 0:
# wp radius and direction is defined by the mission
loiter_rad = wp.param2
else:
# wp radius and direction is defined by the parameter
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
self.map.add_object(mp_slipmap.SlipCircle('Loiter Circle %u' % (next_list[j] + 1), 'LoiterCircles', polygons[i][j],
loiter_rad, (255, 255, 255), 2, arrow = self.map_settings.showdirection))
labeled_wps[next_list[j]] = (i,j)
def geofence_draw_callback(self, points):
'''callback from drawing waypoints'''
if len(points) != self.Geofence0["numV"]:
print("Insufficient points in polygon, try drawing polygon again")
print("(Expected %d points, Received %d points)" %
(self.Geofence0["numV"], len(points)))
return
from MAVProxy.modules.lib import mp_util
from MAVProxy.modules.mavproxy_map import mp_slipmap
for pts in points:
self.Geofence0['Vertices'].append(pts)
# Adjust fence id to not leave empty spaces
if self.Geofence0['id'] > len(self.fenceList) - 1:
self.Geofence0['id'] = len(self.fenceList)
name = "DraftFence" + str(self.Geofence0['id'])
# Check that the geofence polygon is nice
if self.nice_geofence(self.Geofence0):
if self.Geofence0['id'] not in self.drawnFenceIds:
self.drawnFenceList.append(self.Geofence0)
else:
self.drawnFenceList[self.drawnFenceIds.index(
self.Geofence0['id'])] = self.Geofence0
self.mpstate.map.remove_object(name)
print("Geofence %d added (draft)" % (self.Geofence0['id']))
else:
print(
"Geofence not added - Polygon does not meet niceness criteria (must be counterclockwise)"
)
return
# Draw the draft geofence (but do not upload it yet)
points = self.Geofence0['Vertices'][:]
points.append(points[0])
if self.Geofence0['type'] == 0:
gcf = (255, 255, 150)
else:
gcf = (255, 200, 200)
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon(name,
points,
layer=2,
linewidth=2,
colour=gcf))
def cmd_camera(self, args):
'''camera commands'''
usage = "usage: camera <status|view|boundary|set>"
if len(args) == 0:
print(usage)
return
elif args[0] == "status":
#print("Cap imgs: regions:%u" % (self.region_count))
#request status update from air module
pkt = cuav_command.CommandPacket('status')
self.send_packet(pkt)
pkt = cuav_command.CommandPacket('queue')
self.send_packet(pkt)
elif args[0] == "view":
#check cam params
if not self.check_camera_parms():
print("Error - incorrect camera params")
return
if self.mpstate.map is None:
print("Error - load map module first")
return
if not self.viewing:
print("Starting image viewer")
self.joelog = cuav_joe.JoeLog(os.path.join(self.camera_dir,
'joe_ground.log'),
append=self.continue_mode)
if self.view_thread is None:
self.view_thread = self.start_thread(self.view_threadfunc)
self.viewing = True
elif args[0] == "set":
self.camera_settings.command(args[1:])
elif args[0] == "boundary":
if len(args) != 2:
print("boundary=%s" % self.boundary)
else:
self.boundary = args[1]
self.boundary_polygon = cuav_util.polygon_load(self.boundary)
if self.mpstate.map is not None:
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('boundary',
self.boundary_polygon,
layer=1,
linewidth=2,
colour=(0, 0, 255)))
def load_kml(kml):
'''load a kml overlay, return list of map objects'''
print("Loading kml %s" % kml)
nodes = kmlread.readkmz(kml)
ret = []
for n in nodes:
try:
point = kmlread.readObject(n)
except Exception as ex:
continue
if point[0] == 'Polygon':
newcolour = (random.randint(0, 255), 0, random.randint(0, 255))
curpoly = mp_slipmap.SlipPolygon(point[1],
point[2],
layer=2,
linewidth=2,
colour=newcolour)
ret.append(curpoly)
if point[0] == 'Point':
icon = mp_tile.mp_icon('barrell.png')
curpoint = mp_slipmap.SlipIcon(point[1],
latlon=(point[2][0][0],
point[2][0][1]),
layer=3,
img=icon,
rotation=0,
follow=False)
curtext = mp_slipmap.SlipLabel(point[1],
point=(point[2][0][0],
point[2][0][1]),
layer=4,
label=point[1],
colour=(0, 255, 255))
ret.append(curpoint)
ret.append(curtext)
return ret
def display_waypoints(self):
'''display the waypoints'''
self.mission_list = self.module('wp').wploader.view_list()
polygons = self.module('wp').wploader.polygon_list()
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission'))
for i in range(len(polygons)):
p = polygons[i]
if len(p) > 1:
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Set', returnkey='popupMissionSet'),
MPMenuItem('WP Remove',
returnkey='popupMissionRemove'),
MPMenuItem('WP Move', returnkey='popupMissionMove')
])
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('mission %u' % i,
p,
layer='Mission',
linewidth=2,
colour=(255, 255, 255),
popup_menu=popup))
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 * 1.0e-7, m.lng * 1.0e-7),
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 MAVProxy.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 mavflightview_show(path, wp, fen, used_flightmodes, mav_type, options, instances, title=None, timelim_pipe=None):
if not title:
title='MAVFlightView'
boundary_path = []
for p in path[0]:
boundary_path.append((p[0],p[1]))
fence = fen.polygon()
if options.fencebounds:
for p in fence:
boundary_path.append((p[0],p[1]))
bounds = mp_util.polygon_bounds(boundary_path)
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
kml = getattr(options,'kml',None)
if kml is not None:
kml_objects = load_kml(kml)
else:
kml_objects = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj, kml_objects, used_flightmodes=used_flightmodes, mav_type=mav_type)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=options.debug,
show_flightmode_legend=options.show_flightmode_legend,
timelim_pipe=timelim_pipe)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
if kml_objects is not None:
for obj in kml_objects:
map.add_object(obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
if options.colour_source == "flightmode":
tuples = [ (mode, colour_for_flightmode(mav_type, mode))
for mode in used_flightmodes.keys() ]
map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
elif options.colour_source == "type":
tuples = [ (t, map_colours[instances[t]]) for t in instances.keys() ]
map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
else:
print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
overlap=args.overlap,
offset=args.searchAreaOffset,
wobble=args.wobble,
alt=args.altitude)
gen.ExportSearchPattern()
#start a map
sm = mp_slipmap.MPSlipMap(
lat=gen.getMapPolygon(
loiterInSearchArea=args.loiterInSearchArea)[0][0],
lon=gen.getMapPolygon(
loiterInSearchArea=args.loiterInSearchArea)[0][1],
elevation=True,
service='GoogleSat')
sm.add_object(
mp_slipmap.SlipPolygon('Search Pattern',
gen.getMapPolygon(),
layer=1,
linewidth=2,
colour=(0, 255, 0)))
#get the search pattern distance
print "Total Distance = " + str(gen.getPolygonLength()) + "m"
#and export to MAVProxy
gen.exportToMAVProxy(args.loiterInSearchArea, args.basealt, args.outname)
#and to google earth
gen.ExportSearchPattern()
def mavlink_packet(self, m):
'''handle and incoming mavlink packet'''
if self.startSendingFence:
self.t2 = time.time()
if self.t2 - self.t1 > 10:
self.startSendingFence = False
self.communicating = False
self.fence2send = None
self.console.writeln("Sending failed")
self.numSentFence = 0
return
if self.numSentFence == len(self.fenceList):
self.startSendingFence = False
self.communicating = False
self.fence2send = None
elif self.fence2send is None:
self.commmunicating = True
self.fence2send = self.fenceList[self.numSentFence]
target_system = 2
target_component = 0
fence = self.fence2send
self.console.writeln("sending fence description")
self.master.mav.command_long_send(
target_system, target_component,
mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, 0, 0, fence["id"],
fence["type"], fence["numV"], fence["floor"],
fence["roof"], 0)
if m.get_type() == "FENCE_FETCH_POINT":
self.t1 = time.time()
fence = self.fence2send
numV = fence["numV"]
lat = fence["Vertices"][m.idx][0]
lon = fence["Vertices"][m.idx][1]
self.master.mav.fence_point_send(2, 0, m.idx, numV, lat, lon)
self.console.writeln("sending vertex %u" % m.idx)
self.sentVertexCount = m.idx + 1
if m.get_type() == "COMMAND_ACK":
self.t1 = time.time()
if self.sentVertexCount == self.fence2send[
"numV"] and m.result == 0:
self.numSentFence = self.numSentFence + 1
self.console.writeln("Geofence sent")
points = self.fence2send["Vertices"][:]
points.append(points[0])
from MAVProxy.modules.mavproxy_map import mp_slipmap
name = 'Fence' + str(self.fence2send["id"])
if (self.fence2send["type"] == 0):
gcf = (255, 190, 0)
else:
gcf = (255, 0, 0)
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon(name,
points,
layer=1,
linewidth=2,
colour=gcf))
self.fence2send = None
def cmd_camera(args):
'''camera commands'''
state = mpstate.camera_state
if args[0] == "start":
state.capture_count = 0
state.error_count = 0
state.error_msg = None
state.running = True
if state.capture_thread is None:
state.capture_thread = start_thread(capture_thread)
state.save_thread = start_thread(save_thread)
state.scan_thread1 = start_thread(scan_thread)
state.scan_thread2 = start_thread(scan_thread)
state.transmit_thread = start_thread(transmit_thread)
print("started camera running")
elif args[0] == "stop":
state.running = False
print("stopped camera capture")
elif args[0] == "status":
print("Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%u/%u lst:%u sq:%.1f eff:%.2f" % (
state.capture_count, state.error_count, state.scan_count, state.region_count,
state.jpeg_size,
state.xmit_queue, state.xmit_queue2, state.frame_loss, state.scan_queue.qsize(), state.efficiency))
if state.bsend2:
state.bsend2.report(detailed=False)
elif args[0] == "queue":
print("scan %u save %u transmit %u eff %.1f bw %.1f rtt %.1f" % (
state.scan_queue.qsize(),
state.save_queue.qsize(),
state.transmit_queue.qsize(),
state.efficiency,
state.bandwidth_used,
state.rtt_estimate))
elif args[0] == "view":
if mpstate.map is None:
print("Please load map module first")
return
if not state.viewing:
print("Starting image viewer")
if state.view_thread is None:
state.view_thread = start_thread(view_thread)
state.viewing = True
elif args[0] == "noview":
if state.viewing:
print("Stopping image viewer")
state.viewing = False
elif args[0] == "set":
if len(args) < 3:
state.settings.show_all()
else:
state.settings.set(args[1], args[2])
elif args[0] == "boundary":
if len(args) != 2:
print("boundary=%s" % state.boundary)
else:
state.boundary = args[1]
state.boundary_polygon = cuav_util.polygon_load(state.boundary)
if mpstate.map is not None:
mpstate.map.add_object(mp_slipmap.SlipPolygon('boundary', state.boundary_polygon, layer=1, linewidth=2, colour=(0,0,255)))
else:
print("usage: camera <start|stop|status|view|noview|boundary|set>")
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = []
if os.path.isdir(args.directory):
files.extend(file_list(args.directory, ['jpg', 'pgm', 'png']))
else:
if args.directory.find('*') != -1:
files.extend(glob.glob(args.directory))
else:
files.append(args.directory)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=args.service,
elevation=True,
title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(
mp_slipmap.SlipIcon('plane', (0, 0),
icon,
layer=3,
rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
for flag in args.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = slipmap.icon(icon)
slipmap.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
if args.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(args.mission.name)
plist = wp.polygon_list()
if len(plist) > 0:
for i in range(len(plist)):
slipmap.add_object(
mp_slipmap.SlipPolygon('Mission-%s-%u' %
(args.mission.name, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
if args.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(args.mavlog.name)
else:
mpos = None
if args.gammalog is not None:
gamma = parse_gamma_log(args.gammalog)
else:
gamma = None
if args.kmzlog:
kmzpos = mav_position.KmlPosition(args.kmzlog.name)
else:
kmzpos = None
if args.triggerlog:
triggerpos = mav_position.TriggerPosition(args.triggerlog.name)
else:
triggerpos = None
# create a simple lens model using the focal length
if args.camera_params:
C_params = cam_params.CameraParams.fromfile(args.camera_params.name)
else:
C_params = cam_params.CameraParams(lens=args.lens,
sensorwidth=args.sensorwidth,
xresolution=args.xresolution,
yresolution=args.yresolution)
if args.target:
target = args.target.split(',')
else:
target = [0, 0, 0]
camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, args.roll_stabilised,
'Roll Stabilised'),
MPSetting('altitude',
int,
args.altitude,
'Altitude',
range=(0, 10000),
increment=1),
MPSetting(
'minalt', int, 30, 'MinAltitude', range=(0, 10000), increment=1),
MPSetting('mpp100',
float,
0.0977,
'MPPat100m',
range=(0, 10000),
increment=0.001),
MPSetting('rotate180', bool, args.rotate_180, 'rotate180'),
MPSetting('filter_type',
str,
'compactness',
'Filter Type',
choice=['simple', 'compactness']),
MPSetting('target_lattitude',
float,
float(target[0]),
'target latitude',
increment=1.0e-7),
MPSetting('target_longitude',
float,
float(target[1]),
'target longitude',
increment=1.0e-7),
MPSetting('target_radius',
float,
float(target[2]),
'target radius',
increment=1),
MPSetting('quality',
int,
75,
'Compression Quality',
range=(1, 100),
increment=1),
MPSetting('thumbsize',
int,
args.thumbsize,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
args.minscore,
'Min Score',
range=(0, 1000),
increment=1,
tab='Scoring'),
MPSetting('brightness',
float,
1.0,
'Display Brightness',
range=(0.1, 10),
increment=0.1,
digits=2,
tab='Display'),
],
title='Camera Settings')
image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.05,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
4.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MinRegionSize',
float,
0.02,
range=(0, 100),
increment=0.05,
digits=2),
MPSetting('MaxRegionSize',
float,
3.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2),
MPSetting('RegionMergeSize',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('BlueEmphasis', bool, args.blue_emphasis),
MPSetting('SaveIntermediate', bool, args.debug)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap,
C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True,
classify=args.categories,
thumb_size=args.mosaic_thumbsize)
joelog = cuav_joe.JoeLog(None)
if args.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
start_time = time.time()
for f in files:
if not mosaic.started():
print("Waiting for startup")
if args.start:
mosaic.has_started = True
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
if gamma is not None:
frame_time = parse_gamma_time(f, gamma)
else:
frame_time = cuav_util.parse_frame_time(f)
frame_time += args.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += args.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f, rotate180=camera_settings.rotate180)
if im_orig is None:
continue
(w, h) = cuav_util.image_shape(im_orig)
if False:
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = im_orig
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0 = time.time()
img_scan = im_full
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
scan_parms['BlueEmphasis'] = float(scan_parms['BlueEmphasis'])
if pos is not None:
(sw, sh) = cuav_util.image_shape(img_scan)
altitude = pos.altitude
if altitude < camera_settings.minalt:
altitude = camera_settings.minalt
scan_parms[
'MetersPerPixel'] = camera_settings.mpp100 * altitude / 100.0
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(im_full))
count += 1
t1 = time.time()
frame_time = pos.time
if pos:
for r in regions:
r.latlon = cuav_util.gps_position_from_image_region(
r, pos, w, h, altitude=altitude, C=C_params)
if camera_settings.target_radius > 0 and pos is not None:
regions = cuav_region.filter_radius(
regions, (camera_settings.target_lattitude,
camera_settings.target_longitude),
camera_settings.target_radius)
regions = cuav_region.filter_regions(
im_full,
regions,
frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time,
regions,
pos,
f,
width=w,
height=h,
altitude=altitude)
mosaic.add_image(pos.time, f, pos)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if args.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255, 0, 0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title('Image: ' + os.path.basename(f))
if args.saveview:
cv.CvtColor(mat, mat, cv.CV_RGB2BGR)
cv.SaveImage('view-' + os.path.basename(f), mat)
total_time += (t1 - t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' %
(os.path.basename(f), count / total_time, region_count,
scan_count, num_files))
#raw_input("hit ENTER when ready")
print("All images processed (%u seconds)" % (time.time() - start_time))
while True:
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
time.sleep(0.2)
def mavflightview_mav(mlog, options=None, title=None):
'''create a map for a log file'''
if not title:
title = 'MAVFlightView'
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM', 'CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW', 'GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower(
) != options.mode.lower():
continue
if type in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
colour = colour_for_point(mlog, (lat, lng), instance, options)
point = (lat, lng, colour)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[
type] > 1.0 / options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, title),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (title, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat, lon), ground_width,
path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" %
(colour_source_min, colour_source_max))
def process(args):
'''process a set of files'''
global slipmap, mosaic
scan_count = 0
files = []
for a in args:
if os.path.isdir(a):
files.extend(file_list(a, ['jpg', 'pgm', 'png']))
else:
if a.find('*') != -1:
files.extend(glob.glob(a))
else:
files.append(a)
files.sort()
num_files = len(files)
print("num_files=%u" % num_files)
region_count = 0
slipmap = mp_slipmap.MPSlipMap(service=opts.service,
elevation=True,
title='Map')
icon = slipmap.icon('redplane.png')
slipmap.add_object(
mp_slipmap.SlipIcon('plane', (0, 0),
icon,
layer=3,
rotation=0,
follow=True,
trail=mp_slipmap.SlipTrail()))
if opts.mission:
from pymavlink import mavwp
wp = mavwp.MAVWPLoader()
wp.load(opts.mission)
boundary = wp.polygon()
slipmap.add_object(
mp_slipmap.SlipPolygon('mission',
boundary,
layer=1,
linewidth=1,
colour=(255, 255, 255)))
if opts.mavlog:
mpos = mav_position.MavInterpolator()
mpos.set_logfile(opts.mavlog)
else:
mpos = None
if opts.kmzlog:
kmzpos = mav_position.KmlPosition(opts.kmzlog)
else:
kmzpos = None
if opts.triggerlog:
triggerpos = mav_position.TriggerPosition(opts.triggerlog)
else:
triggerpos = None
# create a simple lens model using the focal length
C_params = cam_params.CameraParams(lens=opts.lens,
sensorwidth=opts.sensorwidth)
if opts.camera_params:
C_params.load(opts.camera_params)
camera_settings = MPSettings([
MPSetting('roll_stabilised', bool, True, 'Roll Stabilised'),
MPSetting(
'altitude', int, 0, 'Altitude', range=(0, 10000), increment=1),
MPSetting('filter_type',
str,
'simple',
'Filter Type',
choice=['simple', 'compactness']),
MPSetting('fullres', bool, False, 'Full Resolution'),
MPSetting('quality',
int,
75,
'Compression Quality',
range=(1, 100),
increment=1),
MPSetting('thumbsize',
int,
60,
'Thumbnail Size',
range=(10, 200),
increment=1),
MPSetting('minscore',
int,
75,
'Min Score',
range=(0, 1000),
increment=1,
tab='Scoring'),
MPSetting('brightness',
float,
1.0,
'Display Brightness',
range=(0.1, 10),
increment=0.1,
digits=2,
tab='Display')
],
title='Camera Settings')
image_settings = MPSettings([
MPSetting('MinRegionArea',
float,
0.15,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
2.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MinRegionSize',
float,
0.1,
range=(0, 100),
increment=0.05,
digits=2),
MPSetting(
'MaxRegionSize', float, 2, range=(0, 100), increment=0.1,
digits=1),
MPSetting('MaxRarityPct',
float,
0.02,
range=(0, 100),
increment=0.01,
digits=2),
MPSetting('RegionMergeSize',
float,
3.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('SaveIntermediate', bool, False)
],
title='Image Settings')
mosaic = cuav_mosaic.Mosaic(slipmap,
C=C_params,
camera_settings=camera_settings,
image_settings=image_settings,
start_menu=True)
joelog = cuav_joe.JoeLog(None)
if opts.view:
viewer = mp_image.MPImage(title='Image', can_zoom=True, can_drag=True)
if camera_settings.filter_type == 'compactness':
calculate_compactness = True
print("Using compactness filter")
else:
calculate_compactness = False
for f in files:
if not mosaic.started():
print("Waiting for startup")
while not mosaic.started():
mosaic.check_events()
time.sleep(0.01)
if mpos:
# get the position by interpolating telemetry data from the MAVLink log file
# this assumes that the filename contains the timestamp
frame_time = cuav_util.parse_frame_time(f) + opts.time_offset
if camera_settings.roll_stabilised:
roll = 0
else:
roll = None
try:
pos = mpos.position(frame_time, roll=roll)
except Exception:
print("No position available for %s" % frame_time)
# skip this frame
continue
elif kmzpos is not None:
pos = kmzpos.position(f)
elif triggerpos is not None:
pos = triggerpos.position(f)
else:
# get the position using EXIF data
pos = mav_position.exif_position(f)
pos.time += opts.time_offset
# update the plane icon on the map
if pos is not None:
slipmap.set_position('plane', (pos.lat, pos.lon), rotation=pos.yaw)
if camera_settings.altitude > 0:
pos.altitude = camera_settings.altitude
# check for any events from the map
slipmap.check_events()
mosaic.check_events()
im_orig = cuav_util.LoadImage(f)
(w, h) = cuav_util.image_shape(im_orig)
if not opts.camera_params:
C_params.set_resolution(w, h)
im_full = im_orig
im_640 = cv.CreateImage((640, 480), 8, 3)
cv.Resize(im_full, im_640, cv.CV_INTER_NN)
im_640 = numpy.ascontiguousarray(cv.GetMat(im_640))
im_full = numpy.ascontiguousarray(cv.GetMat(im_full))
count = 0
total_time = 0
t0 = time.time()
if camera_settings.fullres:
img_scan = im_full
else:
img_scan = im_640
scan_parms = {}
for name in image_settings.list():
scan_parms[name] = image_settings.get(name)
scan_parms['SaveIntermediate'] = float(scan_parms['SaveIntermediate'])
if pos is not None:
(sw, sh) = cuav_util.image_shape(img_scan)
mpp = cuav_util.meters_per_pixel(pos, C=C_params)
if mpp is not None:
scan_parms['MetersPerPixel'] = mpp * (w / float(sw))
regions = scanner.scan(img_scan, scan_parms)
else:
regions = scanner.scan(img_scan)
regions = cuav_region.RegionsConvert(regions,
cuav_util.image_shape(img_scan),
cuav_util.image_shape(im_full),
calculate_compactness)
count += 1
t1 = time.time()
frame_time = pos.time
regions = cuav_region.filter_regions(
im_full,
regions,
frame_time=frame_time,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
mosaic.add_image(pos.time, f, pos)
if pos and len(regions) > 0:
altitude = camera_settings.altitude
if altitude <= 0:
altitude = None
joelog.add_regions(frame_time,
regions,
pos,
f,
width=w,
height=h,
altitude=altitude)
region_count += len(regions)
if len(regions) > 0:
composite = cuav_mosaic.CompositeThumbnail(
cv.GetImage(cv.fromarray(im_full)), regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
mosaic.add_regions(regions, thumbs, f, pos)
if opts.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
mat = cv.fromarray(img_view)
for r in regions:
r.draw_rectangle(mat, (255, 0, 0))
cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
viewer.set_image(mat)
viewer.set_title('Image: ' + os.path.basename(f))
total_time += (t1 - t0)
if t1 != t0:
print('%s scan %.1f fps %u regions [%u/%u]' %
(os.path.basename(f), count / total_time, region_count,
scan_count, num_files))
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM', 'CMD']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW', 'GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'GPS'])
if len(types) == 2:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
if m.get_type() in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if m.get_type() == 'AHR2':
if opts.dualgps:
instance = 2
else:
instance = 1
if abs(lat) > 0.01 or abs(lng) > 0.01:
if getattr(mlog, 'flightmode', '') in colourmap:
colour = colourmap[mlog.flightmode]
(r, g, b) = colour
(r, g, b) = (r + instance * 50, g + instance * 50,
b + instance * 50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, filename),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs,
mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() == "HEARTBEAT":
from pymavlink import mavutil
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
self.vehicle_type_name = 'plane'
elif m.type in [
mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
mavutil.mavlink.MAV_TYPE_SUBMARINE
]:
self.vehicle_type_name = 'rover'
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_COAXIAL,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER,
mavutil.mavlink.MAV_TYPE_HELICOPTER
]:
self.vehicle_type_name = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
self.vehicle_type_name = 'antenna'
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()
if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.mpstate.map.set_position('Sim' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
self.mpstate.map.set_position('AHRS2' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.mpstate.map.set_position('GPS' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == 'GLOBAL_POSITION_INT':
(self.lat, self.lon, self.heading) = (m.lat * 1.0e-7,
m.lon * 1.0e-7, m.hdg * 0.01)
if self.lat != 0 or self.lon != 0:
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
self.mpstate.map.set_position('Pos' + vehicle,
(self.lat, self.lon),
rotation=self.heading)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
trajectory = [(self.lat, self.lon),
mp_util.gps_newpos(self.lat, self.lon,
m.target_bearing, m.wp_dist)]
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer='Trajectory',
linewidth=2,
colour=(255, 0, 180)))
else:
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('Trajectory'))
# if the waypoints have changed, redisplay
if self.wp_change_time != self.module('wp').wploader.last_change:
self.wp_change_time = self.module('wp').wploader.last_change
self.display_waypoints()
# if the fence has changed, redisplay
if self.fence_change_time != self.module(
'fence').fenceloader.last_change:
self.display_fence()
# if the rallypoints have changed, redisplay
if self.rally_change_time != self.module(
'rally').rallyloader.last_change:
self.rally_change_time = self.module(
'rally').rallyloader.last_change
icon = self.mpstate.map.icon('rallypoint.png')
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('RallyPoints'))
for i in range(self.module('rally').rallyloader.rally_count()):
rp = self.module('rally').rallyloader.rally_point(i)
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Rally Remove',
returnkey='popupRallyRemove'),
MPMenuItem('Rally Move', returnkey='popupRallyMove')
])
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('Rally %u' % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer='RallyPoints',
rotation=0,
follow=False,
popup_menu=popup))
# check for any events from the map
self.mpstate.map.check_events()
def mavflightview_show(path, wp, fen, options, title=None):
if not title:
title='MAVFlightView'
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() == "HEARTBEAT":
from pymavlink import mavutil
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
self.vehicle_type_name = 'plane'
elif m.type in [
mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
mavutil.mavlink.MAV_TYPE_SUBMARINE
]:
self.vehicle_type_name = 'rover'
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_COAXIAL,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER,
mavutil.mavlink.MAV_TYPE_HELICOPTER
]:
self.vehicle_type_name = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
self.vehicle_type_name = 'antenna'
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()
if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.mpstate.map.set_position('Sim' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
self.mpstate.map.set_position('AHRS2' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.mpstate.map.set_position('GPS' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == 'GLOBAL_POSITION_INT':
(self.lat, self.lon, self.heading) = (m.lat * 1.0e-7,
m.lon * 1.0e-7, m.hdg * 0.01)
if self.lat != 0 or self.lon != 0:
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
self.mpstate.map.set_position('Pos' + vehicle,
(self.lat, self.lon),
rotation=self.heading)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
trajectory = [(self.lat, self.lon),
mp_util.gps_newpos(self.lat, self.lon,
m.target_bearing, m.wp_dist)]
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer='Trajectory',
linewidth=2,
colour=(255, 0, 180)))
else:
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('Trajectory'))
# if the waypoints have changed, redisplay
last_wp_change = self.module('wp').wploader.last_change
if self.wp_change_time != last_wp_change and abs(time.time() -
last_wp_change) > 1:
self.wp_change_time = last_wp_change
self.display_waypoints()
#this may have affected the landing lines from the rally points:
self.rally_change_time = time.time()
# if the fence has changed, redisplay
if self.fence_change_time != self.module(
'fence').fenceloader.last_change:
self.display_fence()
# if the rallypoints have changed, redisplay
if self.rally_change_time != self.module(
'rally').rallyloader.last_change:
self.rally_change_time = self.module(
'rally').rallyloader.last_change
icon = self.mpstate.map.icon('rallypoint.png')
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('RallyPoints'))
for i in range(self.module('rally').rallyloader.rally_count()):
rp = self.module('rally').rallyloader.rally_point(i)
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Rally Remove',
returnkey='popupRallyRemove'),
MPMenuItem('Rally Move', returnkey='popupRallyMove')
])
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('Rally %u' % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer='RallyPoints',
rotation=0,
follow=False,
popup_menu=popup))
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
if self.map_settings.rallycircle:
self.mpstate.map.add_object(
mp_slipmap.SlipCircle(
'Rally Circ %u' % (i + 1),
'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
abs(loiter_rad), (255, 255, 0), 2))
#draw a line between rally point and nearest landing point
nearest_land_wp = None
nearest_distance = 10000000.0
for j in range(self.module('wp').wploader.count()):
w = self.module('wp').wploader.wp(j)
if (w.command == 21): #if landing waypoint
#get distance between rally point and this waypoint
dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
rp.lng * 1.0e-7)
if (dis < nearest_distance):
nearest_land_wp = w
nearest_distance = dis
if nearest_land_wp != None:
points = []
#tangential approach?
if self.get_mav_param('LAND_BREAK_PATH') == 0:
theta = math.degrees(
math.atan(loiter_rad / nearest_distance))
tan_dis = math.sqrt(nearest_distance *
nearest_distance -
(loiter_rad * loiter_rad))
ral_bearing = mp_util.gps_bearing(
nearest_land_wp.x, nearest_land_wp.y,
rp.lat * 1.0e-7, rp.lng * 1.0e-7)
points.append(
mp_util.gps_newpos(nearest_land_wp.x,
nearest_land_wp.y,
ral_bearing + theta, tan_dis))
else: #not tangential approach
points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))
points.append((nearest_land_wp.x, nearest_land_wp.y))
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
points, 'RallyPoints',
(255, 255, 0), 2))
# check for any events from the map
self.mpstate.map.check_events()
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
path = []
while True:
m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT'])
if m is None:
break
if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition(
opts.condition):
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
if lat != 0 or lng != 0:
if mlog.flightmode in colourmap:
point = (lat, lng, colourmap[mlog.flightmode])
else:
point = (lat, lng)
path.append(point)
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
if len(path) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path)
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_obj = mp_slipmap.SlipPolygon('FlightPath',
path,
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission',
wp.polygon(),
layer='Mission',
linewidth=2,
colour=(255, 255, 255))
else:
mission_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj,
mission_obj)
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon)
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
