def newLocationFromAzimuthAndDistance(loc, azimuth, distance):
result = Location(loc.lat, loc.lon, loc.alt)
rLat = math.radians(loc.lat)
rLong = math.radians(loc.lon)
dist = distance / EARTH_RADIUS
az = math.radians(azimuth)
lat = math.asin( math.sin(rLat) * math.cos(dist) + math.cos(rLat) * math.sin(dist) * math.cos(az) )
result.lat = math.degrees(lat)
result.lon = math.degrees(rLong + math.atan2( math.sin(az) * math.sin(dist) * math.cos(rLat), math.cos(dist) - math.sin(rLat) * math.sin(lat) ))
return result
def analyze_image(self):
f = self.get_frame()
self.writer.write(f) # For debugging later...
print "FIXME - add image analysis and fancy-pants math"
# FIXME - analyze the image to get a score indicating likelyhood there is a balloon and if it
# is there the x & y position in frame of the largest balloon
# FIXME - check to see if the image looks like a balloon
# FIXME - check if the balloon gets larger if we think we are approaching it
found_in_image = True # replace this with real code
xpos = 200
ypos = 300
if found_in_image:
vehicle_pos = self.vehicle.location
# FIXME - do math based on current vehicle loc and the x,y frame position
# (Someone needs a brain that still remembers trig)
lat_offset = 0.1 # FIXME
lon_offset = 0.1
alt_offset = 1
target_pos = Location(vehicle_pos.lat + lat_offset,
vehicle_pos.lon + lon_offset,
vehicle_pos.alt + alt_offset,
vehicle_pos.is_relative)
# FIXME - check if vehicle altitude is too low
# FIXME - check if we are too far from the desired flightplan
self.balloon_loc = target_pos
def __init__(self):
# read fake balloon location from config file
self.fake_balloon_location = Location(balloon_config.config.get_float('fake-balloon', 'lat',-35.363274),
balloon_config.config.get_float('fake-balloon', 'lon',149.164630),
balloon_config.config.get_float('fake-balloon', 'alt',15))
# fake balloon's colour is mid way between colour filter's low and high values
h = (balloon_finder.filter_low[0] + balloon_finder.filter_high[0]) / 2
s = (balloon_finder.filter_low[1] + balloon_finder.filter_high[1]) / 2
v = (balloon_finder.filter_low[2] + balloon_finder.filter_high[2]) / 2
# convert colour to BGR palette
fake_balloon_colour_bgr = cv2.cvtColor(numpy.uint8([[[h,s,v]]]),cv2.COLOR_HSV2BGR)
self.fake_balloon_colour_bgr_scalar = cv2.cv.Scalar(fake_balloon_colour_bgr.item(0), fake_balloon_colour_bgr.item(1), fake_balloon_colour_bgr.item(2))
# fake balloon is same radius as actual balloon
self.fake_balloon_radius = balloon_finder.balloon_radius_expected
# background sky and ground colours
self.background_sky_colour_bgr = (232, 228, 227)
self.background_ground_colour_bgr_scalar = cv2.cv.Scalar(87, 145, 158)
# last iterations balloon radius
self.last_balloon_radius = 0
def get_home(self, wait_for_arm=False):
#wait unitl we are armed to grab the INITIAL home position. This will lock up the program.
if (wait_for_arm and self.last_home is None):
sc_logger.text(
sc_logger.GENERAL,
'Waiting for intial home lock: Requires armed status....')
while (self.vehicle.armed == False):
time.sleep(0.3)
sc_logger.text(sc_logger.GENERAL, 'Got valid intial home location')
if (time.time() - self.last_home_call > self.home_update_rate):
self.last_home_call = time.time()
# download the vehicle waypoints
mission_cmds = self.vehicle.commands
mission_cmds.download()
mission_cmds.wait_valid()
# get the home lat and lon
home_lat = mission_cmds[0].x
home_lon = mission_cmds[0].y
self.last_home = Location(home_lat, home_lon, 0)
return self.last_home
def message_handler(self, m):
receive_time = time.time()
typ = m.get_type()
#buffer vehicle location
if typ == 'GLOBAL_POSITION_INT':
timestamp = m.time_boot_ms * 1000 #usec
(lat, lon, alt) = (m.lat / 1.0e7, m.lat / 1.0e7,
m.relative_alt / 1000.0)
(vx, vy, vz) = (m.vx / 100.0, m.vy / 100.0, m.vz / 100.0
) #meter/sec
loc = Location(lat, lon, alt)
vel = Point3(vx, vy, vz)
self.vehicle_pos.put_location(timestamp, loc, vel)
#buffer vehicle attitude
if typ == 'ATTITUDE':
timestamp = m.time_boot_ms * 1000 #usec
att = Attitude(m.pitch, m.yaw, m.roll) #radians
vel = Point3(m.rollspeed, m.pitchspeed, m.yawspeed) #rad/sec
self.vehicle_pos.put_attitude(timestamp, att, vel)
#attempt to sync system time
if typ == 'SYSTEM_TIME':
timestamp = m.time_boot_ms * 1000 #usec
diff = (receive_time * 1e6) - timestamp
#Use shift where USB delay is smallest. This does not account for clock drift!
if self.vehicle_time_diff is None or diff < self.vehicle_time_diff:
self.vehicle_time_diff = diff
def addVectorToLocation(loc, vec):
xToDeg = vec.x / LATLON_TO_M
# calculate longitude scaling factor. We could cache this if necessary
# but we aren't doing so now
scale = abs(math.cos(math.radians(loc.lat)))
yToDeg = vec.y / LATLON_TO_M / scale
return Location(loc.lat + xToDeg, loc.lon + yToDeg, loc.alt + vec.z)
def test(self):
self.put_location(0, Location(37.691163, -122.155766, 10),
Point3(30, 10, 2))
self.put_location(6000000, Location(37.691295, -122.151861, 20),
Point3(-30, 10, 3))
self.put_location(10000000, Location(37.690735, -122.152164, 20),
Point3(5, -6, -10))
for i in range(0, 10000000, 1000000):
print self.get_location(i).lat, ',', self.get_location(i).lon
#plot points on http://www.darrinward.com/lat-long/
self.put_attitude(0, Attitude(30, 60, 10), Point3(30, 10, 2))
self.put_attitude(6000000, Attitude(30, 80, 20), Point3(-30, 10, 3))
self.put_attitude(10000000, Attitude(30, 90, 20), Point3(5, -6, -10))
for i in range(0, 10000000, 1000000):
print self.get_attitude(i).roll, ',', self.get_attitude(i).pitch
def followme():
"""
followme - A DroneAPI example
This is a somewhat more 'meaty' example on how to use the DroneAPI. It uses the
python gps package to read positions from the GPS attached to your laptop an
every two seconds it sends a new goto command to the vehicle.
To use this example:
* Run mavproxy.py with the correct options to connect to your vehicle
* module load api
* api start <path-to-follow_me.py>
When you want to stop follow-me, either change vehicle modes from your RC
transmitter or type "api stop".
"""
try:
# First get an instance of the API endpoint (the connect via web case will be similar)
api = local_connect()
# Now get our vehicle (we assume the user is trying to control the virst vehicle attached to the GCS)
v = api.get_vehicles()[0]
# Don't let the user try to fly while the board is still boogint
if v.mode.name == "INITIALISING":
print "Vehicle still booting, try again later"
return
cmds = v.commands
is_guided = False # Have we sent at least one destination point?
# Use the python gps package to access the laptop GPS
gpsd = gps.gps(mode=gps.WATCH_ENABLE)
while not api.exit:
# This is necessary to read the GPS state from the laptop
gpsd.next()
if is_guided and v.mode.name != "GUIDED":
print "User has changed flight modes - aborting follow-me"
break
# Once we have a valid location (see gpsd documentation) we can start moving our vehicle around
if (gpsd.valid & gps.LATLON_SET) != 0:
altitude = 30 # in meters
dest = Location(gpsd.fix.latitude, gpsd.fix.longitude, altitude, is_relative=True)
print "Going to: %s" % dest
# A better implementation would only send new waypoints if the position had changed significantly
cmds.goto(dest)
is_guided = True
v.flush()
# Send a new target every two seconds
# For a complete implementation of follow me you'd want adjust this delay
time.sleep(2)
except socket.error:
print "Error: gpsd service does not seem to be running, plug in USB GPS or run run-fake-gps.sh"
def goto(self, location, relative=None):
self._log("Goto: {0}, {1}".format(location, self.altitude))
self.commands.goto(
Location(float(location[0]),
float(location[1]),
float(self.altitude),
is_relative=relative))
self.vehicle.flush()
def goto(lat, lon, alt=30):
print 'GOING TO', (lat, lon)
for i in range(0, 5):
time.sleep(.1)
# Set mode to guided - this is optional as the goto method will change the mode if needed.
vehicle.mode = VehicleMode("GUIDED")
# Set the target location and then call flush()
a_location = Location(lat, lon, alt, is_relative=True)
vehicle.commands.goto(a_location)
vehicle.flush()
def __init__(self):
self.size = 50
self.location_buffer = np.empty((self.size), object)
self.attitude_buffer = np.empty((self.size), object)
self.lb_index = 0
self.ab_index = 0
#load buffers
for i in range(0, self.size):
self.put_location(0, Location(0, 0, 0), Point3(0, 0, 0))
self.put_attitude(0, Attitude(0, 0, 0), Point3(0, 0, 0))
def __init__(self, veh_obj):
self.Vehicle = veh_obj
self.arm_UAV()
self.arm_and_takeoff(35)
time.sleep(2)
print 'HOME:', self.Vehicle.home_location
newLoc = Location(self.Vehicle.location.lat, self.Vehicle.location.lon, self.Vehicle.location.alt + 10)
self.gotoGPS(newLoc)
time.sleep(10)
self.Vehicle.mode = self.Vehicle.mode.VehicleMode("LAND")
self.Vehicle.flush()
def check_home(self):
# return immediately if home has already been initialised
if self.home_initialised:
return True
# check for home no more than once every two seconds
if (time.time() - self.last_home_check > 2):
# update that we have performed a status check
self.last_home_check = time.time()
# check if we have a vehicle
if self.vehicle is None:
self.vehicle = self.api.get_vehicles()[0]
return
# ensure the vehicle's position is known
if self.vehicle.location is None:
return False
if self.vehicle.location.lat is None or self.vehicle.location.lon is None or self.vehicle.location.alt is None:
return False
# download the vehicle waypoints if we don't have them already
if self.mission_cmds is None:
self.fetch_mission()
return False
# get the home lat and lon
home_lat = self.mission_cmds[0].x
home_lon = self.mission_cmds[0].y
home_alt = self.mission_cmds[0].z
# sanity check the home position
if home_lat is None or home_lon is None or home_alt is None:
return False
# sanity check again and set home position
if (home_lat != 0 and home_lon != 0):
self.home_location = Location(home_lat, home_lon, home_alt)
self.home_initialised = True
else:
self.mission_cmds = None
# To-Do: if we wish to have the same home position as the flight controller
# we must download the home waypoint again whenever the vehicle is armed
# return whether home has been initialised or not
return self.home_initialised
def Wait(alert,t=2): """ Monitors the SoarQ queue for commands to soar.""" alt = 0 while alert.empty(): start = time.time() if not SoarQ.empty(): altSoar = v.location.alt + SoarQ.get() if altSoar>alt: alt = altSoar if FetchParam(['MODE'])[0] != 'GUIDED': SetParam(['THR_MAX'],[75]) lat = v.location.lat lon = v.location.lon v.commands.goto(Location(lat,lon,alt)) else: v.commands.goto(Location(lat,lon,alt)) elif (FetchParam(['THR_MAX'])[0] != 0) and InHeading(): SetParam(['MODE','THR_MAX'],['AUTO',0]) alt = 0 wait = t - (time.time()-start) if wait>0: time.sleep(wait) # if executed fast enough, sleep for a while pass
def distance_to_current_waypoint(self):
"""
Gets distance in meters to the current waypoint.
It returns `None` for the first waypoint (Home location).
"""
next_waypoint = self.vehicle.commands.next
if next_waypoint == 1:
return None
mission_item = self.vehicle.commands[next_waypoint]
lat = mission_item.x
lon = mission_item.y
alt = mission_item.z
waypoint_location = Location(lat, lon, alt, is_relative=True)
distance = get_distance_meters(self.vehicle.location,
waypoint_location)
return distance
def distance_to_current_waypoint():
"""
Gets distance in metres to the current waypoint.
It returns None for the first waypoint (Home location).
"""
nextwaypoint = vehicle.commands.next
if nextwaypoint == 1:
return None
missionitem = vehicle.commands[nextwaypoint]
lat = missionitem.x
lon = missionitem.y
alt = missionitem.z
targetWaypointLocation = Location(lat, lon, alt, is_relative=True)
distancetopoint = get_distance_metres(vehicle.location,
targetWaypointLocation)
return distancetopoint
def main(self):
# set home position - to tridge's home field (this is just for testing anyway)
PositionVector.set_home_location(Location(-35.362938,149.165085,0))
print "Home %s" % PositionVector.get_home_location()
home_pos = PositionVector(0,0,0)
print "Home %s" % home_pos
# other position
other_pos = PositionVector.get_from_location(PositionVector.get_home_location())
print "Other %s" % other_pos
# set vehicle to be 10m north, 10m east and 10m above home
veh_pos = PositionVector(10,10,10)
print "Vehicle %s" % veh_pos.get_location()
print "Vehicle %s" % veh_pos
print "Distance from home: %f" % PositionVector.get_distance_xyz(home_pos,veh_pos)
def main(self):
# set home to tridge's home field (absolute alt = 270)
PositionVector.set_home_location(Location(-35.362938,149.165085,0))
# calculate balloon position
fake_balloon_pos = PositionVector.get_from_location(self.fake_balloon_location)
# vehicle attitude and position
veh_pos = PositionVector(0,0,fake_balloon_pos.z) # at home location
veh_roll = math.radians(0) # leaned right 10 deg
veh_pitch = math.radians(0) # pitched back at 0 deg
veh_yaw = PositionVector.get_bearing(veh_pos,fake_balloon_pos) # facing towards fake balloon
# display positions from home
print "Vehicle %s" % veh_pos
print "Balloon %s" % fake_balloon_pos
# generate simulated frame of balloon 10m north, 2m above vehicle
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
while(True):
# move vehicle towards balloon
veh_pos = veh_pos + (fake_balloon_pos - veh_pos) * 0.01
# regenerate frame
img = self.get_simulated_frame(veh_pos, veh_roll, veh_pitch, veh_yaw)
# look for balloon in image using blob detector
found_in_image, xpos, ypos, size = balloon_finder.analyse_frame(img)
# display actual vs real distance
dist_actual = PositionVector.get_distance_xyz(veh_pos, fake_balloon_pos)
dist_est = balloon_utils.get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)
print "Dist Est:%f Act:%f Size Est:%f Act:%f" % (dist_est, dist_actual, size, self.last_balloon_radius)
# show image
cv2.imshow("fake balloon", img)
# wait for keypress
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
# destroy windows
cv2.destroyAllWindows()
def get_edge_distance(self, edge, location, angle):
# Based on ray casting calculations from
# http://archive.gamedev.net/archive/reference/articles/article872.html
# except that the coordinate system there is assumed tp revolve around
# the vehicle, which is strange. Instead, use a fixed origin and thus
# the edge's b1 is fixed, and calculate b2 instead.
m2 = math.tan(angle)
b2 = location.lat - m2 * location.lon
if edge[1].lon == edge[0].lon:
# Prevent division by zero
# This should usually become inf, but since m2 is calculated with
# math.tan as well we should use the maximal value that this
# function reaches.
m1 = math.tan(math.pi / 2)
b1 = 0.0
x = edge[0].lon
else:
m1 = (edge[1].lat - edge[0].lat) / (edge[1].lon - edge[0].lon)
if edge[1].lat < edge[0].lat:
b1 = edge[1].lat - m1 * edge[1].lon
else:
b1 = edge[0].lat - m1 * edge[0].lon
x = (b1 - b2) / (m2 - m1)
y = m2 * x + b2
loc_point = Location(y, x, location.alt, location.is_relative)
# Get altitude from edge
edge_dist = get_distance_meters(edge[0], edge[1])
point_dist = get_distance_meters(edge[1], loc_point)
alt = edge[1].alt + (
(edge[0].alt - edge[1].alt) / edge_dist) * point_dist
if alt < location.alt - self.altitude_margin:
print('Not visible due to altitude alt={} v={}'.format(
alt, location.alt))
return sys.float_info.max
d = get_distance_meters(location, loc_point)
return d
def get_location_metres(original_location, dNorth, dEast):
"""
Returns a Location object containing the latitude/longitude `dNorth` and `dEast` metres from the
specified `original_location`. The returned Location has the same `alt and `is_relative` values
as `original_location`.
The function is useful when you want to move the vehicle around specifying locations relative to
the current vehicle position.
The algorithm is relatively accurate over small distances (10m within 1km) except close to the poles.
For more information see:
http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters
"""
earth_radius = 6378137.0 #Radius of "spherical" earth
#Coordinate offsets in radians
dLat = dNorth / earth_radius
dLon = dEast / (earth_radius *
math.cos(math.pi * original_location.lat / 180))
#New position in decimal degrees
newlat = original_location.lat + (dLat * 180 / math.pi)
newlon = original_location.lon + (dLon * 180 / math.pi)
return Location(newlat, newlon, original_location.alt,
original_location.is_relative)
dlat = lat2 - lat1
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * asin(sqrt(a))
r = 6371 # Radius of earth in kilometers. Use 3956 for miles
return c * r
# Old center to new center
h = haversine(40.12076, -83.07773, 40.120662, -83.078250)
print h
arm_and_takeoff(20)
print "Secondary calculated circle (polygon) from haversinve data + vincenty direct vs. new center point"
for point in circlePoints:
print "Going to point..."
print str(vinc_pt(point[0], point[1], b, h * 1000))
point1 = Location(point[0], point[1], 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
# sleep so we can see the change in map
time.sleep(10)
print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
vehicle.flush()
def location(self):
return Location(self.__module.lat, self.__module.lon, self.__module.alt, is_relative=False)
v.armed = True
v.flush()
alt = 30
v.mode = VehicleMode("GUIDED")
v.flush()
time.sleep(3)
poe_wps = [(40.343762, -74.653822), (40.343750, -74.654158),
(40.343519, -74.654493), (40.343547, -74.653938)]
foot_wps = [(40.345763, -74.649955), (40.346069, -74.650159),
(40.345706, -74.649895), (40.345634, -74.649994)]
origin = Location(foot_wps[0][0], foot_wps[0][1], alt, is_relative=True)
print "GOING TO " + str(foot_wps[0]) + "..."
commands.goto(origin)
v.flush()
"""
# sleep 2 seconds so we can see the change in map
time.sleep(5)
destination = Location(poe_wps[1][0], poe_wps[1][0], 30, is_relative=True)
commands.goto(destination)
vehicle.flush()
"""
def get_location(self):
dlat = self.x / posvec_latlon_to_m
dlon = self.y / (posvec_latlon_to_m * posvec_lon_scale)
return Location(posvec_home_location.lat + dlat,
posvec_home_location.lon + dlon,
posvec_home_location.alt + self.z)
"""
PositionVector class : holds a 3 axis position offset from home in meters in NEU format
X = North-South with +ve = North
Y = West-East with +ve = West
Z = Altitude with +ve = Up
"""
import math
from droneapi.lib import Location
# global variables used by position vector class
posvec_home_location = Location(0, 0, 0)
posvec_lon_scale = 1.0 # scaling used to account for shrinking distance between longitude lines as we move away from equator
posvec_latlon_to_m = 111319.5 # converts lat/lon to meters
class PositionVector(object):
# define public members
x = 0 # north-south direction. +ve = north of home
y = 0 # west-east direction, +ve = east of home
z = 0 # vertical direction, +ve = above home
def __init__(self, initial_x=0, initial_y=0, initial_z=0):
# default config file
self.x = initial_x
self.y = initial_y
self.z = initial_z
# get_from_location - returns a position vector created from a location
def sendLookFrom(drone, llh, vel=[0, 0, 0]):
print "sending look from"
dest = Location(llh[0], llh[1], llh[2], is_relative=False)
drone.vehicle.commands.goto(dest)
def go_to_wp(self, lat, lon, alt):
self._log('Command GOTOWP received')
if self.vehicle.mode.name != 'guided':
self.set_mode('GUIDED')
self.vehicle.commands.goto(Location(float(lat), float(lon), float(alt)))
self.vehicle.flush()
Only let commands through at 10hz
'''
msg = self.uav.message_factory.set_position_target_local_ned_encode(
0, # time_boot_ms (not used)
255, 0, # target system, target component
mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
0x01C7, # type_mask (ignore pos | ignore acc)
0, 0, 0, # x, y, z positions (not used)
v_x, v_y, v_z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.uav.send_mavlink(msg)
self.uav.flush()
def goto(self, (lat, lon), alt=30):
"""
send the uav to the designated latitude, longitude, and altidude.
USE WITH CAUTION
requires GPS lock
"""
if self.uav.mode.name == "GUIDED":
loc = Location(lat, lon, alt, is_relative=True)
self.uav.commands.goto(loc)
self.uav.flush()
return True
return False
MESSAGES = {'armed':'ARMED', 'reached_alt':'REACHED_ALT'}
time.sleep(1) # Get Vehicle Home location ((0 index in Vehicle.commands) print "Get home location" cmds = vehicle.commands cmds.download() cmds.wait_valid() print " Home WP: %s" % cmds[0] arm_and_takeoff(3) #After the vehicle reaches a target height, do other things print "Going to first point..." point1 = Location(-35.361354, 149.165218, 3, is_relative=True) vehicle.commands.goto(point1) vehicle.flush() # sleep so we can see the change in map time.sleep(10) print "Going to second point..." point2 = Location(-35.363244, 149.168801, 3, is_relative=True) vehicle.commands.goto(point2) vehicle.flush() # sleep so we can see the change in map time.sleep(10) print "Returning to Launch"
def goto_waypoint(location, altitude):
point = Location(location[0], location[1], altitude, is_relative=True)
drone.commands.goto(point)
time.sleep(10)
drone.flush()