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
