def size(self): '''return tile size as (width,height) in meters''' (lat1, lon1) = self.coord((0,0)) (lat2, lon2) = self.coord((TILES_WIDTH,0)) width = mp_util.gps_distance(lat1, lon1, lat2, lon2) (lat2, lon2) = self.coord((0,TILES_HEIGHT)) height = mp_util.gps_distance(lat1, lon1, lat2, lon2) return (width,height)
def update_position(self):
'''update position text'''
state = self.state
pos = self.mouse_pos
newtext = ''
alt = 0
if pos is not None:
(lat,lon) = self.coordinates(pos.x, pos.y)
newtext += 'Cursor: %f %f (%s)' % (lat, lon, mp_util.latlon_to_grid((lat, lon)))
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
newtext += ' %.1fm' % alt
pending = state.mt.tiles_pending()
if pending:
newtext += ' Map Downloading %u ' % pending
if alt == -1:
newtext += ' SRTM Downloading '
newtext += '\n'
if self.click_pos is not None:
newtext += 'Click: %f %f (%s %s) (%s)' % (self.click_pos[0], self.click_pos[1],
mp_util.degrees_to_dms(self.click_pos[0]),
mp_util.degrees_to_dms(self.click_pos[1]),
mp_util.latlon_to_grid(self.click_pos))
if self.last_click_pos is not None:
distance = mp_util.gps_distance(self.last_click_pos[0], self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
bearing = mp_util.gps_bearing(self.last_click_pos[0], self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
newtext += ' Distance: %.1fm Bearing %.1f' % (distance, bearing)
if newtext != state.oldtext:
self.position.Clear()
self.position.WriteText(newtext)
state.oldtext = newtext
def draw(self, img, pixmapper, bounds):
'''draw a polygon on the image'''
if self.hidden:
return
(x,y,w,h) = bounds
spacing = 1000
while True:
start = mp_util.latlon_round((x,y), spacing)
dist = mp_util.gps_distance(x,y,x+w,y+h)
count = int(dist / spacing)
if count < 2:
spacing /= 10
elif count > 50:
spacing *= 10
else:
break
for i in range(count*2+2):
pos1 = mp_util.gps_newpos(start[0], start[1], 90, i*spacing)
pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 0, 3*count*spacing)
self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
pos1 = mp_util.gps_newpos(start[0], start[1], 0, i*spacing)
pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 90, 3*count*spacing)
self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
def cmd_snap_wp(self, args):
'''snap waypoints to KML'''
threshold = 10.0
if len(args) > 0:
threshold = float(args[0])
wpmod = self.module('wp')
wploader = wpmod.wploader
changed = False
for i in range(1,wploader.count()):
w = wploader.wp(i)
if not wploader.is_location_command(w.command):
continue
lat = w.x
lon = w.y
best = None
best_dist = (threshold+1)*3
for (snap_lat,snap_lon) in self.snap_points:
dist = mp_util.gps_distance(lat, lon, snap_lat, snap_lon)
if dist < best_dist:
best_dist = dist
best = (snap_lat, snap_lon)
if best is not None and best_dist <= threshold:
if w.x != best[0] or w.y != best[1]:
w.x = best[0]
w.y = best[1]
print("Snapping WP %u to %f %f" % (i, w.x, w.y))
wploader.set(w, i)
changed = True
elif best is not None:
if best_dist <= (threshold+1)*3:
print("Not snapping wp %u dist %.1f" % (i, best_dist))
if changed:
wpmod.send_all_waypoints()
def cmd_snap_fence(self, args):
'''snap fence to KML'''
threshold = 10.0
if len(args) > 0:
threshold = float(args[0])
fencemod = self.module('fence')
loader = fencemod.fenceloader
changed = False
for i in range(0,loader.count()):
fp = loader.point(i)
lat = fp.lat
lon = fp.lng
best = None
best_dist = (threshold+1)*3
for (snap_lat,snap_lon) in self.snap_points:
dist = mp_util.gps_distance(lat, lon, snap_lat, snap_lon)
if dist < best_dist:
best_dist = dist
best = (snap_lat, snap_lon)
if best is not None and best_dist <= threshold:
if best[0] != lat or best[1] != lon:
loader.move(i, best[0], best[1])
print("Snapping fence point %u to %f %f" % (i, best[0], best[1]))
changed = True
elif best is not None:
if best_dist <= (threshold+1)*3:
print("Not snapping fence point %u dist %.1f" % (i, best_dist))
if changed:
fencemod.send_fence()
def estimated_time_remaining(self, lat, lon, wpnum, speed):
'''estimate time remaining in mission in seconds'''
idx = wpnum
if wpnum >= self.module('wp').wploader.count():
return 0
distance = 0
done = set()
while idx < self.module('wp').wploader.count():
if idx in done:
break
done.add(idx)
w = self.module('wp').wploader.wp(idx)
if w.command == mavutil.mavlink.MAV_CMD_DO_JUMP:
idx = int(w.param1)
continue
idx += 1
if (w.x != 0 or w.y != 0) and w.command in [mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
mavutil.mavlink.MAV_CMD_NAV_LAND,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF]:
distance += mp_util.gps_distance(lat, lon, w.x, w.y)
lat = w.x
lon = w.y
if w.command == mavutil.mavlink.MAV_CMD_NAV_LAND:
break
return distance / speed
def re_center(self, x, y, lat, lon):
'''re-center view for pixel x,y'''
state = self.state
if lat is None or lon is None:
return
(lat2,lon2) = self.coordinates(x, y)
distance = mp_util.gps_distance(lat2, lon2, lat, lon)
bearing = mp_util.gps_bearing(lat2, lon2, lat, lon)
(state.lat, state.lon) = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
def coord_to_pixel(self, lat, lon, width, ground_width, lat2, lon2): '''return pixel coordinate (px,py) for position (lat2,lon2) in an area image. Note that the results are relative to top,left and may be outside the image''' pixel_width = ground_width / float(width) if lat is None or lon is None or lat2 is None or lon2 is None: return (0,0) dx = mp_util.gps_distance(lat, lon, lat, lon2) if lon2 < lon: dx = -dx dy = mp_util.gps_distance(lat, lon, lat2, lon) if lat2 > lat: dy = -dy dx /= pixel_width dy /= pixel_width return (int(dx), int(dy))
def draw(self, img, pixmapper, bounds):
if self.hidden:
return
center_px = pixmapper(self.latlon)
#figure out pixels per meter
ref_pt = (self.latlon[0] + 1.0, self.latlon[1])
dis = mp_util.gps_distance(self.latlon[0], self.latlon[1], ref_pt[0], ref_pt[1])
ref_px = pixmapper(ref_pt)
dis_px = math.sqrt(float(center_px[1] - ref_px[1]) ** 2.0)
pixels_per_meter = dis_px / dis
cv.Circle(img, center_px, int(self.radius * pixels_per_meter), self.color, self.linewidth)
def could_collide_hor(self, vpos, adsb_pkt):
'''return true if vehicle could come within filter_dist_xy meters of adsb vehicle in timeout seconds'''
margin = self.asterix_settings.filter_dist_xy
timeout = self.asterix_settings.filter_time
alat = adsb_pkt.lat * 1.0e-7
alon = adsb_pkt.lon * 1.0e-7
avel = adsb_pkt.hor_velocity * 0.01
vvel = sqrt(vpos.vx**2 + vpos.vy**2)
dist = mp_util.gps_distance(vpos.lat, vpos.lon, alat, alon)
dist -= avel * timeout
dist -= vvel * timeout
if dist <= margin:
return True
return False
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 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 closest_waypoint(self, latlon):
'''find closest waypoint to a position'''
(lat, lon) = latlon
best_distance = -1
closest = -1
for i in range(self.module('wp').wploader.count()):
w = self.module('wp').wploader.wp(i)
distance = mp_util.gps_distance(lat, lon, w.x, w.y)
if best_distance == -1 or distance < best_distance:
best_distance = distance
closest = i
if best_distance < 20:
return closest
else:
return -1
def wp_slope(self):
'''show slope of waypoints'''
last_w = None
for i in range(1, self.wploader.count()):
w = self.wploader.wp(i)
if w.command not in [mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, mavutil.mavlink.MAV_CMD_NAV_LAND]:
continue
if last_w is not None:
if last_w.frame != w.frame:
print("WARNING: frame change %u -> %u at %u" % (last_w.frame, w.frame, i))
delta_alt = last_w.z - w.z
delta_xy = mp_util.gps_distance(w.x, w.y, last_w.x, last_w.y)
slope = delta_xy / delta_alt
print("WP%u: slope %.1f" % (i, slope))
last_w = w
def draw(self, img, pixmapper, bounds):
if self.hidden:
return
center_px = pixmapper(self.latlon)
# figure out pixels per meter
ref_pt = (self.latlon[0] + 1.0, self.latlon[1])
dis = mp_util.gps_distance(self.latlon[0], self.latlon[1], ref_pt[0], ref_pt[1])
ref_px = pixmapper(ref_pt)
dis_px = math.sqrt(float(center_px[1] - ref_px[1]) ** 2.0)
pixels_per_meter = dis_px / dis
radius_px = int(self.radius * pixels_per_meter)
cv2.circle(img, center_px, radius_px, self.color, self.linewidth)
if self.arrow:
SlipArrow(self.key, self.layer, (center_px[0]-radius_px, center_px[1]),
self.color, self.linewidth, 0, reverse = self.reverse).draw(img)
SlipArrow(self.key, self.layer, (center_px[0]+radius_px, center_px[1]),
self.color, self.linewidth, math.pi, reverse = self.reverse).draw(img)
def draw(self, img, pixmapper, bounds):
'''draw a polygon on the image'''
if self.hidden:
return
center = pixmapper(self.center)
# figure out pixels per meter
ref_pt = (self.center[0] + 1.0, self.center[1])
dis = mp_util.gps_distance(self.center[0], self.center[1], ref_pt[0],
ref_pt[1])
ref_px = pixmapper(ref_pt)
dis_px = math.sqrt(float(center[1] - ref_px[1])**2.0)
pixels_per_meter = dis_px / dis
axes0 = int(self.axes[0] * pixels_per_meter)
axes1 = int(self.axes[1] * pixels_per_meter)
axes = (axes0, axes1)
mp_slipmap.cv2.ellipse(img, center, axes, self.angle, self.startAngle,
self.endAngle, self.colour, self.linewidth)
def update_position(self):
'''update position text'''
state = self.state
pos = self.mouse_pos
newtext = ''
alt = 0
if pos is not None:
(lat, lon) = self.coordinates(pos.x, pos.y)
newtext += 'Cursor: %f %f (%s)' % (lat, lon,
mp_util.latlon_to_grid(
(lat, lon)))
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
if alt is not None:
newtext += ' %.1fm' % alt
state.mt.set_download(state.download)
pending = 0
if state.download:
pending = state.mt.tiles_pending()
if pending:
newtext += ' Map Downloading %u ' % pending
if alt == -1:
newtext += ' SRTM Downloading '
newtext += '\n'
if self.click_pos is not None:
newtext += 'Click: %f %f (%s %s) (%s)' % (
self.click_pos[0], self.click_pos[1],
mp_util.degrees_to_dms(self.click_pos[0]),
mp_util.degrees_to_dms(
self.click_pos[1]), mp_util.latlon_to_grid(self.click_pos))
if self.last_click_pos is not None:
distance = mp_util.gps_distance(self.last_click_pos[0],
self.last_click_pos[1],
self.click_pos[0],
self.click_pos[1])
bearing = mp_util.gps_bearing(self.last_click_pos[0],
self.last_click_pos[1],
self.click_pos[0], self.click_pos[1])
newtext += ' Distance: %.1fm Bearing %.1f' % (distance, bearing)
if newtext != state.oldtext:
self.position.Clear()
self.position.WriteText(newtext)
state.oldtext = newtext
def __init__(self):
super(AUVModules, self).__init__(mpstate, "AUV", "AUV sparse/dense traversal algorithm")
radctrl = rc()
wypt = wp()
batm = bm()
mav = mavutil.mavfile()
self.battery = batm.percentage()
#update using the dataflash logs
self.velocity = 0
self.time = time.clock()
self.surfaced = True
mav.set_mode_apm(9)
#GPS dependent values
self.location = mav.location()
self.home = [self.location.lng, self.location.lat]
self.waypoints = deque(wypt.load_waypoints_dive('/home/pi/waypoints.txt'))
wp = self.waypoints.popleft().rsplit()
self.next_wp = [float(wp[3]), float(wp[2])] #lng,lat
self.heading = self.location.heading #degrees relative to Magnetic North
self.distance = mp_util.gps_distance(self.location.lat, self.location.lng, self.next_wp[1], self.next_wp[0]) #meters
self.intended_heading = self.gps_bearing(self.location.lat, self.location.lng, self.next_wp[1], self.next_wp[0] )#degrees relative to present heading
#need a current sensor for this. Let's try to find average current info online and hardcode it for now.
self.current = 0 #m/s
#MAVProxy parameters
self.status_callcount = 0
self.boredom_interval = 1 # seconds
self.last_bored = time.clock()
self.packets_mytarget = 0
self.packets_othertarget = 0
self.verbose = True
self.AUVModule_settings = mp_settings.MPSettings([ ('verbose', bool, True), ])
self.add_command('auv', self.cmd_auv, "AUV module", ['status','set (LOGSETTING)', 'start', 'reboot'])
#set manual mode
mav.set_mode_manual()
radctrl.set_mode_manual()
#wait for motors to be armed
mav.motors_armed_wait()
#set the apm mav_type
mav.mode_mapping()
def update_position(self):
"""update position text"""
state = self.state
pos = self.mouse_pos
newtext = ""
alt = 0
if pos is not None:
(lat, lon) = self.coordinates(pos.x, pos.y)
newtext += "Cursor: %f %f (%s)" % (lat, lon, mp_util.latlon_to_grid((lat, lon)))
if state.elevation:
alt = self.ElevationMap.GetElevation(lat, lon)
if alt is not None:
newtext += " %.1fm" % alt
state.mt.set_download(state.download)
pending = 0
if state.download:
pending = state.mt.tiles_pending()
if pending:
newtext += " Map Downloading %u " % pending
if alt == -1:
newtext += " SRTM Downloading "
newtext += "\n"
if self.click_pos is not None:
newtext += "Click: %f %f (%s %s) (%s)" % (
self.click_pos[0],
self.click_pos[1],
mp_util.degrees_to_dms(self.click_pos[0]),
mp_util.degrees_to_dms(self.click_pos[1]),
mp_util.latlon_to_grid(self.click_pos),
)
if self.last_click_pos is not None:
distance = mp_util.gps_distance(
self.last_click_pos[0], self.last_click_pos[1], self.click_pos[0], self.click_pos[1]
)
bearing = mp_util.gps_bearing(
self.last_click_pos[0], self.last_click_pos[1], self.click_pos[0], self.click_pos[1]
)
newtext += " Distance: %.1fm Bearing %.1f" % (distance, bearing)
if newtext != state.oldtext:
self.position.Clear()
self.position.WriteText(newtext)
state.oldtext = newtext
def update(self, pkt, tnow):
dt = tnow - self.last_time
if dt < 0 or dt > 10:
self.last_time = tnow
return
if dt < 0.1:
return
self.last_time = tnow
dist = mp_util.gps_distance(self.pkt.lat*1e-7, self.pkt.lon*1e-7,
pkt.lat*1e-7, pkt.lon*1e-7)
if dist > 0.01:
heading = mp_util.gps_bearing(self.pkt.lat*1e-7, self.pkt.lon*1e-7,
pkt.lat*1e-7, pkt.lon*1e-7)
spd = dist / dt
pkt.heading = int(heading*100)
new_vel = int(spd * 100)
#print(pkt.ICAO_address, new_vel*0.01, pkt.hor_velocity*0.01)
pkt.hor_velocity = new_vel
if pkt.hor_velocity > 65535:
pkt.hor_velocity = 65535
self.pkt = pkt
def run(self):
'''Main command loop for the mission'''
try:
print("Traveling to: %s\n" % (str(self.next_wp)))
self.predive_check(
) # checks that auv is ready to dive, throws exception if not
# Distance to the next waypoint
self.distance_to_waypoint = mp_util.gps_distance(
self.lat, self.lon, self.next_wp[0], self.next_wp[1])
# Offset in degrees from the direction of the next waypoint
self.offset_from_intended_heading = mp_util.gps_bearing(
self.lat, self.lon, self.next_wp[0], self.next_wp[1])
# Spins the AUV towards the correct direction
self.orient_heading(self.offset_from_intended_heading)
# self.dive()
self.underwater_traverse(self.distance_to_waypoint)
# self.surface()
# Tells this function to run again, this implements the loop functionality
self.continue_mission()
except HardwareError:
print(HardwareError.message)
print('Hardware Error')
return self.go_home()
except ValueError:
print(ValueError)
print('Value Error')
return self.go_home()
return
def coord_to_pixel(self, lat, lon, width, ground_width, lat2, lon2):
'''return pixel coordinate (px,py) for position (lat2,lon2)
in an area image. Note that the results are relative to top,left
and may be outside the image
ground_width is with at lat,lon
px is pixel coord to the right from top,left
py is pixel coord down from top left
'''
pixel_width_equator = (ground_width / float(width)) / cos(radians(lat))
latr = radians(lat)
lat2r = radians(lat2)
C = mp_util.radius_of_earth / pixel_width_equator
y = C * (log(abs(1.0/cos(latr) + tan(latr))) - log(abs(1.0/cos(lat2r) + tan(lat2r))))
y = int(y+0.5)
dx = mp_util.gps_distance(lat2, lon, lat2, lon2)
if mp_util.gps_bearing(lat2, lon, lat2, lon2) > 180:
dx = -dx
x = int(0.5 + dx / (pixel_width_equator * cos(radians(lat2))))
return (x,y)
def update(self, pkt, tnow):
dt = tnow - self.last_time
if dt < 0 or dt > 10:
self.last_time = tnow
return
if dt < 0.1:
return
self.last_time = tnow
dist = mp_util.gps_distance(self.pkt.lat * 1e-7, self.pkt.lon * 1e-7,
pkt.lat * 1e-7, pkt.lon * 1e-7)
if dist > 0.01:
heading = mp_util.gps_bearing(self.pkt.lat * 1e-7,
self.pkt.lon * 1e-7, pkt.lat * 1e-7,
pkt.lon * 1e-7)
spd = dist / dt
pkt.heading = int(heading * 100)
new_vel = int(spd * 100)
#print(pkt.ICAO_address, new_vel*0.01, pkt.hor_velocity*0.01)
pkt.hor_velocity = new_vel
if pkt.hor_velocity > 65535:
pkt.hor_velocity = 65535
self.pkt = pkt
def find_best_image(images, lat, lon, alt, roll, pitch, yaw):
dist_margin = 20.0
alt_margin = 10.0
roll_margin = 10.0
pitch_margin = 10.0
yaw_margin = 10.0
for img in images:
pos = img.pos
dist = mp_util.gps_distance(lat, lon, pos.lat, pos.lon)
if dist > dist_margin:
continue
if abs(alt - pos.altitude) > alt_margin:
continue
if abs(roll - pos.roll) > roll_margin:
continue
if abs(pitch - pos.pitch) > pitch_margin:
continue
if abs(yaw - pos.yaw) > yaw_margin:
continue
return img
return None
def wp_slope(self):
'''show slope of waypoints'''
last_w = None
for i in range(1, self.wploader.count()):
w = self.wploader.wp(i)
if w.command not in [
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LAND
]:
continue
if last_w is not None:
if last_w.frame != w.frame:
print("WARNING: frame change %u -> %u at %u" %
(last_w.frame, w.frame, i))
delta_alt = last_w.z - w.z
if delta_alt == 0:
slope = "Level"
else:
delta_xy = mp_util.gps_distance(w.x, w.y, last_w.x,
last_w.y)
slope = "%.1f" % (delta_xy / delta_alt)
print("WP%u: slope %s" % (i, slope))
last_w = w
def draw(self, img, pixmapper, bounds):
'''draw a polygon on the image'''
(x,y,w,h) = bounds
spacing = 1000
while True:
start = mp_util.latlon_round((x,y), spacing)
dist = mp_util.gps_distance(x,y,x+w,y+h)
count = int(dist / spacing)
if count < 2:
spacing /= 10
elif count > 50:
spacing *= 10
else:
break
for i in range(count*2+2):
pos1 = mp_util.gps_newpos(start[0], start[1], 90, i*spacing)
pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 0, 3*count*spacing)
self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
pos1 = mp_util.gps_newpos(start[0], start[1], 0, i*spacing)
pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 90, 3*count*spacing)
self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
def cmd_underwater(self):
mav.set_mode_manual()
rc.set_mode_manual()
mav.motors_armed_wait()
#set the apm mav_type
mav.mode_mapping()
if self.predive_check() is not True:
return "Insufficient Battery"
self.distance_to_waypoint = mp_util.gps_distance(
self.lat, self.lon, self.next_wp.MAVLink_mission_item_message.x,
self.next_wp.MAVLink_mission_item_message.y)
self.intended_heading = mp_util.gps_bearing(
self.lat, self.lon, self.next_wp.MAVLink_mission_item_message.x,
self.next_wp.MAVLink_mission_item_message.y)
self.orient_heading(self.intended_heading)
self.pollution_array = None
#x = lat, y = lng
self.dive([self.lng, self.lat], [
self.next_wp.MAVLink_mission_item_message.y,
self.next_wp.MAVLink_mission_item_message.x
], self.distance_to_waypoint, self.intended_heading, 1, 1)
self.underwater_traverse([self.lng, self.lat], [
self.next_wp.MAVLink_mission_item_message.y,
self.next_wp.MAVLink_mission_item_message.x
], self.distance_to_waypoint, heading)
self.surface()
return
def go_home(self):
'''returns to home coordinate'''
print('Returning Home\n')
self.module('rc').override = [
1500, 1700, 1500, 1500, 1500, 1500, 1500, 1500
]
self.module('rc').send_rc_override()
sleep(2)
# Distance to the next waypoint
self.distance_to_waypoint = mp_util.gps_distance(
self.lat, self.lon, self.home[lat], self.home[lng])
# Offset in degrees from the direction of the next waypoint
self.offset_from_intended_heading = mp_util.gps_bearing(
self.lat, self.lon, self.home[lat], self.home[lng])
# Spins the AUV towards the correct direction
self.orient_heading(self.offset_from_intended_heading)
self.module('rc').stop()
self.command_queue.clear()
self.end_time = 0
self.command_queue.append(["f", 1650, self.distance_to_waypoint])
def mavflightview_show(path, wp, fen, options, title=None):
if not title:
title='MAVFlightView'
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
def distance_from(self, lat, lon):
'''get distance from a point'''
lat1 = self.pkt['I105']['Lat']['val']
lon1 = self.pkt['I105']['Lon']['val']
return mp_util.gps_distance(lat1, lon1, lat, lon)
def cmd_wp_movemulti(self, args):
'''handle wp move of multiple waypoints'''
if len(args) < 3:
print("usage: wp movemulti WPNUM WPSTART WPEND <rotation>")
return
idx = int(args[0])
if idx < 1 or idx > self.wploader.count():
print("Invalid wp number %u" % idx)
return
wpstart = int(args[1])
if wpstart < 1 or wpstart > self.wploader.count():
print("Invalid wp number %u" % wpstart)
return
wpend = int(args[2])
if wpend < 1 or wpend > self.wploader.count():
print("Invalid wp number %u" % wpend)
return
if idx < wpstart or idx > wpend:
print("WPNUM must be between WPSTART and WPEND")
return
# optional rotation about center point
if len(args) > 3:
rotation = float(args[3])
else:
rotation = 0
try:
latlon = self.module('map').click_position
except Exception:
print("No map available")
return
if latlon is None:
print("No map click position available")
return
wp = self.wploader.wp(idx)
if not self.wploader.is_location_command(wp.command):
print("WP must be a location command")
return
(lat, lon) = latlon
distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon)
for wpnum in range(wpstart, wpend+1):
wp = self.wploader.wp(wpnum)
if not self.wploader.is_location_command(wp.command):
continue
(newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing, distance)
if wpnum != idx and rotation != 0:
# add in rotation
d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
(newlat, newlon) = mp_util.gps_newpos(lat, lon, b2+rotation, d2)
if getattr(self.console, 'ElevationMap', None) is not None and wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT:
alt1 = self.console.ElevationMap.GetElevation(newlat, newlon)
alt2 = self.console.ElevationMap.GetElevation(wp.x, wp.y)
if alt1 is not None and alt2 is not None:
wp.z += alt1 - alt2
wp.x = newlat
wp.y = newlon
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, wpnum)
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(self.target_system,
self.target_component,
wpstart, wpend+1)
print("Moved WPs %u:%u to %f, %f rotation=%.1f" % (wpstart, wpend, lat, lon, rotation))
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW','GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW','GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1'])
if len(types) == 1:
types.extend(['GPS','GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if lat != 0 or lng != 0:
if getattr(mlog, 'flightmode','') in colourmap:
colour = colourmap[mlog.flightmode]
(r,g,b) = colour
(r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r,g,b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename, wp.polygon(), layer='Mission',
linewidth=2, colour=(255,255,255))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
def mavlink_packet(self, m):
"""handle an incoming mavlink packet"""
if m.get_type() == "HEARTBEAT":
from pymavlink import mavutil
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
self.vehicle_type_name = "plane"
elif m.type in [
mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
mavutil.mavlink.MAV_TYPE_SUBMARINE,
]:
self.vehicle_type_name = "rover"
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_COAXIAL,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER,
mavutil.mavlink.MAV_TYPE_HELICOPTER,
]:
self.vehicle_type_name = "copter"
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
self.vehicle_type_name = "antenna"
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = "Vehicle%u" % m.get_srcSystem()
if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon("Sim" + vehicle, "green")
self.mpstate.map.set_position(
"Sim" + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)
)
if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon("AHRS2" + vehicle, "blue")
self.mpstate.map.set_position(
"AHRS2" + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)
)
if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon("GPS" + vehicle, "blue")
self.mpstate.map.set_position("GPS" + vehicle, (lat, lon), rotation=m.cog * 0.01)
if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon("GPS2" + vehicle, "green")
self.mpstate.map.set_position("GPS2" + vehicle, (lat, lon), rotation=m.cog * 0.01)
if m.get_type() == "GLOBAL_POSITION_INT":
(self.lat, self.lon, self.heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01)
if self.lat != 0 or self.lon != 0:
self.create_vehicle_icon("Pos" + vehicle, "red", follow=True)
self.mpstate.map.set_position("Pos" + vehicle, (self.lat, self.lon), rotation=self.heading)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
trajectory = [(self.lat, self.lon), mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist)]
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon(
"trajectory", trajectory, layer="Trajectory", linewidth=2, colour=(255, 0, 180)
)
)
else:
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer("Trajectory"))
# if the waypoints have changed, redisplay
last_wp_change = self.module("wp").wploader.last_change
if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1:
self.wp_change_time = last_wp_change
self.display_waypoints()
# this may have affected the landing lines from the rally points:
self.rally_change_time = time.time()
# if the fence has changed, redisplay
if self.fence_change_time != self.module("fence").fenceloader.last_change:
self.display_fence()
# if the rallypoints have changed, redisplay
if self.rally_change_time != self.module("rally").rallyloader.last_change:
self.rally_change_time = self.module("rally").rallyloader.last_change
icon = self.mpstate.map.icon("rallypoint.png")
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer("RallyPoints"))
for i in range(self.module("rally").rallyloader.rally_count()):
rp = self.module("rally").rallyloader.rally_point(i)
popup = MPMenuSubMenu(
"Popup",
items=[
MPMenuItem("Rally Remove", returnkey="popupRallyRemove"),
MPMenuItem("Rally Move", returnkey="popupRallyMove"),
],
)
self.mpstate.map.add_object(
mp_slipmap.SlipIcon(
"Rally %u" % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer="RallyPoints",
rotation=0,
follow=False,
popup_menu=popup,
)
)
loiter_rad = self.get_mav_param("WP_LOITER_RAD")
if self.map_settings.rallycircle:
self.mpstate.map.add_object(
mp_slipmap.SlipCircle(
"Rally Circ %u" % (i + 1),
"RallyPoints",
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
abs(loiter_rad),
(255, 255, 0),
2,
)
)
# draw a line between rally point and nearest landing point
nearest_land_wp = None
nearest_distance = 10000000.0
for j in range(self.module("wp").wploader.count()):
w = self.module("wp").wploader.wp(j)
if w.command == 21: # if landing waypoint
# get distance between rally point and this waypoint
dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7)
if dis < nearest_distance:
nearest_land_wp = w
nearest_distance = dis
if nearest_land_wp != None:
points = []
# tangential approach?
if self.get_mav_param("LAND_BREAK_PATH") == 0:
theta = math.degrees(math.atan(loiter_rad / nearest_distance))
tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad))
ral_bearing = mp_util.gps_bearing(
nearest_land_wp.x, nearest_land_wp.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7
)
points.append(
mp_util.gps_newpos(nearest_land_wp.x, nearest_land_wp.y, ral_bearing + theta, tan_dis)
)
else: # not tangential approach
points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))
points.append((nearest_land_wp.x, nearest_land_wp.y))
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon("Rally Land %u" % (i + 1), points, "RallyPoints", (255, 255, 0), 2)
)
# check for any events from the map
self.mpstate.map.check_events()
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
types = ['MISSION_ITEM', 'CMD']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW', 'GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'GPS'])
if len(types) == 2:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
if m.get_type() in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
instance = 0
if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
instance = 1
if m.get_type() == 'EKF1':
if opts.dualgps:
instance = 2
else:
instance = 1
if m.get_type() == 'AHR2':
if opts.dualgps:
instance = 2
else:
instance = 1
if abs(lat) > 0.01 or abs(lng) > 0.01:
if getattr(mlog, 'flightmode', '') in colourmap:
colour = colourmap[mlog.flightmode]
(r, g, b) = colour
(r, g, b) = (r + instance * 50, g + instance * 50,
b + instance * 50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
else:
point = (lat, lng)
while instance >= len(path):
path.append([])
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, filename),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs,
mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
def distance_from(self, lat, lon):
'''get distance from a point'''
lat1 = self.pkt['I105']['Lat']['val']
lon1 = self.pkt['I105']['Lon']['val']
return mp_util.gps_distance(lat1, lon1, lat, lon)
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
instances = {}
ekf_counter = 0
types = ["MISSION_ITEM", "CMD"]
if opts.types is not None:
types.extend(opts.types.split(","))
else:
types.extend(["GPS", "GLOBAL_POSITION_INT"])
if opts.rawgps or opts.dualgps:
types.extend(["GPS", "GPS_RAW_INT"])
if opts.rawgps2 or opts.dualgps:
types.extend(["GPS2_RAW", "GPS2"])
if opts.ekf:
types.extend(["EKF1", "GPS"])
if opts.ahr2:
types.extend(["AHR2", "AHRS2", "GPS"])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == "MISSION_ITEM":
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == "CMD":
m = mavutil.mavlink.MAVLink_mission_item_message(
0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0,
1,
m.Prm1,
m.Prm2,
m.Prm3,
m.Prm4,
m.Lat,
m.Lng,
m.Alt,
)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if type in ["GPS", "GPS2"]:
status = getattr(m, "Status", None)
if status is None:
status = getattr(m, "FixType", None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, "Lng", None)
if lng is None:
lng = getattr(m, "Lon", None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ["EKF1", "ANU1"]:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % opts.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ["ANU5"]:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ["AHR2", "POS", "CHEK"]:
(lat, lng) = (m.Lat, m.Lng)
elif type == "AHRS2":
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
fmode = getattr(mlog, "flightmode", "")
if fmode in colourmap:
colour = colourmap[fmode]
else:
colour = colourmap["UNKNOWN"]
(r, g, b) = colour
(r, g, b) = (r + instance * 80, g + instance * 50, b + instance * 70)
if r > 255:
r = 205
if g > 255:
g = 205
if g < 0:
g = 0
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
if opts.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[type] > 1.0 / opts.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3])
while (
mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20
or mp_util.gps_distance(lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20
):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon(
"FlightPath[%u]-%s" % (i, filename), path[i], layer="FlightPath", linewidth=2, colour=(255, 0, 180)
)
)
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon(
"Mission-%s-%u" % (filename, i), plist[i], layer="Mission", linewidth=2, colour=(255, 255, 255)
)
)
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon(
"Fence-%s" % filename, fen.polygon(), layer="Fence", linewidth=2, colour=(0, 255, 0)
)
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(
title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug,
)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(",")
lat = a[0]
lon = a[1]
icon = "flag.png"
if len(a) > 2:
icon = a[2] + ".png"
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon(
"icon - %s" % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False
)
)
def distance(self, lat, lon):
'''distance of this tile from a given lat/lon'''
(tlat, tlon) = self.coord((TILES_WIDTH / 2, TILES_HEIGHT / 2))
return mp_util.gps_distance(lat, lon, tlat, tlon)
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" ] 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 cmd_wp_movemulti(self, args):
'''handle wp move of multiple waypoints'''
if len(args) < 4:
print(
"usage: wp movemulti WPNUM WPSTART WPEND <rotation> <newLocation>"
)
return
idx = int(args[0])
if idx < 1 or idx > self.wploader.count():
print("Invalid wp number %u" % idx)
return
wpstart = int(args[1])
if wpstart < 1 or wpstart > self.wploader.count():
print("Invalid wp number %u" % wpstart)
return
wpend = int(args[2])
if wpend < 1 or wpend > self.wploader.count():
print("Invalid wp number %u" % wpend)
return
if idx < wpstart or idx > wpend:
print("WPNUM must be between WPSTART and WPEND")
return
# optional rotation about center point
if len(args) > 3:
rotation = float(args[3])
else:
rotation = 0
# optional --> Override mouse click and use a waypoint for the move location instead
if len(args) > 4:
wpmove = int(args[4])
if wpmove < 0 or wpmove > self.wploader.count():
print("Invalid wp number %u" % wpmove)
return
wp = self.wploader.wp(wpmove)
latlon = (wp.x, wp.y)
else:
try:
latlon = self.module('map').click_position
except Exception:
print("No map available")
return
if latlon is None:
print("No map click position available")
return
wp = self.wploader.wp(idx)
if not self.wploader.is_location_command(wp.command):
print("WP must be a location command")
return
(lat, lon) = latlon
distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon)
for wpnum in range(wpstart, wpend + 1):
wp = self.wploader.wp(wpnum)
if not self.wploader.is_location_command(wp.command):
continue
(newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing,
distance)
if wpnum != idx and rotation != 0:
# add in rotation
d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
(newlat, newlon) = mp_util.gps_newpos(lat, lon, b2 + rotation,
d2)
if getattr(
self.console, 'ElevationMap', None
) is not None and wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT:
alt1 = self.console.ElevationMap.GetElevation(newlat, newlon)
alt2 = self.console.ElevationMap.GetElevation(wp.x, wp.y)
if alt1 is not None and alt2 is not None:
wp.z += alt1 - alt2
wp.x = newlat
wp.y = newlon
wp.target_system = self.target_system
wp.target_component = self.target_component
self.wploader.set(wp, wpnum)
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system, self.target_component, wpstart, wpend + 1)
print("Moved WPs %u:%u to %f, %f rotation=%.1f" %
(wpstart, wpend, lat, lon, rotation))
def mavflightview_mav(mlog, options=None, title=None):
'''create a map for a log file'''
if not title:
title = 'MAVFlightView'
wp = mavwp.MAVWPLoader()
if options.mission is not None:
wp.load(options.mission)
fen = mavwp.MAVFenceLoader()
if options.fence is not None:
fen.load(options.fence)
path = [[]]
instances = {}
ekf_counter = 0
nkf_counter = 0
types = ['MISSION_ITEM', 'CMD']
if options.types is not None:
types.extend(options.types.split(','))
else:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
if options.rawgps or options.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if options.rawgps2 or options.dualgps:
types.extend(['GPS2_RAW', 'GPS2'])
if options.ekf:
types.extend(['EKF1', 'GPS'])
if options.nkf:
types.extend(['NKF1', 'GPS'])
if options.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
last_timestamps = {}
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
type = m.get_type()
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower(
) != options.mode.lower():
continue
if type in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif type in ['EKF1', 'ANU1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
ekf_counter += 1
if ekf_counter % options.ekf_sample != 0:
continue
(lat, lng) = pos
elif type in ['NKF1']:
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
nkf_counter += 1
if nkf_counter % options.nkf_sample != 0:
continue
(lat, lng) = pos
elif type in ['ANU5']:
(lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
elif type in ['AHR2', 'POS', 'CHEK']:
(lat, lng) = (m.Lat, m.Lng)
elif type == 'AHRS2':
(lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if type not in instances:
instances[type] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[type]
if abs(lat) > 0.01 or abs(lng) > 0.01:
colour = colour_for_point(mlog, (lat, lng), instance, options)
point = (lat, lng, colour)
if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[
type] > 1.0 / options.rate:
last_timestamps[type] = m._timestamp
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(
mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, title),
path[i],
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(
mp_slipmap.SlipPolygon('Mission-%s-%u' % (title, i),
plist[i],
layer='Mission',
linewidth=2,
colour=(255, 255, 255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title,
fen.polygon(),
layer='Fence',
linewidth=2,
colour=(0, 255, 0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat, lon), ground_width,
path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" %
(colour_source_min, colour_source_max))
def set_grad_dist(self):
'''fix up distance and gradient when changing cell values'''
home_def_alt = float(self.label_home_alt_value.GetLabel())
numrows = self.grid_mission.GetNumberRows()
for row in range(1, numrows):
command = self.grid_mission.GetCellValue(row, ME_COMMAND_COL)
command_prev = self.grid_mission.GetCellValue(
row - 1, ME_COMMAND_COL)
lat = float(self.grid_mission.GetCellValue(row, ME_LAT_COL))
lon = float(self.grid_mission.GetCellValue(row, ME_LON_COL))
if (lat == 0) and (lon == 0):
dist = 0.0
grad = 0.0
else:
if "NAV" in command:
row_prev = row - 1
while ("NAV" not in command_prev) and (row_prev > 0):
command_prev = self.grid_mission.GetCellValue(
row_prev - 1, ME_COMMAND_COL)
row_prev = row_prev - 1
prev_lat = float(
self.grid_mission.GetCellValue(row_prev, ME_LAT_COL))
prev_lon = float(
self.grid_mission.GetCellValue(row_prev, ME_LON_COL))
prev_alt = float(
self.grid_mission.GetCellValue(row_prev, ME_ALT_COL))
if (self.grid_mission.GetCellValue(row_prev,
ME_FRAME_COL) == "Rel"):
prev_alt = prev_alt + home_def_alt
elif (self.grid_mission.GetCellValue(
row_prev, ME_FRAME_COL) == "AGL"):
prev_alt = self.ElevationModel.GetElevation(
prev_lat, prev_lon) + prev_alt
while (prev_lat == 0) and (prev_lon
== 0) and (row_prev > 0):
prev_lat = float(
self.grid_mission.GetCellValue(
row_prev - 1, ME_LAT_COL))
prev_lon = float(
self.grid_mission.GetCellValue(
row_prev - 1, ME_LON_COL))
row_prev = row_prev - 1
dist = mp_util.gps_distance(lat, lon, prev_lat, prev_lon)
curr_alt = float(
self.grid_mission.GetCellValue(row, ME_ALT_COL))
if (self.grid_mission.GetCellValue(row,
ME_FRAME_COL) == "Rel"):
curr_alt = curr_alt + home_def_alt
elif (self.grid_mission.GetCellValue(
row, ME_FRAME_COL) == "AGL"):
curr_alt = curr_alt + self.ElevationModel.GetElevation(
lat, lon)
grad = math.atan2(curr_alt - prev_alt,
dist) * 180 / math.pi
else:
grad = 0.0
dist = 0.0
dist = format(dist, '.1f')
grad = format(grad, '.1f')
self.grid_mission.SetCellValue(row, ME_DIST_COL, str(dist))
self.grid_mission.SetCellValue(row, ME_ANGLE_COL, str(grad))
parser.add_option("--max-zoom", type='int', default=19, help="maximum tile zoom")
parser.add_option("--delay", type='float', default=1.0, help="tile download delay")
parser.add_option("--boundary", default=None, help="region boundary")
parser.add_option("--debug", action='store_true', default=False, help="show debug info")
(opts, args) = parser.parse_args()
lat = opts.lat
lon = opts.lon
ground_width = opts.width
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
bounds = mp_util.polygon_bounds(boundary)
lat = bounds[0]+bounds[2]
lon = bounds[1]
ground_width = max(mp_util.gps_distance(lat, lon, lat, lon+bounds[3]),
mp_util.gps_distance(lat, lon, lat-bounds[2], lon))
print lat, lon, ground_width
mt = MPTile(debug=opts.debug, service=opts.service,
tile_delay=opts.delay, max_zoom=opts.max_zoom)
if opts.zoom is None:
zooms = range(mt.min_zoom, mt.max_zoom+1)
else:
zooms = [opts.zoom]
for zoom in zooms:
tlist = mt.area_to_tile_list(lat, lon, width=1024, height=1024,
ground_width=ground_width, zoom=zoom)
print("zoom %u needs %u tiles" % (zoom, len(tlist)))
for tile in tlist:
mt.load_tile(tile)
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
path = []
while True:
m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT', 'GPS_RAW_INT', 'GPS'])
if m is None:
break
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
continue
if m.get_type() == 'GLOBAL_POSITION_INT' and opts.rawgps:
continue
if m.get_type() == 'GPS_RAW_INT' and not opts.rawgps:
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() == 'GPS':
if m.Status < 2:
continue
# flash log
lat = m.Lat
lng = m.Lng
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
if lat != 0 or lng != 0:
if getattr(mlog, 'flightmode','') in colourmap:
point = (lat, lng, colourmap[mlog.flightmode])
else:
point = (lat, lng)
path.append(point)
if len(path) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path)
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath',
linewidth=2, colour=(255,0,180))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission',
linewidth=2, colour=(255,255,255))
else:
mission_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_obj, mission_obj)
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
from MAVProxy.modules.mavproxy_map import mp_slipmap
mtype = m.get_type()
sysid = m.get_srcSystem()
if mtype == "HEARTBEAT":
vname = 'plane'
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
vname = 'plane'
elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]:
vname = 'rover'
elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]:
vname = 'sub'
elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]:
vname = 'boat'
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER
]:
vname = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]:
vname = 'singlecopter'
elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
vname = 'heli'
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
vname = 'antenna'
self.vehicle_type_by_sysid[sysid] = vname
if not sysid in self.vehicle_type_by_sysid:
self.vehicle_type_by_sysid[sysid] = 'plane'
self.vehicle_type_name = self.vehicle_type_by_sysid[sysid]
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()
if mtype == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.map.set_position('Sim' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
self.map.set_position('AHRS2' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "AHRS3" and self.map_settings.showahrs3pos:
self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
self.map.set_position('AHRS3' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
if m.vel > 300 or 'ATTITUDE' not in self.master.messages:
cog = m.cog * 0.01
else:
cog = math.degrees(self.master.messages['ATTITUDE'].yaw)
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.map.set_position('GPS' + vehicle, (lat, lon),
rotation=cog)
elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.map.set_position('GPS2' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
elif mtype == 'GLOBAL_POSITION_INT' and self.map_settings.showahrspos:
(lat, lon, heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7,
m.hdg * 0.01)
self.lat_lon[m.get_srcSystem()] = (lat, lon)
if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3:
self.have_global_position = True
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
if len(self.vehicle_type_by_sysid) > 1:
label = str(sysid)
else:
label = None
self.map.set_position('Pos' + vehicle, (lat, lon),
rotation=heading,
label=label,
colour=(255, 255, 255))
self.map.set_follow_object('Pos' + vehicle,
self.is_primary_vehicle(m))
elif mtype == 'HOME_POSITION':
(lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7)
icon = self.map.icon('home.png')
self.map.add_object(
mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon),
icon,
layer=3,
rotation=0,
follow=False))
elif mtype == "NAV_CONTROLLER_OUTPUT":
tlayer = 'Trajectory%u' % m.get_srcSystem()
if (self.master.flightmode in [
"AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
"QLAND", "FOLLOW"
] and m.get_srcSystem() in self.lat_lon):
(lat, lon) = self.lat_lon[m.get_srcSystem()]
trajectory = [(lat, lon),
mp_util.gps_newpos(lat, lon, m.target_bearing,
m.wp_dist)]
self.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer=tlayer,
linewidth=2,
colour=(255, 0, 180)))
self.trajectory_layers.add(tlayer)
else:
if tlayer in self.trajectory_layers:
self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
self.trajectory_layers.remove(tlayer)
elif mtype == "POSITION_TARGET_GLOBAL_INT":
# FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL?
if not m.get_srcSystem() in self.lat_lon:
return
tlayer = 'PostionTarget%u' % m.get_srcSystem()
(lat, lon) = self.lat_lon[m.get_srcSystem()]
if (self.master.flightmode in [
"AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
"QLAND", "FOLLOW"
]):
lat_float = m.lat_int * 1e-7
lon_float = m.lon_int * 1e-7
vec = [(lat_float, lon_float), (lat, lon)]
self.map.add_object(
mp_slipmap.SlipPolygon('position_target',
vec,
layer=tlayer,
linewidth=2,
colour=(0, 255, 0)))
else:
self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
if not self.is_primary_vehicle(m):
# the rest should only be done for the primary vehicle
return
# if the waypoints have changed, redisplay
last_wp_change = self.module('wp').wploader.last_change
if self.wp_change_time != last_wp_change and abs(time.time() -
last_wp_change) > 1:
self.wp_change_time = last_wp_change
self.display_waypoints()
#this may have affected the landing lines from the rally points:
self.rally_change_time = time.time()
# if the fence has changed, redisplay
if (self.module('fence') and self.fence_change_time !=
self.module('fence').fenceloader.last_change):
self.display_fence()
# if the rallypoints have changed, redisplay
if (self.module('rally') and
self.rally_change_time != self.module('rally').last_change()):
self.rally_change_time = self.module('rally').last_change()
icon = self.map.icon('rallypoint.png')
self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
for i in range(self.module('rally').rally_count()):
rp = self.module('rally').rally_point(i)
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Rally Remove',
returnkey='popupRallyRemove'),
MPMenuItem('Rally Move', returnkey='popupRallyMove')
])
self.map.add_object(
mp_slipmap.SlipIcon('Rally %u' % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer='RallyPoints',
rotation=0,
follow=False,
popup_menu=popup))
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
if self.map_settings.rallycircle:
self.map.add_object(
mp_slipmap.SlipCircle(
'Rally Circ %u' % (i + 1),
'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
loiter_rad, (255, 255, 0),
2,
arrow=self.map_settings.showdirection))
#draw a line between rally point and nearest landing point
nearest_land_wp = None
nearest_distance = 10000000.0
for j in range(self.module('wp').wploader.count()):
w = self.module('wp').wploader.wp(j)
if (w.command == 21): #if landing waypoint
#get distance between rally point and this waypoint
dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
rp.lng * 1.0e-7)
if (dis < nearest_distance):
nearest_land_wp = w
nearest_distance = dis
if nearest_land_wp is not None:
points = []
#tangential approach?
if self.get_mav_param('LAND_BREAK_PATH') == 0:
theta = math.degrees(
math.atan(loiter_rad / nearest_distance))
tan_dis = math.sqrt(nearest_distance *
nearest_distance -
(loiter_rad * loiter_rad))
ral_bearing = mp_util.gps_bearing(
nearest_land_wp.x, nearest_land_wp.y,
rp.lat * 1.0e-7, rp.lng * 1.0e-7)
points.append(
mp_util.gps_newpos(nearest_land_wp.x,
nearest_land_wp.y,
ral_bearing + theta, tan_dis))
else: #not tangential approach
points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))
points.append((nearest_land_wp.x, nearest_land_wp.y))
self.map.add_object(
mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
points, 'RallyPoints',
(255, 255, 0), 2))
# check for any events from the map
self.map.check_events()
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
path = []
path2 = []
types = ['MISSION_ITEM']
if opts.rawgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2:
types.extend(['GPS2_RAW', 'GPS2'])
if opts.dualgps:
types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
if len(types) == 1:
types.extend(['GPS', 'GLOBAL_POSITION_INT'])
while True:
m = mlog.recv_match(type=types)
if m is None:
break
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
if m.get_type() in ['GPS', 'GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
secondary = opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']
if lat != 0 or lng != 0:
if getattr(mlog, 'flightmode', '') in colourmap:
colour = colourmap[mlog.flightmode]
if secondary:
(r, g, b) = colour
(r, g, b) = (r + 50, g + 50, b + 50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r, g, b)
point = (lat, lng, colour)
else:
point = (lat, lng)
if secondary:
path2.append(point)
else:
path.append(point)
if len(path) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path)
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_obj = mp_slipmap.SlipPolygon('FlightPath-%s' % filename,
path,
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180))
if len(path2) != 0:
path2_obj = mp_slipmap.SlipPolygon('FlightPath2-%s' % filename,
path2,
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180))
else:
path2_obj = None
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename,
wp.polygon(),
layer='Mission',
linewidth=2,
colour=(255, 255, 255))
else:
mission_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon),
ground_width,
path_obj,
mission_obj,
path2_obj=path2_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
map.add_object(path_obj)
if path2_obj is not None:
map.add_object(path2_obj)
if mission_obj is not None:
map.add_object(mission_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
parser.add_option("--max-zoom", type='int', default=19, help="maximum tile zoom")
parser.add_option("--delay", type='float', default=1.0, help="tile download delay")
parser.add_option("--boundary", default=None, help="region boundary")
parser.add_option("--debug", action='store_true', default=False, help="show debug info")
(opts, args) = parser.parse_args()
lat = opts.lat
lon = opts.lon
ground_width = opts.width
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
bounds = mp_util.polygon_bounds(boundary)
lat = bounds[0]+bounds[2]
lon = bounds[1]
ground_width = max(mp_util.gps_distance(lat, lon, lat, lon+bounds[3]),
mp_util.gps_distance(lat, lon, lat-bounds[2], lon))
print(lat, lon, ground_width)
mt = MPTile(debug=opts.debug, service=opts.service,
tile_delay=opts.delay, max_zoom=opts.max_zoom)
if opts.zoom is None:
zooms = range(mt.min_zoom, mt.max_zoom+1)
else:
zooms = [opts.zoom]
for zoom in zooms:
tlist = mt.area_to_tile_list(lat, lon, width=1024, height=1024,
ground_width=ground_width, zoom=zoom)
print("zoom %u needs %u tiles" % (zoom, len(tlist)))
for tile in tlist:
mt.load_tile(tile)
def distance(self, lat, lon): '''distance of this tile from a given lat/lon''' (tlat, tlon) = self.coord((TILES_WIDTH/2,TILES_HEIGHT/2)) return mp_util.gps_distance(lat, lon, tlat, tlon)
parser.add_option("--delay", type="float", default=1.0, help="tile download delay")
parser.add_option("--boundary", default=None, help="region boundary")
parser.add_option("--debug", action="store_true", default=False, help="show debug info")
(opts, args) = parser.parse_args()
lat = opts.lat
lon = opts.lon
ground_width = opts.width
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
bounds = mp_util.polygon_bounds(boundary)
lat = bounds[0] + bounds[2]
lon = bounds[1]
ground_width = max(
mp_util.gps_distance(lat, lon, lat, lon + bounds[3]), mp_util.gps_distance(lat, lon, lat - bounds[2], lon)
)
print lat, lon, ground_width
mt = MPTile(debug=opts.debug, service=opts.service, tile_delay=opts.delay, max_zoom=opts.max_zoom)
if opts.zoom is None:
zooms = range(mt.min_zoom, mt.max_zoom + 1)
else:
zooms = [opts.zoom]
for zoom in zooms:
tlist = mt.area_to_tile_list(lat, lon, width=1024, height=1024, ground_width=ground_width, zoom=zoom)
print ("zoom %u needs %u tiles" % (zoom, len(tlist)))
for tile in tlist:
mt.load_tile(tile)
while mt.tiles_pending() > 0:
time.sleep(2)
def mavflightview_show(path, wp, fen, options, title=None):
if not title:
title='MAVFlightView'
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=options.debug)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
source = getattr(options, "colour_source", "flightmode")
if source != "flightmode":
print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
fen = mavwp.MAVFenceLoader()
if opts.fence is not None:
fen.load(opts.fence)
path = [[]]
instances = {}
types = ['MISSION_ITEM','CMD']
if opts.types is not None:
types.extend(opts.types.split(','))
else:
types.extend(['GPS','GLOBAL_POSITION_INT'])
if opts.rawgps or opts.dualgps:
types.extend(['GPS', 'GPS_RAW_INT'])
if opts.rawgps2 or opts.dualgps:
types.extend(['GPS2_RAW','GPS2'])
if opts.ekf:
types.extend(['EKF1', 'GPS'])
if opts.ahr2:
types.extend(['AHR2', 'AHRS2', 'GPS'])
print("Looking for types %s" % str(types))
while True:
try:
m = mlog.recv_match(type=types)
if m is None:
break
except Exception:
break
if m.get_type() == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if m.get_type() == 'CMD':
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(opts.condition):
continue
if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
continue
if m.get_type() in ['GPS','GPS2']:
status = getattr(m, 'Status', None)
if status is None:
status = getattr(m, 'FixType', None)
if status is None:
print("Can't find status on GPS message")
print(m)
break
if status < 2:
continue
# flash log
lat = m.Lat
lng = getattr(m, 'Lng', None)
if lng is None:
lng = getattr(m, 'Lon', None)
if lng is None:
print("Can't find longitude on GPS message")
print(m)
break
elif m.get_type() == 'EKF1':
pos = mavextra.ekf1_pos(m)
if pos is None:
continue
(lat, lng) = pos
elif m.get_type() == 'AHR2':
(lat, lng) = (m.Lat, m.Lng)
elif m.get_type() == 'AHRS2':
(lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
else:
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
# automatically add new types to instances
if m.get_type() not in instances:
instances[m.get_type()] = len(instances)
while len(instances) >= len(path):
path.append([])
instance = instances[m.get_type()]
if abs(lat)>0.01 or abs(lng)>0.01:
if getattr(mlog, 'flightmode','') in colourmap:
colour = colourmap[mlog.flightmode]
(r,g,b) = colour
(r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
if r > 255:
r = 205
if g > 255:
g = 205
if b > 255:
b = 205
colour = (r,g,b)
point = (lat, lng, colour)
else:
point = (lat, lng)
path[instance].append(point)
if len(path[0]) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path[0])
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
fence = fen.polygon()
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
else:
global multi_map
if opts.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=opts.debug)
if opts.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
action='store_true',
default=False,
help="show debug info")
(opts, args) = parser.parse_args()
lat = opts.lat
lon = opts.lon
ground_width = opts.width
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
bounds = mp_util.polygon_bounds(boundary)
lat = bounds[0] + bounds[2]
lon = bounds[1]
ground_width = max(
mp_util.gps_distance(lat, lon, lat, lon + bounds[3]),
mp_util.gps_distance(lat, lon, lat - bounds[2], lon))
print(lat, lon, ground_width)
mt = MPTile(debug=opts.debug,
service=opts.service,
tile_delay=opts.delay,
max_zoom=opts.max_zoom)
if opts.zoom is None:
zooms = range(mt.min_zoom, mt.max_zoom + 1)
else:
zooms = [opts.zoom]
for zoom in zooms:
tlist = mt.area_to_tile_list(lat,
lon,
width=1024,
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 mavflightview_show(path, wp, fen, used_flightmodes, mav_type, options, instances, title=None, timelim_pipe=None):
if not title:
title='MAVFlightView'
boundary_path = []
for p in path[0]:
boundary_path.append((p[0],p[1]))
fence = fen.polygon()
if options.fencebounds:
for p in fence:
boundary_path.append((p[0],p[1]))
bounds = mp_util.polygon_bounds(boundary_path)
(lat, lon) = (bounds[0]+bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
ground_width += 10
path_objs = []
for i in range(len(path)):
if len(path[i]) != 0:
path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
linewidth=2, colour=(255,0,180)))
plist = wp.polygon_list()
mission_obj = None
if len(plist) > 0:
mission_obj = []
for i in range(len(plist)):
mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
linewidth=2, colour=(255,255,255)))
else:
mission_obj = None
if len(fence) > 1:
fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
linewidth=2, colour=(0,255,0))
else:
fence_obj = None
kml = getattr(options,'kml',None)
if kml is not None:
kml_objects = load_kml(kml)
else:
kml_objects = None
if options.imagefile:
create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj, kml_objects, used_flightmodes=used_flightmodes, mav_type=mav_type)
else:
global multi_map
if options.multi and multi_map is not None:
map = multi_map
else:
map = mp_slipmap.MPSlipMap(title=title,
service=options.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat, lon=lon,
debug=options.debug,
show_flightmode_legend=options.show_flightmode_legend,
timelim_pipe=timelim_pipe)
if options.multi:
multi_map = map
for path_obj in path_objs:
map.add_object(path_obj)
if mission_obj is not None:
display_waypoints(wp, map)
if fence_obj is not None:
map.add_object(fence_obj)
if kml_objects is not None:
for obj in kml_objects:
map.add_object(obj)
for flag in options.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
if options.colour_source == "flightmode":
tuples = [ (mode, colour_for_flightmode(mav_type, mode))
for mode in used_flightmodes.keys() ]
map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
elif options.colour_source == "type":
tuples = [ (t, map_colours[instances[t]]) for t in instances.keys() ]
map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
else:
print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
action='store_true',
default=False,
help="show debug info")
(opts, args) = parser.parse_args()
lat = opts.lat
lon = opts.lon
ground_width = opts.width
if opts.boundary:
boundary = mp_util.polygon_load(opts.boundary)
bounds = mp_util.polygon_bounds(boundary)
lat = bounds[0] + bounds[2]
lon = bounds[1]
ground_width = max(
mp_util.gps_distance(lat, lon, lat,
mp_util.wrap_180(lon + bounds[3])),
mp_util.gps_distance(lat, lon, lat - bounds[2], lon))
print(lat, lon, ground_width)
mt = MPTile(debug=opts.debug,
service=opts.service,
tile_delay=opts.delay,
max_zoom=opts.max_zoom)
if opts.zoom is None:
zooms = range(mt.min_zoom, mt.max_zoom + 1)
else:
zooms = [opts.zoom]
for zoom in zooms:
tlist = mt.area_to_tile_list(lat,
lon,
width=1024,
def mavlink_packet(self, m):
'''handle an incoming mavlink packet'''
if m.get_type() == "HEARTBEAT":
from pymavlink import mavutil
if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
self.vehicle_type_name = 'plane'
elif m.type in [
mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
mavutil.mavlink.MAV_TYPE_SUBMARINE
]:
self.vehicle_type_name = 'rover'
elif m.type in [
mavutil.mavlink.MAV_TYPE_QUADROTOR,
mavutil.mavlink.MAV_TYPE_COAXIAL,
mavutil.mavlink.MAV_TYPE_HEXAROTOR,
mavutil.mavlink.MAV_TYPE_OCTOROTOR,
mavutil.mavlink.MAV_TYPE_TRICOPTER,
mavutil.mavlink.MAV_TYPE_HELICOPTER
]:
self.vehicle_type_name = 'copter'
elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
self.vehicle_type_name = 'antenna'
# this is the beginnings of allowing support for multiple vehicles
# in the air at the same time
vehicle = 'Vehicle%u' % m.get_srcSystem()
if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
self.create_vehicle_icon('Sim' + vehicle, 'green')
self.mpstate.map.set_position('Sim' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
self.mpstate.map.set_position('AHRS2' + vehicle,
(m.lat * 1.0e-7, m.lng * 1.0e-7),
rotation=math.degrees(m.yaw))
if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS' + vehicle, 'blue')
self.mpstate.map.set_position('GPS' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
(lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
if lat != 0 or lon != 0:
self.create_vehicle_icon('GPS2' + vehicle, 'green')
self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon),
rotation=m.cog * 0.01)
if m.get_type() == 'GLOBAL_POSITION_INT':
(self.lat, self.lon, self.heading) = (m.lat * 1.0e-7,
m.lon * 1.0e-7, m.hdg * 0.01)
if self.lat != 0 or self.lon != 0:
self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
self.mpstate.map.set_position('Pos' + vehicle,
(self.lat, self.lon),
rotation=self.heading)
if m.get_type() == "NAV_CONTROLLER_OUTPUT":
if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
trajectory = [(self.lat, self.lon),
mp_util.gps_newpos(self.lat, self.lon,
m.target_bearing, m.wp_dist)]
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('trajectory',
trajectory,
layer='Trajectory',
linewidth=2,
colour=(255, 0, 180)))
else:
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('Trajectory'))
# if the waypoints have changed, redisplay
last_wp_change = self.module('wp').wploader.last_change
if self.wp_change_time != last_wp_change and abs(time.time() -
last_wp_change) > 1:
self.wp_change_time = last_wp_change
self.display_waypoints()
#this may have affected the landing lines from the rally points:
self.rally_change_time = time.time()
# if the fence has changed, redisplay
if self.fence_change_time != self.module(
'fence').fenceloader.last_change:
self.display_fence()
# if the rallypoints have changed, redisplay
if self.rally_change_time != self.module(
'rally').rallyloader.last_change:
self.rally_change_time = self.module(
'rally').rallyloader.last_change
icon = self.mpstate.map.icon('rallypoint.png')
self.mpstate.map.add_object(
mp_slipmap.SlipClearLayer('RallyPoints'))
for i in range(self.module('rally').rallyloader.rally_count()):
rp = self.module('rally').rallyloader.rally_point(i)
popup = MPMenuSubMenu(
'Popup',
items=[
MPMenuItem('Rally Remove',
returnkey='popupRallyRemove'),
MPMenuItem('Rally Move', returnkey='popupRallyMove')
])
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('Rally %u' % (i + 1),
(rp.lat * 1.0e-7, rp.lng * 1.0e-7),
icon,
layer='RallyPoints',
rotation=0,
follow=False,
popup_menu=popup))
loiter_rad = self.get_mav_param('WP_LOITER_RAD')
if self.map_settings.rallycircle:
self.mpstate.map.add_object(
mp_slipmap.SlipCircle(
'Rally Circ %u' % (i + 1),
'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
abs(loiter_rad), (255, 255, 0), 2))
#draw a line between rally point and nearest landing point
nearest_land_wp = None
nearest_distance = 10000000.0
for j in range(self.module('wp').wploader.count()):
w = self.module('wp').wploader.wp(j)
if (w.command == 21): #if landing waypoint
#get distance between rally point and this waypoint
dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
rp.lng * 1.0e-7)
if (dis < nearest_distance):
nearest_land_wp = w
nearest_distance = dis
if nearest_land_wp != None:
points = []
#tangential approach?
if self.get_mav_param('LAND_BREAK_PATH') == 0:
theta = math.degrees(
math.atan(loiter_rad / nearest_distance))
tan_dis = math.sqrt(nearest_distance *
nearest_distance -
(loiter_rad * loiter_rad))
ral_bearing = mp_util.gps_bearing(
nearest_land_wp.x, nearest_land_wp.y,
rp.lat * 1.0e-7, rp.lng * 1.0e-7)
points.append(
mp_util.gps_newpos(nearest_land_wp.x,
nearest_land_wp.y,
ral_bearing + theta, tan_dis))
else: #not tangential approach
points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))
points.append((nearest_land_wp.x, nearest_land_wp.y))
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
points, 'RallyPoints',
(255, 255, 0), 2))
# check for any events from the map
self.mpstate.map.check_events()
def mavflightview(filename):
print("Loading %s ..." % filename)
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
if opts.mission is not None:
wp.load(opts.mission)
path = []
while True:
m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT'])
if m is None:
break
if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition(
opts.condition):
if opts.mode is not None and mlog.flightmode.lower(
) != opts.mode.lower():
continue
lat = m.lat * 1.0e-7
lng = m.lon * 1.0e-7
if lat != 0 or lng != 0:
if mlog.flightmode in colourmap:
point = (lat, lng, colourmap[mlog.flightmode])
else:
point = (lat, lng)
path.append(point)
if m.get_type() == 'MISSION_ITEM':
wp.set(m, m.seq)
if len(path) == 0:
print("No points to plot")
return
bounds = mp_util.polygon_bounds(path)
(lat, lon) = (bounds[0] + bounds[2], bounds[1])
(lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
lon + bounds[3])
while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
>= ground_width - 20 or mp_util.gps_distance(
lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
ground_width += 10
path_obj = mp_slipmap.SlipPolygon('FlightPath',
path,
layer='FlightPath',
linewidth=2,
colour=(255, 0, 180))
mission = wp.polygon()
if len(mission) > 1:
mission_obj = mp_slipmap.SlipPolygon('Mission',
wp.polygon(),
layer='Mission',
linewidth=2,
colour=(255, 255, 255))
else:
mission_obj = None
if opts.imagefile:
create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj,
mission_obj)
else:
map = mp_slipmap.MPSlipMap(title=filename,
service=opts.service,
elevation=True,
width=600,
height=600,
ground_width=ground_width,
lat=lat,
lon=lon)
map.add_object(path_obj)
if mission_obj is not None:
map.add_object(mission_obj)
for flag in opts.flag:
a = flag.split(',')
lat = a[0]
lon = a[1]
icon = 'flag.png'
if len(a) > 2:
icon = a[2] + '.png'
icon = map.icon(icon)
map.add_object(
mp_slipmap.SlipIcon('icon - %s' % str(flag),
(float(lat), float(lon)),
icon,
layer=3,
rotation=0,
follow=False))
