def show_LandingZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
pos = self.click_position
'Create a new layer of two circles - at 30m and 80m radius around the above point'
if (self.mpstate.showLandingZone):
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('LandingZone'))
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('LandingZoneInner',
layer='LandingZone',
latlon=pos,
radius=30,
linewidth=2,
color=(0, 0, 255)))
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('LandingZoneOuter',
layer='LandingZone',
latlon=pos,
radius=80,
linewidth=2,
color=(0, 0, 255)))
else:
self.mpstate.map.remove_object('LandingZoneInner')
self.mpstate.map.remove_object('LandingZoneOuter')
self.mpstate.map.remove_object('LandingZone')
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 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 MAVProxy.modules.lib import mp_menu
from MAVProxy.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 cmd_obstacle(self, args):
'''control behaviour of the module'''
if len(args) == 0:
print(self.usage())
elif args[0] == "status":
print(self.status())
elif args[0] == "set":
self.obstacle_settings.command(args[1:])
else:
print(self.usage())
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Obstacle'))
#missions = client.get_missions()
#print missions
#stationary_obstacles, moving_obstacles = self.client.get_obstacles()
#s_o=stationary_obstacles
#print m_o
#print s_o[0].latitude
#print s_o[0].longitude
#print s_o[0].cylinder_radius
obstaclefile = open("/home/abhinavjava/cells.txt", "r")
print "Before slipcircle"
lines = obstaclefile.readlines()
for i in lines:
thisline = i.split(" ")
location = (float(thisline[0]), float(thisline[1]))
r = float(thisline[2])
if r == 3:
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('Obstacle Circle' + i,
layer='Obstacle',
latlon=location,
radius=r,
linewidth=-1,
color=(0, 255, 0)))
elif r == 5:
self.mpstate.map.add_object(
mp_slipmap.SlipCircle('Obstacle Circle' + i,
layer='Obstacle',
latlon=location,
radius=r,
linewidth=-1,
color=(255, 0, 0)))
print "After slipcircle"
"""while True: #dynamic obstacles
def update_map(self):
'''update the map graphics'''
for mp in self.module_matching('map*'):
for id in self.threat_vehicles.keys():
mstate = self.threat_vehicles[id].state
threat_radius = self.threat_vehicles[id].getThreatRadius(
self.AIS_settings.threat_radius)
# update if existing object on map, else create
if self.threat_vehicles[id].onMap:
mp.map.set_position(id,
(mstate.lat * 1e-7, mstate.lon * 1e-7),
3,
rotation=mstate.heading * 0.01,
label=str(mstate.callsign))
# Turns out we can't edit the circle's radius, so have to remove and re-add
mp.map.remove_object(id + ":circle")
mp.map.add_object(
mp_slipmap.SlipCircle(
id + ":circle",
3, (mstate.lat * 1e-7, mstate.lon * 1e-7),
threat_radius, (0, 255, 255),
linewidth=1))
else:
self.threat_vehicles[id].menu_item = mp_menu.MPMenuItem(
name=id, returnkey=None)
# draw the vehicle on the map
popup = mp_menu.MPMenuSubMenu(
'AIS', items=[self.threat_vehicles[id].menu_item])
icon = mp_slipmap.SlipIcon(
id,
(mstate.lat * 1e-7, mstate.lon * 1e-7),
mp.map.icon(self.threat_vehicles[id].icon),
3,
rotation=mstate.heading * 0.01,
follow=False,
trail=mp_slipmap.SlipTrail(colour=(0, 255, 255)),
popup_menu=popup,
)
mp.map.add_object(icon)
mp.map.add_object(
mp_slipmap.SlipCircle(
id + ":circle",
3, (mstate.lat * 1e-7, mstate.lon * 1e-7),
threat_radius, (0, 255, 255),
linewidth=1))
self.threat_vehicles[id].onMap = True
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)
start_angle = -increment / 2.0
end_angle = increment / 2.0
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))
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=(-90 + (heading + i * increment)) % 360,
)
self.foreach_map(lambda a_map: a_map.add_object(circle))
# add a circle for max-range
slipkey = "%s-range" % tlayer
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,
)))
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 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 show_JoeZone(self):
'''show the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera_ground')
if camera is None:
print("camera_ground module is not loaded")
return
target = (camera.camera_settings.target_latitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
for m in self.module_matching('map*'):
m.map.add_object(mp_slipmap.SlipClearLayer('JoeZone'))
m.map.add_object(
mp_slipmap.SlipCircle('JoeZoneCircle',
layer='JoeZone',
latlon=(target[0], target[1]),
radius=target[2],
linewidth=2,
color=(0, 0, 128)))
def toggle_JoeZone(self):
'''show/hide the UAV Challenge landing zone around the clicked point'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
camera = self.module('camera')
if camera is None:
print("camera module is not loaded")
return
if camera.camera_settings.target_radius <= 0:
print("camera module target_radius is not set")
return
target = (camera.camera_settings.target_lattitude,
camera.camera_settings.target_longitude,
camera.camera_settings.target_radius)
self.target = target
'Create a new layer with given radius around the above point'
if self.showJoeZone:
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('JoeZone'))
self.mpstate.map.add_object(mp_slipmap.SlipCircle('JoeZoneCircle', layer='JoeZone',
latlon=(target[0],target[1]), radius=target[2], linewidth=2, color=(0,0,128)))
else:
self.mpstate.map.remove_object('JoeZoneCircle')
self.mpstate.map.remove_object('JoeZone')
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 cmd_map(self, args):
'''map commands'''
from MAVProxy.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>")
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())
if self.mpstate.map: # if the map is loaded...
icon = self.mpstate.map.icon(self.threat_vehicles[id].icon)
popup = MPMenuSubMenu(
'ADSB', items=[MPMenuItem(name=id, returnkey=None)])
# draw the vehicle on the map
self.mpstate.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))
else: # the vehicle is in the dict
# update the dict entry
self.threat_vehicles[id].update(m.to_dict())
if self.mpstate.map: # if the map is loaded...
# update the map
self.mpstate.map.set_position(id,
(m.lat * 1e-7, m.lon * 1e-7),
rotation=m.heading * 0.01)
if m.get_type() == "GLOBAL_POSITION_INT":
if self.mpstate.map:
if len(self.active_threat_ids) > 0:
threat_circle_width = 2
else:
threat_circle_width = 1
# update the threat circle on the map
threat_circle = mp_slipmap.SlipCircle(
"threat_circle",
3, (m.lat * 1e-7, m.lon * 1e-7),
self.ADSB_settings.threat_radius, (0, 255, 255),
linewidth=threat_circle_width)
threat_circle.set_hidden(
not self.ADSB_settings.show_threat_radius
) # show the circle?
self.mpstate.map.add_object(threat_circle)
# update the threat clear circle on the map
threat_radius_clear = self.ADSB_settings.threat_radius * \
self.ADSB_settings.threat_radius_clear_multiplier
threat_clear_circle = mp_slipmap.SlipCircle(
"threat_clear_circle",
3, (m.lat * 1e-7, m.lon * 1e-7),
threat_radius_clear, (0, 255, 255),
linewidth=1)
# show the circle?
threat_clear_circle.set_hidden(
not self.ADSB_settings.show_threat_radius_clear)
self.mpstate.map.add_object(threat_clear_circle)
# we assume this is handled much more oftern than ADS-B messages
# so update the distance between vehicle and threat here
self.update_threat_distances(
(m.lat * 1e-7, m.lon * 1e-7, m.alt * 1e-3))
def check_commands(self, bsend):
'''check for remote commands'''
if bsend is None:
return
buf = bsend.recv(0)
if buf is None:
return
try:
obj = cPickle.loads(str(buf))
if obj is None:
return
except Exception as e:
return
if isinstance(obj, cuav_command.StampedCommand):
bidx = None
for i in range(len(self.bsend)):
if bsend == self.bsend[i]:
bidx = i + 1
if bidx is None and bsend == self.msend:
bidx = 0
if bidx is not None:
now = time.time()
self.mcount[bidx] += 1
self.last_msg_stamp[bidx] = now
if obj.timestamp in self.handled_timestamps:
# we've seen this packet before, discard
return
self.handled_timestamps[obj.timestamp] = time.time()
if isinstance(obj, cuav_command.HeartBeat):
self.image_count = obj.icount
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6)
if isinstance(obj, cuav_command.ThumbPacket):
# we've received a set of thumbnails from the plane for a positive hit
if obj.frame_time not in self.thumbs_received:
self.thumbs_received.add(obj.frame_time)
self.thumb_total_bytes += len(buf)
# add the thumbnails to the mosaic
thumbdec = cv2.imdecode(obj.thumb, 1)
if thumbdec is None:
pass
thumbs = cuav_mosaic.ExtractThumbs(thumbdec, len(obj.regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
# log the joe positions
# note the filename is where the fullsize image would be downloaded
# to, if requested
filename = os.path.join(
self.view_dir, cuav_util.frame_time(obj.frame_time)) + ".jpg"
self.log_joe_position(obj.pos, obj.frame_time, obj.regions,
filename, None)
# update the mosaic and map
self.mosaic.add_regions(obj.regions, thumbsRGB, filename, obj.pos)
# update console display
self.region_count += len(obj.regions)
self.thumb_count += 1
self.console.set_status('Regions',
'Regions %u' % self.region_count,
row=6)
self.console.set_status('Thumbs',
'Thumbs %u' % self.thumb_count,
row=7)
self.console.set_status(
'ThumbSize',
'ThumbSize %.0f' % (self.thumb_total_bytes / self.thumb_count),
row=7)
if isinstance(obj, cuav_command.ImagePacket):
# we have an image from the plane
self.image_total_bytes += len(buf)
#save to file
imagedec = cv2.imdecode(obj.jpeg, 1)
ff = os.path.join(self.view_dir,
cuav_util.frame_time(obj.frame_time)) + ".jpg"
write_param = [int(cv2.IMWRITE_JPEG_QUALITY), 99]
cv2.imwrite(ff, imagedec, write_param)
self.mosaic.tag_image(obj.frame_time)
if obj.pos is not None:
self.mosaic.add_image(obj.frame_time, ff, obj.pos)
# update console
self.image_count += 1
color = 'black'
self.console.set_status('Images',
'Images %u' % self.image_count,
row=6,
fg=color)
if isinstance(obj, cuav_command.CommandPacket):
# ground doesn't accept command packets from air
pass
if isinstance(obj, cuav_command.CommandResponse):
# reply to CommandPacket
print('REMOTE: %s' % obj.response)
if isinstance(obj, cuav_command.CameraMessage):
print('CUAV AIR REMOTE: %s' % obj.msg)
if isinstance(obj, cuav_landingregion.LandingZoneDisplay):
lzresult = obj
# display on all maps
for m in self.module_matching('map?'):
m.map.add_object(
mp_slipmap.SlipCircle('LZ',
'LZ',
lzresult.latlon,
lzresult.maxrange,
linewidth=3,
color=(0, 255, 0)))
m.map.add_object(
mp_slipmap.SlipCircle('LZMid',
'LZMid',
lzresult.latlon,
2.0,
linewidth=3,
color=(0, 255, 0)))
lztext = 'LZ: %.6f %.6f E:%.1f AS:%.0f N:%u' % (
lzresult.latlon[0], lzresult.latlon[1], lzresult.maxrange,
lzresult.avgscore, lzresult.numregions)
m.map.add_object(mp_slipmap.SlipInfoText(
'landingzone', lztext))
# assume map2 is the search map
map2 = self.module('map2')
if map2 is not None:
#map2.map.set_zoom(250)
map2.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map2.map.set_follow(0)
# assume map3 is the lz map
map3 = self.module('map3')
if map3 is not None:
map3.map.set_zoom(max(50, 2 * lzresult.maxrange))
map3.map.set_center(lzresult.latlon[0], lzresult.latlon[1])
map3.map.set_follow(0)
try:
cuav = self.module('CUAV')
cuav.show_JoeZone()
except Exception as ex:
print("err: ", ex)
return
if isinstance(obj, cuav_command.FilePacket):
print("got file %s" % obj.filename)
try:
open(obj.filename, "w").write(obj.contents)
except Exception as ex:
print("file save failed", ex)
return
if obj.filename == "newwp.txt":
try:
wpmod = self.module('wp')
wpmod.wploader.load(obj.filename)
except Exception as ex:
print("wp load failed", ex)
if isinstance(obj, cuav_command.PreviewPacket):
# we have a preview image from the plane
img = cv2.imdecode(obj.jpeg, 1)
if self.preview_window is None or not self.preview_window.is_alive(
):
self.preview_window = mp_image.MPImage(title='Preview',
width=410,
height=308,
auto_size=True)
if self.preview_window is not None:
self.preview_window.set_image(img, bgr=True)
self.preview_window.poll()
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', 'jpeg', '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')
if args.vehicle_type == "Copter":
icon = slipmap.icon('redcopter.png')
else:
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)
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)
else:
kmzpos = None
if args.triggerlog:
triggerpos = mav_position.TriggerPosition(args.triggerlog)
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)
else:
im_orig = cv2.imread(files[0])
(w, h) = cuav_util.image_shape(im_orig)
C_params = cam_params.CameraParams(lens=args.lens,
sensorwidth=args.sensorwidth,
xresolution=w,
yresolution=h)
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('pitch_stabilised', bool, args.pitch_stabilised,
'Pitch Stabilised'),
MPSetting('roll_offset', float, args.roll_offset, 'Roll Offset'),
MPSetting('pitch_offset', float, args.pitch_offset, 'Pitch Offset'),
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, False, 'rotate180'),
MPSetting('showlz', bool, args.showlz,
'Show calculated landing zone from regions'),
MPSetting(
'filter_type', str, 'simple', 'Filter Type', choice=['simple']),
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('map_thumbsize',
int,
args.map_thumbsize,
'Map 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.15,
range=(0, 100),
increment=0.05,
digits=2,
tab='Image Processing'),
MPSetting('MaxRegionArea',
float,
1.0,
range=(0, 100),
increment=0.1,
digits=1),
MPSetting('MinRegionSize',
float,
0.2,
range=(0, 100),
increment=0.05,
digits=2),
MPSetting('MaxRegionSize',
float,
1.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,
map_thumb_size=args.map_thumbsize)
joelog = cuav_joe.JoeLog(None)
lz = cuav_landingregion.LandingZone()
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
if camera_settings.pitch_stabilised:
pitch = 0
else:
pitch = None
try:
pos = mpos.position(frame_time,
roll=roll,
pitch=pitch,
roll_offset=camera_settings.roll_offset,
pitch_offset=camera_settings.pitch_offset)
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 vehicle 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)
im_orig = cv2.imread(f)
if im_orig is None:
continue
(w, h) = cuav_util.image_shape(im_orig)
if args.downsample:
im_full = cv2.resize(im_orig, (0, 0), fx=0.5, fy=0.5)
else:
im_full = im_orig
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,
min_score=camera_settings.minscore,
filter_type=camera_settings.filter_type)
scan_count += 1
if pos and len(regions) > 0:
joelog.add_regions(frame_time, regions, pos, f)
mosaic.add_image(pos.time, f, pos)
if camera_settings.showlz:
for r in regions:
if r.score >= camera_settings.minscore and pos is not None:
lz.checkaddregion(r, pos)
#if lz.landingzoneconfidence and lz.landingzoneconfidence < 20:
# print("Refining LZ")
lzresult = lz.calclandingzone()
if lzresult is not None:
slipmap.add_object(
mp_slipmap.SlipCircle('LZ',
'LZ',
lzresult.latlon,
lzresult.maxrange,
linewidth=3,
color=(0, 255, 0)))
slipmap.add_object(
mp_slipmap.SlipCircle('LZMid',
'LZMid',
lzresult.latlon,
2.0,
linewidth=3,
color=(0, 255, 0)))
region_count += len(regions)
if len(regions) > 0:
composite = cuav_region.CompositeThumbnail(im_full, regions)
thumbs = cuav_mosaic.ExtractThumbs(composite, len(regions))
thumbsRGB = []
#colour space conversion
for thumb in thumbs:
thumbsRGB.append(cv2.cvtColor(thumb, cv2.COLOR_BGR2RGB))
mosaic.add_regions(regions, thumbsRGB, f, pos)
if args.view:
img_view = img_scan
(wview, hview) = cuav_util.image_shape(img_view)
for r in regions:
r.draw_rectangle(img_view, (255, 0, 0))
#cv.CvtColor(mat, mat, cv.CV_BGR2RGB)
img_view = cv2.cvtColor(img_view, cv2.COLOR_BGR2RGB)
viewer.set_image(img_view)
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))
#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 mavlink_packet(self, m):
'''handle and incoming mavlink packet'''
self.Update_traffic()
if (len(self.traffic_list) > 0):
self.kmbMsgCounter = self.kmbMsgCounter + 1
wcv_volume = mp_slipmap.SlipCircle("well_clear_volume", 3,\
(self.module('map').lat,self.module('map').lon),\
self.radius,\
(0, 0, 255), linewidth=2)
self.mpstate.map.add_object(wcv_volume)
if (self.kmbMsgCounter == 95):
self.Bands = []
if m.get_type() == "ICAROUS_KINEMATIC_BANDS":
self.kmbMsgCounter = 0
self.oldNumBands = self.numBands
self.numBands = m.numBands
numBands = 0
numBands = numBands + 1
self.Bands = []
if (numBands <= self.numBands):
low = m.min1
high = m.max1
bands = [low, high, m.type1]
self.Bands.append(bands)
numBands = numBands + 1
if (numBands <= self.numBands):
low = m.min2
high = m.max2
bands = [low, high, m.type2]
self.Bands.append(bands)
numBands = numBands + 1
if (numBands <= self.numBands):
low = m.min3
high = m.max3
bands = [low, high, m.type3]
self.Bands.append(bands)
numBands = numBands + 1
if (numBands <= self.numBands):
low = m.min4
high = m.max4
bands = [low, high, m.type4]
self.Bands.append(bands)
numBands = numBands + 1
if (numBands <= self.numBands):
low = m.min5
high = m.max5
bands = [low, high, m.type5]
self.Bands.append(bands)
numBands = numBands + 1
if (self.oldNumBands > self.numBands) or self.kmbMsgCounter == 100:
for i in range(self.oldNumBands):
self.mpstate.map.remove_object("band" + str(i))
for i, kmb in enumerate(self.Bands):
self.AddBand(i, kmb)
if m.get_type() == "TRAFFIC_INFO":
print m.breach_status
if m.get_type() == "COMMAND_LONG":
if self.V2V:
self.show_received_traffic(m)
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 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,
)))
