def drawing_end(self):
'''end line drawing'''
if self.draw_callback is None:
return
self.draw_callback(self.draw_line)
self.draw_callback = None
self.mpstate.map.add_object(mp_slipmap.SlipDefaultPopup(self.default_popup, combine=True))
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Drawing'))
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 display_waypoints():
'''display the waypoints'''
polygons = mpstate.status.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 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))
labeled_wps = {}
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)))
labeled_wps[next_list[j]] = (i,j)
def mavlink_packet(m):
'''handle an incoming mavlink packet'''
state = mpstate.map_state
if m.get_type() == "SIMSTATE":
if not mpstate.map_state.have_simstate:
mpstate.map_state.have_simstate = True
create_blueplane()
mpstate.map.set_position('blueplane', (m.lat, m.lng),
rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and not mpstate.map_state.have_simstate:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if state.lat is not None and (
mpstate.map_state.have_blueplane
or mp_util.gps_distance(lat, lon, state.lat, state.lon) > 10):
create_blueplane()
mpstate.map.set_position('blueplane', (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if mpstate.master().flightmode == "AUTO":
trajectory = [(state.lat, state.lon),
mp_util.gps_newpos(state.lat, state.lon,
m.target_bearing, m.wp_dist)]
mpstate.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer='Trajectory',
linewidth=2,
colour=(255, 0, 180)))
else:
mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))
if m.get_type() == 'GLOBAL_POSITION_INT':
(state.lat, state.lon, state.heading) = (m.lat * 1.0e-7,
m.lon * 1.0e-7, m.hdg * 0.01)
else:
return
if state.lat != 0 or state.lon != 0:
mpstate.map.set_position('plane', (state.lat, state.lon),
rotation=state.heading)
# if the waypoints have changed, redisplay
if state.wp_change_time != mpstate.status.wploader.last_change:
state.wp_change_time = mpstate.status.wploader.last_change
display_waypoints()
# if the fence has changed, redisplay
if state.fence_change_time != mpstate.status.fenceloader.last_change:
state.fence_change_time = mpstate.status.fenceloader.last_change
points = mpstate.status.fenceloader.polygon()
if len(points) > 1:
mpstate.map.add_object(
mp_slipmap.SlipPolygon('fence',
points,
layer=1,
linewidth=2,
colour=(0, 255, 0)))
# check for any events from the map
mpstate.map.check_events()
def 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()