def drawing_update(self): '''update line drawing''' if self.draw_callback is None: return self.draw_line.append(self.click_position) if len(self.draw_line) > 1: self.mpstate.map.add_object(mp_slipmap.SlipPolygon('drawing', self.draw_line, layer='Drawing', linewidth=2, colour=(128,128,255)))
def display_fence(self): '''display the fence''' self.fence_change_time = self.module('fence').fenceloader.last_change points = self.module('fence').fenceloader.polygon() self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Fence')) if len(points) > 1: popup = MPMenuSubMenu('Popup', items=[MPMenuItem('FencePoint Remove', returnkey='popupFenceRemove'), MPMenuItem('FencePoint Move', returnkey='popupFenceMove')]) self.mpstate.map.add_object(mp_slipmap.SlipPolygon('Fence', points, layer=1, linewidth=2, colour=(0,255,0), popup_menu=popup))
def mavlink_packet(m): '''handle an incoming mavlink packet''' state = mpstate.map_state if m.get_type() == 'GPS_RAW': (state.lat, state.lon) = (m.lat, m.lon) elif m.get_type() == 'GPS_RAW_INT': (state.lat, state.lon) = (m.lat / 1.0e7, m.lon / 1.0e7) elif m.get_type() == "VFR_HUD": state.heading = m.heading else: return 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 points = mpstate.status.wploader.polygon() if len(points) > 1: mpstate.map.add_object( mp_slipmap.SlipPolygon('mission', points, layer=1, linewidth=2, colour=(255, 255, 255))) # 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 display_waypoints(): '''display the waypoints''' polygons = mpstate.status.wploader.polygon_list() mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission')) for i in range(len(polygons)): p = polygons[i] if len(p) > 1: mpstate.map.add_object( mp_slipmap.SlipPolygon('mission%u' % i, p, layer='Mission', linewidth=2, colour=(255, 255, 255)))
def display_waypoints(self): '''display the waypoints''' self.mission_list = self.module('wp').wploader.view_list() polygons = self.module('wp').wploader.polygon_list() self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Mission')) for i in range(len(polygons)): p = polygons[i] if len(p) > 1: popup = MPMenuSubMenu('Popup', items=[MPMenuItem('Set', returnkey='popupMissionSet'), MPMenuItem('WP Remove', returnkey='popupMissionRemove'), MPMenuItem('WP Move', returnkey='popupMissionMove')]) self.mpstate.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p, layer='Mission', linewidth=2, colour=(255,255,255), popup_menu=popup)) labeled_wps = {} for i in range(len(self.mission_list)): next_list = self.mission_list[i] for j in range(len(next_list)): #label already printed for this wp? if (next_list[j] not in labeled_wps): self.mpstate.map.add_object(mp_slipmap.SlipLabel( 'miss_cmd %u/%u' % (i,j), polygons[i][j], str(next_list[j]), 'Mission', colour=(0,255,255))) labeled_wps[next_list[j]] = (i,j)
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 == "AUTO": 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 cmd_camera(args): '''camera commands''' state = mpstate.camera_state if args[0] == "start": state.capture_count = 0 state.error_count = 0 state.error_msg = None state.running = True if state.capture_thread is None: state.capture_thread = start_thread(capture_thread) state.save_thread = start_thread(save_thread) state.scan_thread1 = start_thread(scan_thread) state.scan_thread2 = start_thread(scan_thread) state.transmit_thread = start_thread(transmit_thread) print("started camera running") elif args[0] == "stop": state.running = False print("stopped camera capture") elif args[0] == "status": print( "Cap imgs:%u err:%u scan:%u regions:%u jsize:%.0f xmitq:%u/%u lst:%u sq:%.1f eff:%.2f" % (state.capture_count, state.error_count, state.scan_count, state.region_count, state.jpeg_size, state.xmit_queue, state.xmit_queue2, state.frame_loss, state.scan_queue.qsize(), state.efficiency)) if state.bsend2: state.bsend2.report(detailed=False) elif args[0] == "queue": print("scan %u save %u transmit %u eff %.1f bw %.1f rtt %.1f" % (state.scan_queue.qsize(), state.save_queue.qsize(), state.transmit_queue.qsize(), state.efficiency, state.bandwidth_used, state.rtt_estimate)) elif args[0] == "view": if mpstate.map is None: print("Please load map module first") return if not state.viewing: print("Starting image viewer") if state.view_thread is None: state.view_thread = start_thread(view_thread) state.viewing = True elif args[0] == "noview": if state.viewing: print("Stopping image viewer") state.viewing = False elif args[0] == "set": if len(args) < 3: state.settings.show_all() else: state.settings.set(args[1], args[2]) elif args[0] == "boundary": if len(args) != 2: print("boundary=%s" % state.boundary) else: state.boundary = args[1] state.boundary_polygon = cuav_util.polygon_load(state.boundary) if mpstate.map is not None: mpstate.map.add_object( mp_slipmap.SlipPolygon('boundary', state.boundary_polygon, layer=1, linewidth=2, colour=(0, 0, 255))) else: print("usage: camera <start|stop|status|view|noview|boundary|set>")
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()