def mavlink_packet_distance_sensor(self, vehicle, m):
heading = self.heading_for_vehicle.get(vehicle, 0)
tlayer = "Distance sensor for %u.%u id=%u" % (
m.get_srcSystem(), m.get_srcComponent(), m.id)
slipkey = '%s-POS%u' % (tlayer, m.orientation)
if not m.get_srcSystem() in self.lat_lon_for_vehicle:
return
if m.current_distance == m.max_distance:
self.foreach_map(lambda a_map: a_map.remove_object(slipkey))
return
(lat, lon) = self.lat_lon_for_vehicle[m.get_srcSystem()]
mav_sensor_rotation_to_degrees = {
mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 0,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 45,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 90,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 135,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 180,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 225,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 270,
mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 315,
}
if m.orientation in mav_sensor_rotation_to_degrees:
degrees = mav_sensor_rotation_to_degrees[m.orientation]
else:
# print("bad orientation (%u)" % m.orientation)
return
if m.current_distance >= m.max_distance:
return
p = mp_util.gps_newpos(lat, lon, heading + degrees, 0)
# start angle/end angle are either side of the primary axis,
# which is rotated to be North
start_angle = -22.5
end_angle = 22.5
self.foreach_map(lambda a_map: a_map.add_object(
mp_slipmap.SlipCircle(
slipkey,
3,
p,
m.current_distance / 100.0,
(255, 127, 0),
linewidth=3,
start_angle=start_angle,
end_angle=end_angle,
rotation=(-90 + (heading + degrees)) % 360,
)))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() == "ADSB_VEHICLE":
id = 'ADSB-' + str(m.ICAO_address)
if id not in self.threat_vehicles.keys(): # check to see if the vehicle is in the dict
# if not then add it
self.threat_vehicles[id] = ADSBVehicle(id=id, state=m.to_dict())
for mp in self.module_matching('map*'):
from modules.lib import mp_menu
from modules.mavproxy_map import mp_slipmap
self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem(name=id, returnkey=None)
threat_radius = get_threat_radius(m)
selected_icon = get_threat_icon(m, self.threat_vehicles[id].icon)
if selected_icon is not None:
# draw the vehicle on the map
popup = mp_menu.MPMenuSubMenu('ADSB', items=[self.threat_vehicles[id].menu_item])
icon = mp.map.icon(selected_icon)
mp.map.add_object(mp_slipmap.SlipIcon(id, (m.lat * 1e-7, m.lon * 1e-7),
icon, layer=3, rotation=m.heading*0.01, follow=False,
trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)),
popup_menu=popup))
if threat_radius > 0:
mp.map.add_object(mp_slipmap.SlipCircle(id+":circle", 3,
(m.lat * 1e-7, m.lon * 1e-7),
threat_radius, (0, 255, 255), linewidth=1))
else: # the vehicle is in the dict
# update the dict entry
self.threat_vehicles[id].update(m.to_dict(), self.get_time())
for mp in self.module_matching('map*'):
# update the map
ground_alt = mp.ElevationMap.GetElevation(m.lat*1e-7, m.lon*1e-7)
alt_amsl = m.altitude * 0.001
if alt_amsl > 0:
alt = int(alt_amsl - ground_alt)
label = str(alt) + "m"
else:
label = None
mp.map.set_position(id, (m.lat * 1e-7, m.lon * 1e-7), rotation=m.heading*0.01, label=label, colour=(0,250,250))
mp.map.set_position(id+":circle", (m.lat * 1e-7, m.lon * 1e-7))
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 mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
from modules.mavproxy_map import mp_slipmap
mtype = m.get_type()
sysid = m.get_srcSystem()
if mtype == "HEARTBEAT":
vname = 'plane'
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
vname = 'plane'
elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]:
vname = 'rover'
elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]:
vname = 'sub'
elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]:
vname = 'boat'
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER
]:
vname = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]:
vname = 'singlecopter'
elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
vname = 'heli'
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
vname = 'antenna'
self.vehicle_type_by_sysid[sysid] = vname
if not sysid in self.vehicle_type_by_sysid:
self.vehicle_type_by_sysid[sysid] = 'plane'
self.vehicle_type_name = self.vehicle_type_by_sysid[sysid]
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()
if mtype == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.map.set_position('Sim' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
self.map.set_position('AHRS2' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "AHRS3" and self.map_settings.showahrs3pos:
self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
self.map.set_position('AHRS3' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
if m.vel > 300 or 'ATTITUDE' not in self.master.messages:
cog = m.cog * 0.01
else:
cog = math.degrees(self.master.messages['ATTITUDE'].yaw)
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.map.set_position('GPS' + vehicle, (lat, lon),
rotation=cog)
elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.map.set_position('GPS2' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
elif mtype == 'GLOBAL_POSITION_INT' and self.map_settings.showahrspos:
(lat, lon, heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7,
m.hdg * 0.01)
self.lat_lon[m.get_srcSystem()] = (lat, lon)
if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3:
self.have_global_position = True
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
if len(self.vehicle_type_by_sysid) > 1:
label = str(sysid)
else:
label = None
self.map.set_position('Pos' + vehicle, (lat, lon),
rotation=heading,
label=label,
colour=(255, 255, 255))
self.map.set_follow_object('Pos' + vehicle,
self.is_primary_vehicle(m))
elif mtype == 'HOME_POSITION':
(lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7)
icon = self.map.icon('home.png')
self.map.add_object(
mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon),
icon,
layer=3,
rotation=0,
follow=False))
elif mtype == "NAV_CONTROLLER_OUTPUT":
tlayer = 'Trajectory%u' % m.get_srcSystem()
if (self.master.flightmode in [
"AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
"QLAND", "FOLLOW"
] and m.get_srcSystem() in self.lat_lon):
(lat, lon) = self.lat_lon[m.get_srcSystem()]
trajectory = [(lat, lon),
mp_util.gps_newpos(lat, lon, m.target_bearing,
m.wp_dist)]
self.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer=tlayer,
linewidth=2,
colour=(255, 0, 180)))
self.trajectory_layers.add(tlayer)
else:
if tlayer in self.trajectory_layers:
self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
self.trajectory_layers.remove(tlayer)
elif mtype == "POSITION_TARGET_GLOBAL_INT":
# FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL?
if not m.get_srcSystem() in self.lat_lon:
return
tlayer = 'PostionTarget%u' % m.get_srcSystem()
(lat, lon) = self.lat_lon[m.get_srcSystem()]
if (self.master.flightmode in [
"AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
"QLAND", "FOLLOW"
]):
lat_float = m.lat_int * 1e-7
lon_float = m.lon_int * 1e-7
vec = [(lat_float, lon_float), (lat, lon)]
self.map.add_object(
mp_slipmap.SlipPolygon('position_target',
vec,
layer=tlayer,
linewidth=2,
colour=(0, 255, 0)))
else:
self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
if not self.is_primary_vehicle(m):
# the rest should only be done for the primary vehicle
return
# if the waypoints have changed, redisplay
last_wp_change = self.module('wp').wploader.last_change
if self.wp_change_time != last_wp_change and abs(time.time() -
last_wp_change) > 1:
self.wp_change_time = last_wp_change
self.display_waypoints()
#this may have affected the landing lines from the rally points:
self.rally_change_time = time.time()
# if the fence has changed, redisplay
if (self.module('fence') and self.fence_change_time !=
self.module('fence').fenceloader.last_change):
self.display_fence()
# if the rallypoints have changed, redisplay
if (self.module('rally') and
self.rally_change_time != self.module('rally').last_change()):
self.rally_change_time = self.module('rally').last_change()
icon = self.map.icon('rallypoint.png')
self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
for i in range(self.module('rally').rally_count()):
rp = self.module('rally').rally_point(i)
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Rally Remove',
returnkey='popupRallyRemove'),
MPMenuItem('Rally Move', returnkey='popupRallyMove')
])
self.map.add_object(
mp_slipmap.SlipIcon('Rally %u' % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer='RallyPoints',
rotation=0,
follow=False,
popup_menu=popup))
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
if self.map_settings.rallycircle:
self.map.add_object(
mp_slipmap.SlipCircle(
'Rally Circ %u' % (i + 1),
'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
loiter_rad, (255, 255, 0),
2,
arrow=self.map_settings.showdirection))
#draw a line between rally point and nearest landing point
nearest_land_wp = None
nearest_distance = 10000000.0
for j in range(self.module('wp').wploader.count()):
w = self.module('wp').wploader.wp(j)
if (w.command == 21): #if landing waypoint
#get distance between rally point and this waypoint
dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
rp.lng * 1.0e-7)
if (dis < nearest_distance):
nearest_land_wp = w
nearest_distance = dis
if nearest_land_wp is not None:
points = []
#tangential approach?
if self.get_mav_param('LAND_BREAK_PATH') == 0:
theta = math.degrees(
math.atan(loiter_rad / nearest_distance))
tan_dis = math.sqrt(nearest_distance *
nearest_distance -
(loiter_rad * loiter_rad))
ral_bearing = mp_util.gps_bearing(
nearest_land_wp.x, nearest_land_wp.y,
rp.lat * 1.0e-7, rp.lng * 1.0e-7)
points.append(
mp_util.gps_newpos(nearest_land_wp.x,
nearest_land_wp.y,
ral_bearing + theta, tan_dis))
else: #not tangential approach
points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))
points.append((nearest_land_wp.x, nearest_land_wp.y))
self.map.add_object(
mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
points, 'RallyPoints',
(255, 255, 0), 2))
# check for any events from the map
self.map.check_events()
def display_waypoints(self):
'''display the waypoints'''
from 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'))
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 cmd_map(self, args):
'''map commands'''
from modules.mavproxy_map import mp_slipmap
if len(args) < 1:
print("usage: map <icon|set>")
elif args[0] == "icon":
if len(args) < 3:
print("Usage: map icon <lat> <lon> <icon>")
else:
lat = args[1]
lon = args[2]
flag = 'flag.png'
if len(args) > 3:
flag = args[3] + '.png'
icon = self.map.icon(flag)
self.map.add_object(
mp_slipmap.SlipIcon('icon - %s [%u]' %
(str(flag), self.icon_counter),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
self.icon_counter += 1
elif args[0] == "circle":
if len(args) < 4:
# map circle -27.70533373 153.23404844 5 red
print("Usage: map circle <lat> <lon> <radius> <colour>")
else:
lat = args[1]
lon = args[2]
radius = args[3]
colour = 'red'
if len(args) > 4:
colour = args[4]
if colour == "red":
colour = (255, 0, 0)
elif colour == "green":
colour = (0, 255, 0)
elif colour == "blue":
colour = (0, 0, 255)
else:
colour = eval(colour)
circle = mp_slipmap.SlipCircle(
"circle %u" % self.circle_counter,
3,
(float(lat), float(lon)),
float(radius),
colour,
linewidth=1,
)
self.map.add_object(circle)
self.circle_counter += 1
elif args[0] == "set":
self.map_settings.command(args[1:])
self.map.add_object(
mp_slipmap.SlipBrightness(self.map_settings.brightness))
elif args[0] == "sethome":
self.cmd_set_home(args)
elif args[0] == "sethomepos":
self.cmd_set_homepos(args)
elif args[0] == "setorigin":
self.cmd_set_origin(args)
elif args[0] == "setoriginpos":
self.cmd_set_originpos(args)
elif args[0] == "zoom":
self.cmd_zoom(args)
elif args[0] == "center":
self.cmd_center(args)
elif args[0] == "follow":
self.cmd_follow(args)
elif args[0] == "clear":
self.cmd_clear(args)
else:
print("usage: map <icon|set>")