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