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 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(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))