def gps_to_pixel(location, map_origin, map_length, map_width, heading=0): '''Converts gps dcoordinates to pixel coordinates Returns tuple of location (tuple) and orientation Assumes map is oriented North''' # TODO look into this code with test cases. For now return hardcoded value return (333, 11193) altitude = location.alt # pylint: disable=unreachable print("len {} and width {}".format(map_length, map_width)) horizonatal_resolution = 3280 * SCALE horizontal_angle = 62.2 * SCALE vertical_resolution = 2464 * SCALE vertical_angle = 48.8 * SCALE alt_pixel_x_lon = horizonatal_resolution / tan(radians(horizontal_angle)) alt_pixel_y_lat = vertical_resolution / tan(radians(vertical_angle)) origin_pixel = (map_length // 2, map_width // 2) met_1640 = altitude / alt_pixel_x_lon met_1232 = altitude / alt_pixel_y_lat origin_lat, origin_lon = map_origin.lat, map_origin.lon poi_lat, poi_lon = location.lat, location.lon c_n, c_e, c_d = geodetic2ecef(poi_lat, poi_lon, altitude) delta_lat, delta_lon, _ = ecef2ned(c_n, c_e, c_d, origin_lat, origin_lon, altitude) delta_lat_pixel = delta_lat / met_1232 delta_lon_pixel = delta_lon / met_1640 angled_lon = (delta_lon_pixel * (cos(radians(heading)))) + (delta_lat_pixel * (sin(radians(heading)))) angled_lat = (delta_lat_pixel * (cos(radians(heading)))) - (delta_lon_pixel * (sin(radians(heading)))) origin_x, origin_y = origin_pixel print("xOrigin {}, yOrigin {}".format(origin_x, origin_y)) poi_x, poi_y = origin_x + angled_lon, origin_y + angled_lat print("xPixel {}, yPixel {}".format(poi_x, poi_y)) return poi_x, poi_y
def gpsToPixels(location, map_origin, map_length, map_width, HEADING=0): # TODO look into this code with test cases. For now return hardcoded value return (333, 11193) altitude = location.alt print("len {} and width {}".format(map_length, map_width)) HORIZONTAL_RESOLUTION = 3280 * SCALE HORIZONTAL_ANGLE = 62.2 * SCALE VERTICAL_RESOLUTION = 2464 * SCALE VERTICAL_ANGLE = 48.8 * SCALE ALT_PIXEL_X_Lon = HORIZONTAL_RESOLUTION / math.tan(math.radians(HORIZONTAL_ANGLE)) ALT_PIXEL_Y_Lat = VERTICAL_RESOLUTION / math.tan(math.radians(VERTICAL_ANGLE)) originPixel = (map_length // 2, map_width // 2) met_1640 = altitude / ALT_PIXEL_X_Lon met_1232 = altitude / ALT_PIXEL_Y_Lat originLat, originLon = map_origin.lat, map_origin.lon POI_Lat, POI_Lon = location.lat, location.lon cn, ce, cd = geodetic2ecef(POI_Lat, POI_Lon, altitude) delta_Lat, delta_Lon, d = ecef2ned(cn, ce, cd, originLat, originLon, altitude) delta_lat_pixel = delta_Lat / met_1232 delta_lon_pixel = delta_Lon / met_1640 angled_lon = (delta_lon_pixel * (math.cos(math.radians(HEADING)))) + ( delta_lat_pixel * (math.sin(math.radians(HEADING))) ) angled_lat = (delta_lat_pixel * (math.cos(math.radians(HEADING)))) - ( delta_lon_pixel * (math.sin(math.radians(HEADING))) ) originX, originY = originPixel print("xOrigin {}, yOrigin {}".format(originX, originY)) POIX, POIY = originX + angled_lon, originY + angled_lat print("xPixel {}, yPixel {}".format(POIX, POIY)) return POIX, POIY
def orbit_poi(vehicle, poi, configs): waypoints = [] # waypoints in LLA altitude = configs["altitude"] poi_scan_altitude = configs["detailed_search_specific"][ "poi_scan_altitude"] waypoint_tolerance = configs["waypoint_tolerance"] radius = configs["radius"] # radius of circle travelled orbit_number = configs["orbit_number"] # how many times we repeat orbit x, y, z = geodetic2ecef(poi.lat, poi.lon, poi_scan_altitude) # LLA -> ECEF n, e, d = ecef2ned(x, y, z, poi.lat, poi.lon, poi_scan_altitude) # ECEF -> NED cmds = vehicle.commands cmds.clear() # add circle of waypoints around the poi c = math.sqrt(2) / 2 point_list = [[1, 0], [c, c], [0, 1], [-c, c], [-1, 0], [-c, -c], [0, -1], [c, -c], [1, 0]] for orbit in range(orbit_number): for point in point_list: a = (radius * point[0]) + n b = (radius * point[1]) + e lat, lon, alt = ned2geodetic(a, b, d, poi.lat, poi.lon, poi_scan_altitude) # NED -> LLA waypoints.append(LocationGlobalRelative(lat, lon, alt)) # Go to center of POI if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, waypoint_tolerance, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) elif (configs["vehicle_type"] == "Quadcopter"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) # Transition to quadcopter if applicable if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0, mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) # Circular waypoints if (configs["vehicle_type"] == "VTOL"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, waypoint_tolerance, 0, 0, 0, point.lat, point.lon, point.alt)) elif (configs["vehicle_type"] == "Quadcopter"): for point in waypoints: cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point.lat, point.lon, point.alt)) # Transition to forward flight if applicable if (configs["vehicle_type"] == "VTOL"): cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, 0, 0, mavutil.mavlink.MAV_VTOL_STATE_MC, 0, 0, 0, 0, 0, 0)) # Add dummy endpoint cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, poi.lat, poi.lon, poi_scan_altitude)) print("Upload new commands to vehicle") cmds.upload()