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