def move(plane, communicator, method):
timer = 0
stop = False
# Todo: Make an option for centralized/decentralized collision avoidance & movement
# Move the plan in a straight line to the direction of the target waypoint
while not stop and not plane.dead:
# Simulate time by delaying planes update. Will need this for GUI.
if defaultValues.SIMULATE_TIME:
time.sleep(defaultValues.DELAY)
timer += defaultValues.DELAY
# find distance to target location
clat = plane.cLoc.latitude
clon = plane.cLoc.longitude
calt = plane.cLoc.altitude
tlat = plane.tLoc.latitude
tlon = plane.tLoc.longitude
talt = plane.tLoc.altitude
# Todo: with each adjustment, calculate a distance travled
plane.cBearing = plane.tBearing
plane.cElevation = plane.tElevation
if plane.tdistance <= 20:
pass
straightLine.straightline(plane)
uav_positions = communicator.read(plane)
for elem in uav_positions:
distance = standardFuncs.totalDistance(plane.cLoc, elem["cLoc"])
if (distance < defaultValues.CRASH_DISTANCE and elem["ID"] != plane.id and elem["dead"] == False):
# time.sleep(random.uniform(0, .001)) # Just so that the console doesn't get screwed up
plane.dead = True
plane.killedBy = elem["ID"]
stop = True
elif elem["ID"] == plane.id and elem["dead"] == True:
plane.dead = True
plane.killedBy = elem["killedBy"]
stop = True
# Move through queue to next waypoint, or if done stop thread.
if (plane.tdistance < defaultValues.WAYPOINT_DISTANCE) and (plane.wpAchieved <= plane.numWayPoints):
plane.wpAchieved += 1
if plane.wpAchieved < plane.numWayPoints:
plane.nextwp()
if plane.wpAchieved >= plane.numWayPoints: stop = True
communicator.update(plane)
# Todo: make this pretty
if plane.dead: print "\r%-80s" % "UAV #%3i has crashed!!" % plane.id
if plane.wpAchieved == plane.numWayPoints: print "\r%-80s" % "UAV #%3i reached all waypoints." % plane.id
def generate_planes(
numPlanes,
numWayPoints,
gridSize,
communicator,
location=OUR_LOCATION,
):
plane = [] # Create list of planes
# Creates a set number of planes
for i in range(0, numPlanes):
plane.append(Plane())
plane[i].numWayPoints = numWayPoints
logging.info("UAV #%3i generated." % plane[i].id)
for j in range(0, plane[i].numWayPoints +
2): # +2 to git inital and previous location
waypoint = randomLocation(gridSize, location)
plane[i].wayPoints.append(waypoint)
plane[i].queue.put(plane[i].wayPoints[j])
logging.info("UAV #%3i generated waypoint #%i at: %s" %
(plane[i].id, (j + 1), waypoint))
plane[i].set_cLoc(plane[i].queue.get_nowait()
) # Set current location to first generated waypoint
plane[i].set_cLoc(plane[i].queue.get_nowait())
plane[i].nextwp() # and removes it from the queue
# Calculate current and target bearing (both set to equal initially)
plane[i].tBearing = standardFuncs.find_bearing(plane[i].cLoc,
plane[i].tLoc)
plane[i].cBearing = plane[i].tBearing
# Calculate current and target elevation angles (also equa)
plane[i].tElevation = standardFuncs.elevation_angle(
plane[i].cLoc, plane[i].tLoc)
plane[i].cElevation = plane[i].tElevation
# Calculate the three dimensional and two dimensional distance to target
plane[i].distance = standardFuncs.findDistance(plane[i].cLoc,
plane[i].tLoc)
plane[i].tdistance = standardFuncs.totalDistance(
plane[i].cLoc, plane[i].tLoc)
if IS_TEST:
print "Plane ID is", plane[i].id, "and has", plane[
i].numWayPoints, "waypoints"
print plane[i].wayPoints
communicator.startUp(plane[i])
return plane
def updateTelemetry(self, newLoc):
# Set the current location
self.pLoc = self.cLoc # Move current location to previous location
self.cLoc = newLoc # Set new current location
# Calculate new elevation
self.tBearing = standardFuncs.find_bearing(self.pLoc, self.tLoc)
self.tElevation = standardFuncs.elevation_angle(self.pLoc, self.tLoc)
# haversine's horizontal distance
self.distance = standardFuncs.findDistance(newLoc, self.tLoc)
# haversine's horizontal distance w/ vertical distance taken into account
self.tdistance = standardFuncs.totalDistance(newLoc, self.tLoc)
self.distanceTraveled += self.speed * self.args.DELAY
def updateTelemetry(self, newLoc):
# Set the current location
self.pLoc = self.cLoc # Move current location to previous location
self.cLoc = newLoc # Set new current location
# Calculate new elevation
self.tBearing = standardFuncs.find_bearing(self.pLoc, self.tLoc)
self.tElevation = standardFuncs.elevation_angle(self.pLoc, self.tLoc)
# haversine's horizontal distance
self.distance = standardFuncs.findDistance(newLoc, self.tLoc)
# haversine's horizontal distance w/ vertical distance taken into account
self.tdistance = standardFuncs.totalDistance(newLoc, self.tLoc)
self.distanceTraveled += self.speed * self.args.DELAY
def straightline (plane):
if plane.avoid:
target = plane.avoidanceWaypoint
logging.info ("UAV #3i is moving toward an avoidance waypoint" % plane.id)
else: target = plane.tLoc
speed = plane.speed # Get speed from plane
distanceTraveled = plane.speed * defaultValues.DELAY #Get frequency of updates.
plane.distanceTraveled += distanceTraveled #The total distance travelled.
# Calculate new position
position = vMath.vector(distanceTraveled, plane.cBearing, plane.cElevation)
new_lat = plane.cLoc.latitude + (position.x / standardFuncs.LATITUDE_TO_METERS)
new_lon = plane.cLoc.longitude + (position.y / standardFuncs.LONGITUDE_TO_METERS)
new_alt = plane.cLoc.altitude + position.z
newLoc = standardFuncs.loc(new_lat,new_lon,new_alt)
# Update current location, distance, total distance, target bearing,
newloc = standardFuncs.loc(new_lat,new_lon,new_alt)
plane.set_cLoc(newloc)
# Calculate new bearing
plane.cBearing = standardFuncs.find_bearing(plane.pLoc, plane.cLoc)
plane.cElevation = standardFuncs.elevation_angle(plane.pLoc, plane.cLoc)
# Calculate new elevation
plane.tBearing = standardFuncs.find_bearing(newLoc, plane.tLoc)
plane.tElevation = standardFuncs.elevation_angle(newLoc,plane.tLoc)
# haversine's horizontal distance
plane.distance = standardFuncs.findDistance(newLoc, plane.tLoc)
# haversine's horizontal distance w/ vertical distance taken into account
plane.tdistance = standardFuncs.totalDistance(newLoc, plane.tLoc)
def generate_planes(numPlanes, numWayPoints, gridSize, communicator, location=OUR_LOCATION, ):
plane = [] # Create list of planes
# Creates a set number of planes
for i in range(0, numPlanes):
plane.append(Plane())
plane[i].numWayPoints = numWayPoints
logging.info("UAV #%3i generated." % plane[i].id)
for j in range(0, plane[i].numWayPoints + 2): # +2 to git inital and previous location
waypoint = randomLocation(gridSize, location)
plane[i].wayPoints.append(waypoint)
plane[i].queue.put(plane[i].wayPoints[j])
logging.info("UAV #%3i generated waypoint #%i at: %s" % (plane[i].id, (j + 1), waypoint))
plane[i].set_cLoc(plane[i].queue.get_nowait()) # Set current location to first generated waypoint
plane[i].set_cLoc(plane[i].queue.get_nowait())
plane[i].nextwp() # and removes it from the queue
# Calculate current and target bearing (both set to equal initially)
plane[i].tBearing = standardFuncs.find_bearing(plane[i].cLoc, plane[i].tLoc)
plane[i].cBearing = plane[i].tBearing
# Calculate current and target elevation angles (also equa)
plane[i].tElevation = standardFuncs.elevation_angle(plane[i].cLoc, plane[i].tLoc)
plane[i].cElevation = plane[i].tElevation
# Calculate the three dimensional and two dimensional distance to target
plane[i].distance = standardFuncs.findDistance(plane[i].cLoc, plane[i].tLoc)
plane[i].tdistance = standardFuncs.totalDistance(plane[i].cLoc, plane[i].tLoc)
if IS_TEST:
print "Plane ID is", plane[i].id, "and has", plane[i].numWayPoints, "waypoints"
print plane[i].wayPoints
communicator.startUp(plane[i])
return plane
def move(plane, communicator, method):
timer = 0
stop = False
# Todo: Make an option for centralized/decentralized collision avoidance & movement
# Move the plan in a straight line to the direction of the target waypoint
while not stop and not plane.dead:
# Simulate time by delaying planes update. Will need this for GUI.
if defaultValues.SIMULATE_TIME:
time.sleep(defaultValues.DELAY)
timer += defaultValues.DELAY
# find distance to target location
clat = plane.cLoc.latitude
clon = plane.cLoc.longitude
calt = plane.cLoc.altitude
tlat = plane.tLoc.latitude
tlon = plane.tLoc.longitude
talt = plane.tLoc.altitude
# Todo: with each adjustment, calculate a distance travled
plane.cBearing = plane.tBearing
plane.cElevation = plane.tElevation
if plane.tdistance <= 20:
pass
straightLine.straightline(plane)
uav_positions = communicator.read(plane)
for elem in uav_positions:
distance = standardFuncs.totalDistance(plane.cLoc, elem["cLoc"])
if (distance < defaultValues.CRASH_DISTANCE
and elem["ID"] != plane.id and elem["dead"] == False):
# time.sleep(random.uniform(0, .001)) # Just so that the console doesn't get screwed up
plane.dead = True
plane.killedBy = elem["ID"]
stop = True
elif elem["ID"] == plane.id and elem["dead"] == True:
plane.dead = True
plane.killedBy = elem["killedBy"]
stop = True
# Move through queue to next waypoint, or if done stop thread.
if (plane.tdistance < defaultValues.WAYPOINT_DISTANCE) and (
plane.wpAchieved <= plane.numWayPoints):
plane.wpAchieved += 1
if plane.wpAchieved < plane.numWayPoints:
plane.nextwp()
if plane.wpAchieved >= plane.numWayPoints: stop = True
communicator.update(plane)
# Todo: make this pretty
if plane.dead: print "\r%-80s" % "UAV #%3i has crashed!!" % plane.id
if plane.wpAchieved == plane.numWayPoints:
print "\r%-80s" % "UAV #%3i reached all waypoints." % plane.id
