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)
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 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 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 first vehicle attached to the GCS)
v = api.get_vehicles()[0]
# Don't let the user try to fly while the board is still booting
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"
vehicle.flush()
# Wait until the vehicle reaches a safe height before processing the goto (otherwise the command
# after Vehicle.commands.takeoff will execute immediately).
while not api.exit:
print " Altitude: ", vehicle.location.alt
if vehicle.location.alt >= aTargetAltitude * 0.95: #Just below target, in case of undershoot.
print "Reached target altitude"
break
time.sleep(1)
arm_and_takeoff(20)
print "Going to first point..."
point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(30)
print "Going to second point..."
point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(20)
print "Returning to Launch"
def location(self):
return Location(self.__module.lat,
self.__module.lon,
self.__module.alt,
is_relative=False)
def test_goto(connpath):
vehicle = connect(connpath, await_params=True)
# NOTE these are *very inappropriate settings*
# to make on a real vehicle. They are leveraged
# exclusively for simulation. Take heed!!!
vehicle.parameters['ARMING_CHECK'] = 0
vehicle.parameters['FS_THR_ENABLE'] = 0
vehicle.parameters['FS_GCS_ENABLE'] = 0
vehicle.parameters['EKF_CHECK_THRESH'] = 0
def arm_and_takeoff(aTargetAltitude):
"""
Arms vehicle and fly to aTargetAltitude.
"""
# print "Basic pre-arm checks"
# Don't let the user try to fly autopilot is booting
if vehicle.mode.name == "INITIALISING":
# print "Waiting for vehicle to initialise"
time.sleep(1)
while vehicle.gps_0.fix_type < 2:
# print "Waiting for GPS...:", vehicle.gps_0.fix_type
time.sleep(1)
# print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.flush()
i = 60
while vehicle.mode.name != 'GUIDED' and i > 0:
# print " Waiting for guided %s seconds..." % (i,)
time.sleep(1)
i = i - 1
vehicle.armed = True
vehicle.flush()
i = 60
while not vehicle.armed and vehicle.mode.name == 'GUIDED' and i > 0:
# print " Waiting for arming %s seconds..." % (i,)
time.sleep(1)
i = i - 1
# Failure will result in arming but immediately landing
assert vehicle.armed
assert_equals(vehicle.mode.name, 'GUIDED')
# print "Taking off!"
vehicle.commands.takeoff(aTargetAltitude) # Take off to target altitude
vehicle.flush()
# 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.alt
# Test for altitude just below target, in case of undershoot.
if vehicle.location.alt >= aTargetAltitude * 0.95:
# print "Reached target altitude"
break
assert_equals(vehicle.mode.name, 'GUIDED')
time.sleep(1)
arm_and_takeoff(10)
# print "Going to first point..."
point1 = Location(-35.361354, 149.165218, 20, is_relative=True)
vehicle.commands.goto(point1)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(3)
# print "Going to second point..."
point2 = Location(-35.363244, 149.168801, 20, is_relative=True)
vehicle.commands.goto(point2)
vehicle.flush()
# sleep so we can see the change in map
time.sleep(3)
# print "Returning to Launch"
vehicle.mode = VehicleMode("RTL")
vehicle.flush()