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.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),
arrow=self.map_settings.showdirection,
popup_menu=popup))
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):
label = self.label_for_waypoint(next_list[j])
colour = self.colour_for_wp(next_list[j])
self.mpstate.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.mpstate.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 mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
if m.get_type() == "HEARTBEAT":
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
]:
self.vehicle_type_name = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
self.vehicle_type_name = 'heli'
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() == "AHRS3" and self.map_settings.showahrs3pos:
self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
self.mpstate.map.set_position('AHRS3' + 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 abs(self.lat) > 1.0e-3 or abs(self.lon) > 1.0e-3:
self.have_global_position = True
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(
) == 'LOCAL_POSITION_NED' and not self.have_global_position:
(self.lat, self.lon) = mp_util.gps_offset(0, 0, m.x, m.y)
self.heading = math.degrees(math.atan2(m.vy, m.vx))
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", "QRTL", "QLOITER",
"QLAND"
] and self.lat is not None and self.lon is not None):
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 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':
(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 == 'LOCAL_POSITION_NED' and not self.have_global_position:
(lat, lon) = mp_util.gps_offset(0, 0, m.x, m.y)
self.lat_lon[m.get_srcSystem()] = (lat, lon)
heading = math.degrees(math.atan2(m.vy, m.vx))
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
self.map.set_position('Pos' + vehicle, (lat, lon),
rotation=heading)
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)))
else:
self.map.add_object(mp_slipmap.SlipClearLayer(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.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').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 mavlink_packet_obstacle_distance(self, vehicle, m):
heading = self.heading_for_vehicle.get(vehicle, 0)
tlayer = "Obstacle Distance for %u.%u type=%u" % (
m.get_srcSystem(), m.get_srcComponent(), m.sensor_type)
if m.get_srcSystem() in self.lat_lon_for_vehicle:
color = (255, 0, 255)
(lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()]
self.foreach_map(lambda a_map: a_map.add_object(
mp_slipmap.SlipClearLayer(tlayer)))
increment = m.increment_f
if increment == 0:
increment = float(m.increment)
measurement_count = 0
for i in range(0, 72):
if m.distances[i] != 65535:
measurement_count += 1
fov = measurement_count * increment
start_angle = -increment / 2.0
end_angle = increment / 2.0
rotation_start = -90 + heading
for i in range(0, 72):
slipkey = '%s-POS%u' % (tlayer, i)
if m.distances[i] == m.max_distance + 1:
# no measurement
self.foreach_map(
lambda a_map: a_map.remove_object(slipkey))
measurement_count += 1
continue
distance = m.distances[i] / 100.0 # cm -> m
circle = mp_slipmap.SlipCircle(
slipkey,
3,
(lat, lon),
distance,
color,
linewidth=3,
start_angle=start_angle,
end_angle=end_angle,
rotation=(m.angle_offset + rotation_start + i * increment)
% 360,
)
if m.distances[i] < m.min_distance:
measurement_count += 1
self.foreach_map(lambda a_map: a_map.add_object(circle))
slipkey = "%s-range" % tlayer
if fov == 360:
# perfect circle
# add a circle for max-range
self.foreach_map(lambda a_map: a_map.add_object(
mp_slipmap.SlipCircle(
slipkey,
3,
(lat, lon),
m.max_distance / 100.0,
color,
linewidth=1,
)))
else:
# add an arc for max-range
self.foreach_map(lambda a_map: a_map.add_object(
mp_slipmap.SlipCircle(
slipkey,
3,
(lat, lon),
m.max_distance / 100.0,
color,
linewidth=1,
start_angle=-fov / 2,
end_angle=fov / 2,
rotation=(m.angle_offset + -90 + (heading)) % 360,
add_radii=True,
)))
def cmd_clear(self, args):
'''clear displayed vehicle icons'''
self.map.add_object(mp_slipmap.SlipClearLayer(3))
self.have_vehicle = {}
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if not self.mpstate.map:
# don't draw if no map
return
if m.get_type() != 'GIMBAL_REPORT':
return
needed = ['ATTITUDE', 'GLOBAL_POSITION_INT']
for n in needed:
if not n in self.master.messages:
return
# clear the camera icon
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView'))
gpi = self.master.messages['GLOBAL_POSITION_INT']
att = self.master.messages['ATTITUDE']
vehicle_dcm = Matrix3()
vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw)
rotmat_copter_gimbal = Matrix3()
rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el,
m.joint_az)
gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal
lat = gpi.lat * 1.0e-7
lon = gpi.lon * 1.0e-7
alt = gpi.relative_alt * 1.0e-3
# ground plane
ground_plane = Plane()
# the position of the camera in the air, remembering its a right
# hand coordinate system, so +ve z is down
camera_point = Vector3(0, 0, -alt)
# get view point of camera when not rotated
view_point = Vector3(1, 0, 0)
# rotate view_point to form current view vector
rot_point = gimbal_dcm * view_point
# a line from the camera to the ground
line = Line(camera_point, rot_point)
# find the intersection with the ground
pt = line.plane_intersection(ground_plane, forward_only=True)
if pt is None:
# its pointing up into the sky
return None
(view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x)
icon = self.mpstate.map.icon('camera-small-red.png')
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('gimbalview', (view_lat, view_lon),
icon,
layer='GimbalView',
rotation=0,
follow=False))
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
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 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
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 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 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()
