def nofly_add(self): '''add a square flight exclusion zone''' latlon = self.mpstate.click_location if latlon is None: print("No position chosen") return loader = self.wploader (center_lat, center_lon) = latlon points = [] points.append(mp_util.gps_offset(center_lat, center_lon, -25, 25)) points.append(mp_util.gps_offset(center_lat, center_lon, 25, 25)) points.append(mp_util.gps_offset(center_lat, center_lon, 25, -25)) points.append(mp_util.gps_offset(center_lat, center_lon, -25, -25)) start_idx = loader.count() for p in points: wp = mavutil.mavlink.MAVLink_mission_item_message( 0, 0, 0, 0, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0, 1, 4, 0, 0, 0, p[0], p[1], 0) loader.add(wp) self.loading_waypoints = True self.loading_waypoint_lasttime = time.time() self.master.mav.mission_write_partial_list_send( self.target_system, self.target_component, start_idx, start_idx + 4) print("Added nofly zone")
def nofly_add(self): '''add a square flight exclusion zone''' try: latlon = self.module('map').click_position except Exception: print("No position chosen") return loader = self.wploader (center_lat, center_lon) = latlon points = [] points.append(mp_util.gps_offset(center_lat, center_lon, -25, 25)) points.append(mp_util.gps_offset(center_lat, center_lon, 25, 25)) points.append(mp_util.gps_offset(center_lat, center_lon, 25, -25)) points.append(mp_util.gps_offset(center_lat, center_lon, -25, -25)) start_idx = loader.count() for p in points: wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, 0, mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0, 1, 4, 0, 0, 0, p[0], p[1], 0) loader.add(wp) self.loading_waypoints = True self.loading_waypoint_lasttime = time.time() self.master.mav.mission_write_partial_list_send(self.target_system, self.target_component, start_idx, start_idx+4) print("Added nofly zone")
def mavlink_packet(self, m): '''handle an incoming mavlink packet''' if not self.mpstate.map: # don't draw if no map return if m.get_type() != 'GIMBAL_REPORT': return needed = ['ATTITUDE', 'GLOBAL_POSITION_INT'] for n in needed: if not n in self.master.messages: return # clear the camera icon self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView')) gpi = self.master.messages['GLOBAL_POSITION_INT'] att = self.master.messages['ATTITUDE'] vehicle_dcm = Matrix3() vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw) rotmat_copter_gimbal = Matrix3() rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el, m.joint_az) gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal lat = gpi.lat * 1.0e-7 lon = gpi.lon * 1.0e-7 alt = gpi.relative_alt * 1.0e-3 # ground plane ground_plane = Plane() # the position of the camera in the air, remembering its a right # hand coordinate system, so +ve z is down camera_point = Vector3(0, 0, -alt) # get view point of camera when not rotated view_point = Vector3(1, 0, 0) # rotate view_point to form current view vector rot_point = gimbal_dcm * view_point # a line from the camera to the ground line = Line(camera_point, rot_point) # find the intersection with the ground pt = line.plane_intersection(ground_plane, forward_only=True) if pt is None: # its pointing up into the sky return None (view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x) icon = self.mpstate.map.icon('camera-small-red.png') self.mpstate.map.add_object(mp_slipmap.SlipIcon('gimbalview', (view_lat,view_lon), icon, layer='GimbalView', rotation=0, follow=False))
def coord_from_area(self, x, y, lat, lon, width, ground_width): '''return (lat,lon) for a pixel in an area image''' pixel_width = ground_width / float(width) dx = x * pixel_width dy = y * pixel_width return mp_util.gps_offset(lat, lon, dx, -dy)
def send_terrain_data_bit(self, bit): '''send some terrain data''' lat = self.current_request.lat * 1.0e-7 lon = self.current_request.lon * 1.0e-7 bit_spacing = self.current_request.grid_spacing * 4 (lat, lon) = mp_util.gps_offset(lat, lon, east=bit_spacing * (bit % 8), north=bit_spacing * (bit // 8)) data = [] for i in range(4 * 4): y = i % 4 x = i // 4 (lat2, lon2) = mp_util.gps_offset( lat, lon, east=self.current_request.grid_spacing * y, north=self.current_request.grid_spacing * x) alt = self.ElevationModel.GetElevation(lat2, lon2) if alt is None: if self.terrain_settings.debug: print("no alt ", lat2, lon2) return data.append(int(alt)) for m, _, _ in self.master: m.mav.terrain_data_send(self.current_request.lat, self.current_request.lon, self.current_request.grid_spacing, bit, data) self.blocks_sent += 1 self.last_send_time = time.time() self.sent_mask |= 1 << bit if self.terrain_settings.debug and bit == 55: lat = self.current_request.lat * 1.0e-7 lon = self.current_request.lon * 1.0e-7 print("--lat=%f --lon=%f %.1f" % (lat, lon, self.ElevationModel.GetElevation(lat, lon))) (lat2, lon2) = mp_util.gps_offset( lat, lon, east=32 * self.current_request.grid_spacing, north=28 * self.current_request.grid_spacing) print("--lat=%f --lon=%f %.1f" % (lat2, lon2, self.ElevationModel.GetElevation(lat2, lon2)))
def send_terrain_data_bit(self, bit): '''send some terrain data''' lat = self.current_request.lat * 1.0e-7 lon = self.current_request.lon * 1.0e-7 bit_spacing = self.current_request.grid_spacing * 4 (lat, lon) = mp_util.gps_offset(lat, lon, east=bit_spacing * (bit % 8), north=bit_spacing * (bit // 8)) data = [] for i in range(4*4): y = i % 4 x = i // 4 (lat2,lon2) = mp_util.gps_offset(lat, lon, east=self.current_request.grid_spacing * y, north=self.current_request.grid_spacing * x) alt = self.ElevationModel.GetElevation(lat2, lon2) if alt is None: if self.terrain_settings.debug: print("no alt ", lat2, lon2) return data.append(int(alt)) self.master.mav.terrain_data_send(self.current_request.lat, self.current_request.lon, self.current_request.grid_spacing, bit, data) self.blocks_sent += 1 self.last_send_time = time.time() self.sent_mask |= 1<<bit if self.terrain_settings.debug and bit == 55: lat = self.current_request.lat * 1.0e-7 lon = self.current_request.lon * 1.0e-7 print("--lat=%f --lon=%f %.1f" % ( lat, lon, self.ElevationModel.GetElevation(lat, lon))) (lat2,lon2) = mp_util.gps_offset(lat, lon, east=32*self.current_request.grid_spacing, north=28*self.current_request.grid_spacing) print("--lat=%f --lon=%f %.1f" % ( lat2, lon2, self.ElevationModel.GetElevation(lat2, lon2)))
def Update_traffic(self): '''Update traffic icon on map''' #from MAVProxy.modules.mavproxy_map import mp_slipmap t = time.time() if (t - self.lastUpdateTime < 0.5): return for i, tffc in enumerate(self.traffic_list): vehicle = 'Traffic%d' % i if (vehicle not in self.traffic_on_map): colour = "blue" vehicle_type = "copter" icon = self.mpstate.map.icon(colour + vehicle_type + '.png') self.mpstate.map.add_object(mp_slipmap.SlipIcon(vehicle, (0,0), icon, layer=3, rotation=0, follow=False, \ trail=mp_slipmap.SlipTrail(colour=(0,255,0)))) self.traffic_on_map.append(vehicle) self.traffic_list[i].get_pos(t) (lat, lon) = mp_util.gps_offset(self.start_lat, self.start_lon, self.traffic_list[i].y, self.traffic_list[i].x) heading = math.degrees( math.atan2(self.traffic_list[i].vy0, self.traffic_list[i].vx0)) self.mpstate.map.set_position(vehicle, (lat, lon), rotation=heading) self.master.mav.command_long_send( 1, # target_system 0, # target_component mavutil.mavlink.MAV_CMD_SPATIAL_USER_1, # command 0, # confirmation i, # param1 self.traffic_list[i].vx0, # param2 self.traffic_list[i].vy0, # param3 self.traffic_list[i].vz0, # param4 lat, # param5 lon, # param6 self.traffic_list[i].z0) # param7 self.lastUpdateTime = t
def find_new_spawn(loc, file_path): (lat, lon, alt, heading)=loc.split(",") swarminit_filepath = os.path.join(autotest, "swarminit.txt") for path2 in [file_path, swarminit_filepath]: if os.path.isfile(path2): with open(path2,'r') as swd: next(swd) for lines in swd: if len(lines) == 0: continue (instance, offset) = lines.split("=") if ((int)(instance) == (int)(cmd_opts.instance)): (x,y,z,head) = offset.split(",") g=mp_util.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y)) loc=str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head) return loc g=mp_util.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance)) loc=str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading) return loc
def find_new_spawn(loc, file_path): (lat, lon, alt, heading) = loc.split(",") swarminit_filepath = os.path.join(find_autotest_dir(), "swarminit.txt") for path2 in [file_path, swarminit_filepath]: if os.path.isfile(path2): with open(path2, 'r') as swd: next(swd) for lines in swd: if len(lines) == 0: continue (instance, offset) = lines.split("=") if ((int)(instance) == (int)(cmd_opts.instance)): (x, y, z, head) = offset.split(",") g = mp_util.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y)) loc = str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head) return loc g = mp_util.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance)) loc = str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading) return loc
def coord_from_area(self, x, y, lat, lon, width, ground_width): '''return (lat,lon) for a pixel in an area image x is pixel coord to the right from top,left y is pixel coord down from top left ''' scale1 = mp_util.constrain(cos(radians(lat)), 1.0e-15, 1) pixel_width = ground_width / float(width) pixel_width_equator = (ground_width / float(width)) / cos(radians(lat)) latr = radians(lat) y0 = abs(1.0/cos(latr) + tan(latr)) lat2 = 2 * atan(y0 * exp(-(y * pixel_width_equator) / mp_util.radius_of_earth)) - pi/2.0 lat2 = degrees(lat2) dx = pixel_width_equator * cos(radians(lat2)) * x (lat2,lon2) = mp_util.gps_offset(lat2, lon, dx, 0) return (lat2,lon2)
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 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_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER ]: self.vehicle_type_name = 'copter' elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]: self.vehicle_type_name = 'singlecopter' 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() == 'HOME_POSITION': (lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7) icon = self.mpstate.map.icon('home.png') self.mpstate.map.add_object( mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon), icon, layer=3, rotation=0, follow=False)) if m.get_type() == "NAV_CONTROLLER_OUTPUT": if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER", "QLAND" ] 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), 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 != 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()