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