def get_location_metres(self, original_location, dNorth, dEast): 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) if type(original_location) is LocationGlobal: targetlocation = LocationGlobal( newlat, newlon, original_location.alt) elif type(original_location) is LocationGlobalRelative: targetlocation = LocationGlobalRelative( newlat, newlon, original_location.alt) else: raise Exception("Invalid Location object passed") return targetlocation
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 Location has the same `alt` value 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)
def get_location_metres(original_location, dNorth, dEast): """ Calculate a coordinate by a vector related to a known coordinate. Returns a `Location` object containing the latitude/longitude `dNorth` and `dEast` metres from the specified `original_location`. The returned `Location` has the same `alt` value as `original_location`. Added the compensation of the eclipse effect to the earth, increasing accuracy. This is a approximation, and would get sloppy and inaccurate when closing to poles. Args: original_location(dronekit.LocationXXYY): a location coordinate. dNorth(float): meters to the north. dEast(float): meters to the east. Returns: dronekit.LocationXXYY: calculated coordinate of the same type. """ [r_center, r_level] = eclipse_compensate(original_location) # coordinate offsets in radians dLat = dNorth / r_center dLon = dEast / r_level # new position in decimal degrees newlat = original_location.lat + math.degrees(dLat) newlon = original_location.lon + math.degrees(dLon) # return according to the input coordinate Class if isinstance(original_location, LocationGlobal): targetlocation = LocationGlobal(newlat, newlon, original_location.alt) elif isinstance(original_location, LocationGlobalRelative): targetlocation = LocationGlobalRelative(newlat, newlon, original_location.alt) else: raise Exception("Invalid Location object passed") return targetlocation
def set_home_loc(vehicle, home_loc=''): present_loc = LocationGlobal(18.589266, 73.863886, 10) vehicle.home_location = present_loc # If home_loc is null, then print/log the current home location. if not home_loc: # Get Vehicle Home location - will be `None` until first set by autopilot while not vehicle.home_location: cmds = vehicle.commands cmds.download() cmds.wait_ready() if not vehicle.home_location: print("Waiting for home location ...") # We have a home location, so print it! print("Home location: {}".format(vehicle.home_location)) print("Vehicle global frmae: {}".format(vehicle.location.global_frame)) print("Location in Global frame: {}".format(vehicle.location.global_frame)) print("Location in global relative frame: {}".format( vehicle.location.global_relative_frame)) print("Location in Local frame: {}".format(vehicle.location.local_frame))
def get_location_metres(original_location, dNorth, dEast): ''' Parameters - original position - LocationGlobal() of refrence location dNorth - meters moved in north dEast - meters moved in east Return - LocationGlobal of desired point. ''' 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, 10)
def goto(self, location, relative=None): wait = 30 #seconds self._log("Goto: {0}, {1}".format(location, self.altitude)) # Altitude is relative to Home position if relative: self.vehicle.simple_goto( LocationGlobalRelative(float(location[0][:-9]), float(location[1][:-9]), float(self.altitude))) print(" Altitude: ", self.vehicle.location.global_relative_frame.alt, " Location: ", self.vehicle.location.global_relative_frame.lat, ", ", self.vehicle.location.global_relative_frame.lon) time.sleep(wait) #give it time to fy print(" Altitude: ", self.vehicle.location.global_relative_frame.alt, " Location: ", self.vehicle.location.global_relative_frame.lat, ", ", self.vehicle.location.global_relative_frame.lon) else: self.vehicle.simple_goto( LocationGlobal(float(location[0][:-9]), float(location[1][:-9]), float(self.altitude))) print(" Altitude: ", self.vehicle.location.global_relative_frame.alt, " Location: ", self.vehicle.location.global_relative_frame.lat, ", ", self.vehicle.location.global_relative_frame.lon) time.sleep(wait) #give it time to fy print(" Altitude: ", self.vehicle.location.global_relative_frame.alt, " Location: ", self.vehicle.location.global_relative_frame.lat, ", ", self.vehicle.location.global_relative_frame.lon)
def get_location_metres(original_location, dNorth, dEast, altitude): """ A basic function from DroneKit that returns the GPS coordinates in the global reference frame from the original x, y and z coordinates. For that it assumes an spherical Earth radius of 6378.137km and a flat Earth approximation. It is call each time a new mission is added, in order to transform the output waypoints from the guidance algorithm into GPS coordinate and altitude. Returns a LocationGlobal object containing the latitude/longitude `dNorth` and `dEast` metres from the specified `original_location`. The returned LocationGlobal has the same `alt` value 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) newalt = original_location.alt + altitude if type(original_location) is LocationGlobal: targetlocation = LocationGlobal(newlat, newlon, newalt) elif type(original_location) is LocationGlobalRelative: targetlocation = LocationGlobalRelative(newlat, newlon, newalt) else: raise Exception("Invalid Location object passed") return targetlocation
def setHome(data): global vehicle lockV() try: print "SETHOME" parameters = data['command']['parameters'] for i in parameters: if i['name'] == "lat": lat = i['value'] if i['name'] == "lon": lon = i['value'] point1 = LocationGlobal(float(lat), float(lon), vehicle.home_location.alt) if vehicle.home_location != None: vehicle.home_location=point1 return "OK" finally: unlockV()
def test_get_location_local_other(self): home_loc = self._make_global_location(5.0, 3.14, 10.0) self.geometry.set_home_location(home_loc) rel_loc = self.geometry.get_location_meters(home_loc, 0.4, 0.06, 1.0) local_loc = LocationLocal(0.4, 0.06, -1.0) new_loc = self.geometry.get_location_local(rel_loc) self.assertAlmostEqual(new_loc.north, local_loc.north, delta=self.coord_delta) self.assertAlmostEqual(new_loc.east, local_loc.east, delta=self.coord_delta) self.assertAlmostEqual(new_loc.down, local_loc.down, delta=self.coord_delta) global_loc = LocationGlobal(rel_loc.lat, rel_loc.lon, 11.0) new_loc = self.geometry.get_location_local(global_loc) self.assertAlmostEqual(new_loc.north, local_loc.north, delta=self.coord_delta) self.assertAlmostEqual(new_loc.east, local_loc.east, delta=self.coord_delta) self.assertAlmostEqual(new_loc.down, local_loc.down, delta=self.coord_delta)
def test_set_home(connpath): vehicle = connect(connpath, wait_ready=True) # Wait for home position to be real and not 0, 0, 0 # once we request it via cmds.download() time.sleep(10) vehicle.commands.download() vehicle.commands.wait_ready() assert_not_equals(vehicle.home_location, None) # Note: If the GPS values differ heavily from EKF values, this command # will basically fail silently. This GPS coordinate is tailored for that # the with_sitl initializer uses to not fail. vehicle.home_location = LocationGlobal(-35, 149, 600) vehicle.commands.download() vehicle.commands.wait_ready() assert_equals(vehicle.home_location.lat, -35) assert_equals(vehicle.home_location.lon, 149) assert_equals(vehicle.home_location.alt, 600) vehicle.close()
def get_set_home_location(self, set_home_location_list=None): """ 获取和设置home位置,传入home_location_list表示设置,不传表示获取 :param home_location_list:[纬度,经度] :return: """ cmds = self.vehicle.commands cmds.download() cmds.wait_ready() home = drone_obj.vehicle.home_location try: if home is None: print('home还未初始化成功') else: print('current vehicle.home_location', home.alt, home.lat, home.lon) if set_home_location_list is not None: drone_obj.vehicle.home_location = LocationGlobal( set_home_location_list[1], set_home_location_list[0], home.alt) except Exception as e: print({'error': e})
def getSSMeters(aLocation, alpha, dLat, dLon, turn): """ Returns a LocationGlobal object containing the latitude/longitude values of the next position in the sector search """ earth_radius = 6378137.0 #Radius of "spherical" earth if turn == 0: #Coordinate offsets in radians bearing = math.radians(alpha - 180 + 60) if bearing >= 360: bearing += 360 Lat = (dLat * math.sin(bearing)) / earth_radius Lon = (dLat * math.cos(bearing)) / earth_radius print "TestCos: %s" % (math.cos(bearing)) print "New dx: %s" % (Lon * 180 / math.pi) print "New dy: %s" % (Lat * 180 / math.pi) print "New Bearing should be: %s" % (bearing) # elif turn == 1: #Coordinate offsets in radians print "alpha: %s" % (alpha) bearing = math.radians(alpha + 180 - 60) if bearing >= 360: bearing += 360 Lat = (dLat * math.sin(bearing)) / earth_radius Lon = (dLat * math.cos(bearing)) / earth_radius print "TestSin: %s" % (math.sin(bearing)) print "New dx: %s" % (Lon * 180 / math.pi) print "New dy: %s" % (Lat * 180 / math.pi) print "New Bearing should be: %s" % (math.degrees(bearing)) #New position in decimal degrees print "Old Latitude: %s Longitude: %s" % (aLocation.lat, aLocation.lon) newlat = aLocation.lat + (Lat * 180 / math.pi) newlon = aLocation.lon + (Lon * 180 / math.pi) print "New Latitude: %s Longitude: %s" % (newlat, newlon) return LocationGlobal(newlat, newlon, aLocation.alt)
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` value as `original_location`. """ 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) if type(original_location) is LocationGlobal: targetlocation = LocationGlobal(newlat, newlon, original_location.alt) elif type(original_location) is LocationGlobalRelative: targetlocation = LocationGlobalRelative(newlat, newlon, original_location.alt) else: raise Exception("Invalid Location object passed") return targetlocation
def get_location_meters(self, original_location, north, east, alt=0): """ Returns a Location object containing the latitude/longitude `north` and `east` (floating point) meters from the specified `original_location`, and optionally `alt` meters above the `original_location`. The returned location is the same type as `original_location`. The algorithm is relatively accurate over small distances, and has an error of at most 10 meters for displacements up to 1 kilometer, except when 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 """ if isinstance(original_location, LocationLocal): return super(Geometry_Spherical, self).get_location_meters(original_location, north, east, alt) if isinstance(original_location, Locations): original_location = original_location.global_relative_frame # Coordinate offsets in radians lat = north / self.EARTH_RADIUS lon = east / (self.EARTH_RADIUS * math.cos(original_location.lat * math.pi / 180)) # New position in decimal degrees newlat = original_location.lat + (lat * 180 / math.pi) newlon = original_location.lon + (lon * 180 / math.pi) newalt = original_location.alt + alt if isinstance(original_location, LocationGlobal): return LocationGlobal(newlat, newlon, newalt) return LocationGlobalRelative(newlat, newlon, newalt)
parser.add_argument( '--connect', help= "Vehicle connection target string. If not specified, SITL automatically started and used." ) args = parser.parse_args() connection_string = args.connect sitl = None print "Connection string: %s" % connection_string if not connection_string: start_lat = 41.833474 start_lon = -87.626819 start_location = LocationGlobal(start_lat, start_lon, 584) import dronekit_sitl sitl = dronekit_sitl.start_default(start_lat, start_lon) connection_string = sitl.connection_string() # # Get waypoint latitude and longitude # latitude = raw_input("Enter latitude:") # longitude = raw_input("Enter longitude:") # altitude = raw_input("Enter altitude:") # latitude = float(latitude) # longitude = float(longitude) # altitude = float(altitude) # print "Set waypoint as %f, %f, %f?" % (latitude, longitude, altitude) # while raw_input("y/n?")is not "y": # latitude = raw_input("Enter latitude:")
def move_left(height): """ Adds a takeoff command and four waypoint commands to the current mission. The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation). The function assumes vehicle.commands matches the vehicle mission state (you must have called download at least once in the session and after clearing the mission) """ cmds = vehicle.commands print(" Clear any existing commands") cmds.clear() print(" Define/add new commands.") # Add new commands. The meaning/order of the parameters is documented in the Command class. #Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air. cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, height)) #Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands point1 = LocationGlobal(29.867521, 77.899015, height) point2 = LocationGlobal(29.867889, 77.899059, height) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1], yy[1], height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2], yy[2], height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2] + (xx[3] - xx[2]) / 5, yy[2] + (yy[3] - yy[2]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + (xx[4] - xx[1]) / 5, yy[1] + (yy[4] - yy[1]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + 2 * (xx[4] - xx[1]) / 5, yy[1] + 2 * (yy[4] - yy[1]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2] + 2 * (xx[3] - xx[2]) / 5, yy[2] + 2 * (yy[3] - yy[2]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2] + 3 * (xx[3] - xx[2]) / 5, yy[2] + 3 * (yy[3] - yy[2]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + 3 * (xx[4] - xx[1]) / 5, yy[1] + 3 * (yy[4] - yy[1]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + 4 * (xx[4] - xx[1]) / 5, yy[1] + 4 * (yy[4] - yy[1]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2] + 4 * (xx[3] - xx[2]) / 5, yy[2] + 4 * (yy[3] - yy[2]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[2] + 5 * (xx[3] - xx[2]) / 5, yy[2] + 5 * (yy[3] - yy[2]) / 5, height)) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + 5 * (xx[4] - xx[1]) / 5, yy[1] + 5 * (yy[4] - yy[1]) / 5, height)) #add dummyy waypoint "5" at point 4 (lets us know when have reached destination) cmds.add( Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, xx[1] + 5 * (xx[4] - xx[1]) / 5, yy[1] + 5 * (yy[4] - yy[1]) / 5, height)) print(" Upload new commands to vehicle") cmds.upload()
#!/usr/bin/env python from dronekit import LocationGlobal import math position_vector_home_location = LocationGlobal(0,0,0) # scaling used to account for shrinking distances between longitudinal lines as we move away from the equator # not sure if I understood the formula but it has tested to be working fine position_vector_lon_scale = 1.0 # converts lat and lon to metres position_vector_latlon_to_metres = 111319.5 class PositionVector(object): def __init__(self): pass def __str__(self): pass @classmethod def set_home_location(cls,home_location): # trying out @classmethod # tested to be working global position_vector_home_location position_vector_home_location = home_location print 'Home location: ', position_vector_home_location PositionVector.update_lon_scale(position_vector_home_location.lat) @classmethod def update_lon_scale (cls, lat): global position_vector_lon_scale if lat <> 0:
def intercept(): while True: global x, y, z, key, vel, accel, correction_x, correction_y, correction_z, trkx, trky global edist, elat, elon, ealt, submode, yaw, turn, between, climb, red, meh, loft, homedist climb = vehicle.location.global_frame.alt meh = 3 home1 = homedist() if home1 >= 1000: ser.write("Vehicle is out of bounds\n") time.sleep(1) ser.write("Vehicle is RTB\n") submode = "landing" break if vehicle.battery.level <= 15: ser.write("Low Battery\n") time.sleep(1) ser.write("Vehicle is RTB\n") submode = "landing" break ser.write("Key Request\n") while True: [Name, key] = rec_key("key") break print "climb: %s" % climb #print home_lat #print home_lon #print home_alt #key=msvcrt.getch() print "key is %s" % key yaw = vehicle.heading between1 = between() d1 = between1 start = time.time() time.sleep(1) between2 = between() d2 = between2 end = time.time() tgt = d2-d1 vel = (tgt/(end-start))*.9 #print tgt #print trkx #print trky #print vel #tracking(vel*trkx, vel*trky,0,5) #not sure if we want drone to automatically go towards other enemy or make that a manual decision #if abs(ealt - vehicle.location.global_relative_frame.alt) > 1 and abs(between) > 30: #submode = "goto" #ser.write("Acquired new target\n") #break #if between > 5: #turn = 15 #time = 1 #elif between<5: #turn = 5 #time = .5 if key == "a": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (s_lat*.00002), vehicle.location.global_relative_frame.lon - (s_lon*.00002), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "d": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat - (s_lat*.00002), vehicle.location.global_relative_frame.lon + (s_lon*.00002), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "w": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (f_lat*.00002), vehicle.location.global_relative_frame.lon + (f_lon*.00002), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "s": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (b_lat*.00002), vehicle.location.global_relative_frame.lon + (b_lon*.00002), vehicle.location.global_frame.alt) gotoGPS(newLoc) #makes the drone fly down 1 m elif key == "z": loft = climb - meh #lift = lift - 3 print "going down" ser.write("Descending\n") newLoc = LocationGlobal (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon , loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt <= loft + 1: #Can remove the hard number if mission planner waypoint radius is changable print "check" break #makes the drone fly up 1 m elif key == "x": loft = climb + meh print "going up" ser.write("Ascending\n") newLoc = LocationGlobal (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt >= loft - 1: #Can remove the hard number if mission planner waypoint radius is changable print "check" break #p leaves function elif key == "p": #Used to track another target #Get WP and enemy's WP submode = "goto" break elif key == "t": strafe() loft = climb + meh print "Engaging!" ser.write("Engaging!\n") newLoc = LocationGlobal (vehicle.location.global_frame.lat + (b_lat*.00003), vehicle.location.global_frame.lon + (b_lon*.00003), loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt >= loft - 1: #Can remove the hard number if mission planner waypoint radius is changable break elif key == "k": repeat = 0 #pwm.stop() pwm.start(10) time.sleep(10) pwm.stop while True: loft = climb + meh strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (s_lat*.00005), vehicle.location.global_relative_frame.lon - (s_lon*.00005), loft) gotoGPS(newLoc) time.sleep(1.2) strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat - (s_lat*.00005), vehicle.location.global_relative_frame.lon + (s_lon*.00005), loft) gotoGPS(newLoc) time.sleep(1.2) repeat = repeat + 1 if repeat == 2: break #n releases another net elif key == "n": #pwm.stop() pwm.start(5) time.sleep(6) pwm.stop submode = "goto" elif key == "/": submode = "landing" break #tracking(vel*trkx, vel*trky, 0, 1) #print " Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) ser.write("Current Location: Lat:%s, Lon:%s, Alt:%s\n" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt)) La = float(vehicle.location.global_relative_frame.lat) la = float(vehicle.location.global_relative_frame.lon) Lb = float(elat) lb = float(elon) int(La) int(la) int(Lb) int(lb) U = math.cos(math.radians(Lb))*math.sin(math.radians(lb-la)) T = math.cos(math.radians(La))*math.sin(math.radians(Lb))-math.sin(math.radians(La))*math.cos(math.radians(Lb))*math.cos(math.radians(lb-la)) Bearing = math.atan2(U,T) if Bearing < 0: Hdg = math.degrees(Bearing)+360 else: Hdg = math.degrees(Bearing) #print Bearing conditionyaw(Hdg) while True: print " Heading: ", vehicle.heading ser.write("Heading: %s\n" % vehicle.heading) if Hdg - 5 <= vehicle.heading and Hdg + 5 >= vehicle.heading: print "Lock-on" break print "Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) print vehicle.heading ser.write("Current Location: Lat:%s, Lon:%s, Alt:%s\n" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt)) print "tgt is located at %s" % Hdg ser.write("Tgt heading is %s\n" % Hdg) ser.write("Vehicle heading is %s\n" % vehicle.heading)
vehicle.mode = VehicleMode("AUTO") vehicle.flush() else: arm_and_takeoff(vehicle,2) time.sleep(1) elif word1 == 'show': shows_data(vehicle) elif word1 == 'search': Pattern = input("Enter 'SS' = Sector Search \n 'PT' = Parallel Track \n 'ES' = Expanding Square\nSpecify desired search pattern: ") Lat = input("Enter Latitude: ") Lon = input("Enter Longitude: ") Area = input("Enter Specify search area (m2): ") Alt = input("Enter Specify search altitude (m): ") point = LocationGlobal(float(Lat), float(Lon), float(Alt)) if Pattern == 'SS': addsSectorSearch(float(Area), point, float(Alt)) elif Pattern == 'PT': addsParallelTrack(float(Area), point, float(Alt)) elif Pattern == 'ES': addsExpandSquare(point, float(Area), float(Alt)) arm_and_takeoff(vehicle, 2) vehicle.commands.next=0 vehicle.mode = VehicleMode("AUTO") vehicle.flush() elif word1=='land': print("\n\nLanding!\n\n") print("\nSet Vehicle mode = LAND (currently: %s)"%vehicle.mode.name) while not vehicle.mode=='LAND': print('waiting to change Mode to LAND...')
def setNavHome(self, Latitude, Longitude, Altitude): self.Home = LocationGlobal(Latitude, Longitude, Altitude)
def traveling(): while True: global x, y, z, key, vel, accel, correction_x, correction_y, correction_z, trkx, trky global elat, elon, ealt, submode, edist, between, homedist count = 1 home1 = homedist() if home1 >= 1000: ser.write("Vehicle is out of bounds\n") time.sleep(1) ser.write("Vehicle is RTB\n") submode = "landing" break if vehicle.battery.level <= 15: ser.write("Low Battery\n") time.sleep(1) ser.write("Vehicle is RTB\n") submode = "landing" break a = float(x - vehicle.location.global_relative_frame.lat) b = float(y - vehicle.location.global_relative_frame.lon) c = float(z - vehicle.location.global_relative_frame.alt) + 2 int(a) int(b) int(c) newLoc = LocationGlobal (vehicle.location.global_frame.lat + a, vehicle.location.global_frame.lon + b, vehicle.location.global_frame.alt + c) gotoGPS(newLoc) #print " Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) #print " Enroute to Lat:%s, Lon:%s, Alt:%s" % (x,y,z) ser.write("Current Location: Lat:%s, Lon:%s, Alt:%s\n" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt)) ser.write("Enroute to Lat:%s, Lon:%s, Alt:%s\n" % (x,y,z)) dist1 = distance() between1 = between() #print " Distance to Waypoint: %s" % dist1 #print "Distance to Enemy: %s" % between1 ser.write("Distance to Waypoint: %s\n" % dist1) ser.write("Distance to Enemy: %s\n" % between1) time.sleep(2) La = float(vehicle.location.global_relative_frame.lat) la = float(vehicle.location.global_relative_frame.lon) Lb = float(elat) lb = float(elon) int(La) int(la) int(Lb) int(lb) U = math.cos(math.radians(Lb))*math.sin(math.radians(lb-la)) T = math.cos(math.radians(La))*math.sin(math.radians(Lb))-math.sin(math.radians(La))*math.cos(math.radians(Lb))*math.cos(math.radians(lb-la)) Bearing = math.atan2(U,T) if Bearing < 0: Hdg = math.degrees(Bearing)+360 else: Hdg = math.degrees(Bearing) conditionyaw(Hdg) while True: print " Heading: ", vehicle.heading ser.write("Heading: %s\n" % vehicle.heading) if Hdg - 5 <= vehicle.heading and Hdg + 5 >= vehicle.heading: print "Lock-on" ser.write("Lock-on\n") break #check if in the correct position ############################################## #need to add the function to determine if abs(between1) < 100 and abs(dist1) < 2: # Update the numbers is this necessary? abs(ealt - vehicle.location.global_relative_frame.alt) <= 1 submode = "intercept" ser.write("Switching to intercept\n") break else: ser.write("Need new set of coords\n") [Name, x, y, z] = rec_full_data("WP") [Name, elat, elon, ealt] = rec_full_data("EnemyWP")
def __init__(self, home_lat=0.0, home_lon=0.0, home_alt=0.0): self.Home = LocationGlobal(home_lat, home_lon, home_alt)
vehicle = connect('tcp:127.0.0.1:5763', wait_ready=True) cmds = vehicle.commands vehicle.initialize(4, heartbeat_timeout=120) # let initialization and pre-arming checks happen while not vehicle.is_armable: print('initializing vehicle') time.sleep(1) #ensure that the vehicle is in the proper autopilot mode for guidance vehicle.mode = VehicleMode('GUIDED') vehicle.armed = True # give time to arm no idea why same variable used for check #TODO figure out why were checking same variable that we just set while not vehicle.armed: print('Arming vehicle') time.sleep(1) #takeoff, hover for 10 seconds and then land vehicle.simple_takeoff(15) vehicle.wait_for_alt(15) vehicle.simple_goto(LocationGlobal(41.881829, -87.630875, 20)) vehicle.w print('taking off') vehicle.mode = VehicleMode('LAND') time.sleep(10) vehicle.close()
time.sleep(1) print "Keeping motors armed for 5s" time.sleep(5) print "Disarming" vehicle.armed = False #disarm vehicle time.sleep(1) vehicle.armed = True #re-armed #simple take-off if vehicle.armed == False: print "wait until armed" print "--Ready for flight--" vehicle.simple_takeoff(15) vehicle.airspeed = 10 #m/s vehicle.simple_goto(LocationGlobal(-32.8,152.1)) time.sleep(10) vehicle.mode = VehicleMode("LOITER") #control servo test #sending MAVLINK commands to simulated drone msg = vehicle.message_factory.command_long_encode( 0, 0, # target_system, target_component (target_component is set to 0) mavutil.mavlink.MAV_CMD_DO_SET_SERVO, #command 0, #confirmation 1, # param 1, servo number 1100, # param 2, PWM (1000-2000) 0, 0, 0, 0, 0) # param 3 ~ 7 not used #send command to vehicle vehicle.send_mavlink(msg) time.sleep(2)
############################## ##Send drone home at the altitude set in the "Home Point" section a = float(home_lat - vehicle.location.global_relative_frame.lat) b = float(home_lon - vehicle.location.global_relative_frame.lon) c = float(home_alt - vehicle.location.global_relative_frame.alt) d = vehicle.location.global_relative_frame.lat e = vehicle.location.global_relative_frame.lon f = vehicle.location.global_relative_frame.alt int(a) int(b) int(c) ############################## Home = LocationGlobal (vehicle.location.global_frame.lat+a, vehicle.location.global_frame.lon+b, vehicle.location.global_frame.alt+c) gotoHome(Home) ##Slow decent to set alt, example: RTL("##") where "##" is the landing altitude, currently set >0 to account for landing gear RTL(.10) print "Landing Alt Reached" ser.write("Landing Alt Reached\n") ##Countdown warning before drone powers off props count = 5 while count>0: print count ser.write("%s" % count) count = count-1 time.sleep(1.5)
def intercept(): while True: global x, y, z, key, vel, accel, correction_x, correction_y, correction_z, trkx, trky global edist, elat, elon, ealt, submode, yaw, turn, between, climb, red, meh, loft, homedist climb = vehicle.location.global_frame.alt meh = 3 home1 = homedist() if home1 >= 200: print "Vehicle is out of bounds" time.sleep(1) print "Vehicle is RTB" submode = "landing" break #print "gimme a key" ser.write("Key Request\n") while True: [Name, key] = rec_char("key") break print "climb: %s" % climb #print home_lat #print home_lon #print home_alt #key=msvcrt.getch() print "key is %s" % key yaw = vehicle.heading between1 = between() d1 = between1 start = time.time() time.sleep(1) between2 = between() d2 = between2 end = time.time() tgt = d2-d1 vel = (tgt/(end-start))*.9 #print tgt #print trkx #print trky #print vel #tracking(vel*trkx, vel*trky,0,5) #not sure if we want drone to automatically go towards other enemy or make that a manual decision """ if abs(ealt - vehicle.location.global_relative_frame.alt) > 1 and abs(between) > 30: submode = "goto" #ser.write("Acquired new target") break """ #if between > 5: #turn = 15 #time = 1 #elif between<5: #turn = 5 #time = .5 if key == "a": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (s_lat*.00005), vehicle.location.global_relative_frame.lon - (s_lon*.00005), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "d": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat - (s_lat*.00005), vehicle.location.global_relative_frame.lon + (s_lon*.00005), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "w": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (f_lat*.00005), vehicle.location.global_relative_frame.lon + (f_lon*.00005), vehicle.location.global_frame.alt) gotoGPS(newLoc) elif key == "s": strafe() newLoc = LocationGlobal (vehicle.location.global_relative_frame.lat + (b_lat*.00005), vehicle.location.global_relative_frame.lon + (b_lon*.00005), vehicle.location.global_frame.alt) gotoGPS(newLoc) #makes the drone fly down 1 m elif key == "z": loft = climb - meh #lift = lift - 3 print "going down" ser.write("Decending") newLoc = LocationGlobal (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon , loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt <= loft + 1: #Can remove the hard number if mission planner waypoint radius is changable print "check" break #makes the drone fly up 1 m elif key == "x": loft = climb + meh print "going up" ser.write("Acending") newLoc = LocationGlobal (vehicle.location.global_frame.lat, vehicle.location.global_frame.lon, loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt >= loft - 1: #Can remove the hard number if mission planner waypoint radius is changable print "check" break #p leaves function elif key == "p": #Used to track another target #Get WP and enemy's WP submode = "goto" break elif key == "t": print "Is this a Real World Test? (Y/N)" safe=msvcrt.getch() if safe == "y": print "set new tgt. (N,S,E,W)" red=msvcrt.getch() print "red is %s" % red if red == "n": elat = 39.793893 elon = -84.17082 ealt = 10 elif red == "s": elat = 39.793723 elon = -84.171005 ealt = 10 elif red == "e": elat = 39.793708 elon = -84.170764 ealt = 10 elif red == "w": elat = 39.793908 elon = -84.17106 ealt = 10 elif red == "h": elat = 39.793805 elon = -84.170903 elat = 10 else: print "set new tgt. (N,S,E,W)" red=msvcrt.getch() print "red is %s" % red if red == "n": elat = -35.362339 elon = 149.165204 ealt = 10 elif red == "s": elat = -35.364806 elon = 149.165244 ealt = 10 elif red == "e": elat = -35.36311 elon = 149.166904 ealt = 10 elif red == "w": elat = -35.36315 elon = 149.163761 ealt = 10 elif red == "h": elat = -35.363261 elon = 149.1652299 ealt = 10 elif key == "l": strafe() loft = climb + meh print "Engaging!" ser.write("Engaging!\n") newLoc = LocationGlobal (vehicle.location.global_frame.lat + (b_lat*.00003), vehicle.location.global_frame.lon + (b_lon*.00003), loft) gotoGPS(newLoc) while True: if vehicle.location.global_frame.alt >= loft - 1: #Can remove the hard number if mission planner waypoint radius is changable break #n releases another net elif key == "n": pwm.start(10) time.sleep(10) pwm.stop submode = "goto" break elif key == "/": submode = "landing" break #tracking(vel*trkx, vel*trky, 0, 1) #print " Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) #ser.write("Current altitude is: %s\n" % vehicle.location.global_relative_frame.alt) La = float(vehicle.location.global_relative_frame.lat) la = float(vehicle.location.global_relative_frame.lon) Lb = float(elat) lb = float(elon) int(La) int(la) int(Lb) int(lb) U = math.cos(math.radians(Lb))*math.sin(math.radians(lb-la)) T = math.cos(math.radians(La))*math.sin(math.radians(Lb))-math.sin(math.radians(La))*math.cos(math.radians(Lb))*math.cos(math.radians(lb-la)) Bearing = math.atan2(U,T) if Bearing < 0: Hdg = math.degrees(Bearing)+360 else: Hdg = math.degrees(Bearing) #print Bearing conditionyaw(Hdg) while True: print " Heading: ", vehicle.heading ser.write("Heading: %s\n" % vehicle.heading) if Hdg - 5 <= vehicle.heading and Hdg + 5 >= vehicle.heading: print "Lock-on" break print "Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) print vehicle.heading print "tgt is located at %s" % Hdg ser.write("Tgt is located at %s\n" % Hdg) ser.write("Vehicle heading is %s\n" % vehicle.heading)
def traveling(): count = 1 while True: global x, y, z, key, vel, accel, correction_x, correction_y, correction_z, trkx, trky global elat, elon, ealt, submode, edist, between, homedist home1 = homedist() if home1 >= 200: print "Vehicle is out of bounds" time.sleep(1) print "Vehicle is RTB" submode = "landing" break a = float(x - vehicle.location.global_relative_frame.lat) b = float(y - vehicle.location.global_relative_frame.lon) c = float(z - vehicle.location.global_relative_frame.alt) int(a) int(b) int(c) newLoc = LocationGlobal (vehicle.location.global_frame.lat + a, vehicle.location.global_frame.lon + b, vehicle.location.global_frame.alt + c) gotoGPS(newLoc) print " Current Location: Lat:%s, Lon:%s, Alt:%s" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt) print " Enroute to Lat:%s, Lon:%s, Alt:%s" % (x,y,z) ser.write("Current Location: Lat:%s, Lon:%s, Alt:%s\n" % (vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt)) ser.write("Enroute to Lat:%s, Lon:%s, Alt:%s\n" % (x,y,z)) #time.sleep(3) #print " Check Current Alt:%s" % vehicle.location.global_relative_frame.alt #print " Heading: {}".format(vehicle.heading) dist1 = distance() between1 = between() print " Distance to Waypoint: %s" % dist1 print "Distance to Enemy: %s" % between1 ser.write("Distance to Waypoint: %s\n" % dist1) ser.write("Distance to Enemy: %s\n" % between1) time.sleep(2) La = float(vehicle.location.global_relative_frame.lat) la = float(vehicle.location.global_relative_frame.lon) Lb = float(elat) lb = float(elon) int(La) int(la) int(Lb) int(lb) U = math.cos(math.radians(Lb))*math.sin(math.radians(lb-la)) T = math.cos(math.radians(La))*math.sin(math.radians(Lb))-math.sin(math.radians(La))*math.cos(math.radians(Lb))*math.cos(math.radians(lb-la)) Bearing = math.atan2(U,T) if Bearing < 0: Hdg = math.degrees(Bearing)+360 else: Hdg = math.degrees(Bearing) #print Bearing conditionyaw(Hdg) while True: print " Heading: ", vehicle.heading ser.write("Heading: %s\n" % vehicle.heading) if Hdg - 5 <= vehicle.heading and Hdg + 5 >= vehicle.heading: print "Lock-on" ser.write("Lock-on") break #check if in the correct position ############################################## #need to add the function to determine if abs(ealt - vehicle.location.global_relative_frame.alt) <= 1 and abs(between) < 100 and abs(distance) < 2: # Update the numbers for submode = "intercept" break elif: [Name, x, y, z] = rec_full_data("WP%s" % count) ser.write("Latitude is: %s\n" % x) ser.write("Longitude is: %s\n" % y) ser.write("Altitude is: %s\n" % z) ser.write("Type is: %s\n" % Name) [Name, elat, elon, ealt] = rec_full_data("EnemyWP") ser.write("Enemy Latitude is: %s\n" % elat) ser.write("Enemy Longitude is: %s\n" % elon) ser.write("Enemy Altitude is: %s\n" % ealt) ser.write("Type is: %s\n" % Name) count = count + 1 if count >= 2: count = 2
print "vehicle path not specified" sys.exit(1) #abort cause of error.. # Connect to the Vehicle. # Set `wait_ready=True` to ensure default attributes are populated before `connect()` return print "\nConnecting to vehicle on: %s" % connection_string vehicle = connect(connection_string, wait_ready=True, baud=args.baudrate) v = Vehicle_params() param = Params(vehicle, v) #get home position ... home_position = v.home_location logging.info("home position is %s", home_position) PositionVector.set_home_location( LocationGlobal(home_position.lat, home_position.lon, home_position.alt)) arm_and_takeoff(vehicle, 5) vehicle.simple_goto(pos1.get_location()) #set_yaw(vehicle,0) #set yaw while True: pos = PositionVector.get_from_location(vehicle.location.global_frame) logging.debug("vehicle_pos in N-S direction from home is %f" % pos.x) logging.debug("vehicle_pos in W-E direction from home is %f" % pos.y) print "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1) logging.debug( "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame),
tf.euler_from_matrix(H_T265Ref_T265body, 'sxyz')) * 180 / m.pi)) progress("DEBUG: NED RPY[deg]: {}".format( np.array( tf.euler_from_matrix(H_aeroRef_aeroBody, 'sxyz')) * 180 / m.pi)) progress("DEBUG: Raw pos xyz : {}".format( np.array([ data.translation.x, data.translation.y, data.translation.z ]))) progress("DEBUG: NED pos xyz : {}".format( np.array( tf.translation_from_matrix(H_aeroRef_aeroBody)))) conn.home_location = LocationGlobal(home_lat, home_lon, home_alt) arm_and_takeoff(0) except Exception as e: progress(e) except: send_msg_to_gcs('ERROR IN SCRIPT') progress("Unexpected error: %s" % sys.exc_info()[0]) finally: progress('Closing the script...') # start a timer in case stopping everything nicely doesn't work. signal.setitimer(signal.ITIMER_REAL, 5) # seconds... pipe.stop() #COMMENTED OUT 2/27
def wait_home_location(self, id, mav_connection): print("Waiting for APM home location from aircraft %s" % id) msg = mav_connection.recv_match(type="MISSION_ITEM", blocking=True) home_location = LocationGlobal(msg.x, msg.y, msg.z) print("Waiting for APM home location from aircraft %s" % id) return home_location