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 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 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 setStart(self):
# get a previous LOCATION
self.pLoc = self.waypoints.get()
# get a current LOCATION
self.cLoc = self.waypoints.get()
self.nextwp() # and removes it from the queue
# Calculate current and target bearing (both set to equal initially)
self.tBearing = standardFuncs.find_bearing(self.cLoc, self.tLoc)
self.cBearing = self.tBearing
# logging.info("Initial bearing set to %.2f" % self.cBearing)
# Calculate current and target elevation angles (also equal)
self.tElevation = standardFuncs.elevation_angle(self.cLoc, self.tLoc)
self.cElevation = self.tElevation
# Calculate the three dimensional and two dimensional distance to target
self.tdistance = standardFuncs.findDistance(self.cLoc, self.tLoc)
def setStart(self):
# get a previous LOCATION
self.pLoc = self.waypoints.get()
# get a current LOCATION
self.cLoc = self.waypoints.get()
self.nextwp() # and removes it from the queue
# Calculate current and target bearing (both set to equal initially)
self.tBearing = standardFuncs.find_bearing(self.cLoc, self.tLoc)
self.cBearing = self.tBearing
# logging.info("Initial bearing set to %.2f" % self.cBearing)
# Calculate current and target elevation angles (also equal)
self.tElevation = standardFuncs.elevation_angle(self.cLoc, self.tLoc)
self.cElevation = self.tElevation
# Calculate the three dimensional and two dimensional distance to target
self.tdistance = standardFuncs.findDistance(self.cLoc, self.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
