def get_location_metres(original_location, dNorth, dEast):
"""
Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the
specified `original_location`. The returned LocationGlobal 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 LocationGlobal(newlat, newlon, original_location.alt,
original_location.is_relative)
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 = LocationGlobal(lat, lon, alt, is_relative=True)
distancetopoint = get_distance_metres(vehicle.location.global_frame,
targetWaypointLocation)
return distancetopoint
vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while True:
print " Altitude: ", vehicle.location.global_frame.alt
if vehicle.location.global_frame.alt >= aTargetAltitude * 0.95: #Trigger just below target alt.
print "Reached target altitude"
break
time.sleep(1)
arm_and_takeoff(20)
print "Going to first point..."
point1 = LocationGlobal(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
# sleep so we can see the change in map
time.sleep(30)
print "Going to second point..."
point2 = LocationGlobal(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
# sleep so we can see the change in map
time.sleep(30)
print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
arm_and_takeoff(5)
while True:
if vehicle.mode.name != "GUIDED":
print "User has changed flight modes - aborting follow-me"
break
# Read the GPS state from the laptop
gpsd.next()
# 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 = LocationGlobal(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
vehicle.commands.goto(dest)
# 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"
sys.exit(1)
#Close vehicle object before exiting script
def location(self):
# For backward compatibility, this is (itself) a LocationLocal object.
ret = LocationGlobal(self.__module.lat, self.__module.lon, self.__module.alt, is_relative=False)
ret.local_frame = LocationLocal(self.__module.north, self.__module.east, self.__module.down)
ret.global_frame = LocationGlobal(self.__module.lat, self.__module.lon, self.__module.alt, is_relative=False)
return ret