def landDrones(self, ignoreInterrupt=False):
if not (self.appController.trajectoryEnabled or self.appController.positioningEnabled):
return
Logger.log("Landing drones...")
swarmController = self.appController.swarmController
commander = swarmController.broadcaster.high_level_commander
light_controller = swarmController.broadcaster.light_controller
light_controller.set_color(32, 32, 32, 4.0, True)
commander.land(SequenceController.LANDING_HEIGHT, 4.0)
for drone in swarmController.connectedDrones:
if (drone.state == DroneState.IN_FLIGHT):
drone.state = DroneState.LANDING
self.appController.sequenceUpdated.emit()
threadUtil.interruptibleSleep(4.5, ignoreInterrupt)
light_controller.set_color(0, 0, 0, 0.25, True)
commander.stop()
for drone in swarmController.connectedDrones:
if (drone.state == DroneState.LANDING):
drone.state = DroneState.CONNECTED
self.appController.sequenceUpdated.emit()
exceptionUtil.checkInterrupt(ignoreInterrupt)
def setInitialPosition(self, drone):
exceptionUtil.checkInterrupt()
sequence = SequenceController.CURRENT
track = sequence.getTrack(drone.swarmIndex)
commander = drone.commander
self.appController.updateSequence()
exceptionUtil.checkInterrupt()
if track is not None:
r, g, b = sequence.getStartingColor(drone.swarmIndex)
x, y, z = sequence.getStartingPosition(drone.swarmIndex)
drone.light_controller.set_color(r, g, b, 0.25, True)
if self.appController.trajectoryEnabled or self.appController.positioningEnabled:
Logger.log("Getting into position", drone.swarmIndex)
# Step 0 - Make sure the drone has reached its takeoff position
initialX, initialY, initialZ = drone.startPosition
print("Waiting for drone", drone.swarmIndex, "to converge to position", ("({:.2f}, {:.2f}, {:.2f})".format(initialX, initialY, SequenceController.MIN_HEIGHT)))
drone.waitForTargetPosition(initialX, initialY, SequenceController.MIN_HEIGHT)
# Step 1 - Move to some height determined by index
targetHeight = min(SequenceController.MIN_HEIGHT + (drone.swarmIndex * 0.1), SequenceController.MAX_HEIGHT)
numDrones = len(self.appController.swarmController.connectedDrones)
moveTime = 5.0 + (numDrones * 0.3)
print("Moving drone", drone.swarmIndex, "to position", ("({:.2f}, {:.2f}, {:.2f})".format(initialX, initialY, targetHeight)), "over time:", moveTime)
commander.go_to(initialX, initialY, targetHeight, 0, moveTime)
threadUtil.interruptibleSleep(moveTime + 0.25)
print("Waiting for drone", drone.swarmIndex, "to converge to position", ("({:.2f}, {:.2f}, {:.2f})".format(initialX, initialY, targetHeight)))
drone.waitForTargetPosition(initialX, initialY, targetHeight)
# Step 2 - Move to position, maintaining target height
print("Moving drone", drone.swarmIndex, "to position", ("({:.2f}, {:.2f}, {:.2f})".format(x, y, targetHeight)), "over time:", 8.0)
commander.go_to(x, y, targetHeight, 0, 8.0)
threadUtil.interruptibleSleep(8.25)
print("Waiting for drone", drone.swarmIndex, "to converge to position", ("({:.2f}, {:.2f}, {:.2f})".format(x, y, targetHeight)))
drone.waitForTargetPosition(x, y, targetHeight)
# Step 3 - Move to actual starting position
print("Moving drone", drone.swarmIndex, "to position", ("({:.2f}, {:.2f}, {:.2f})".format(x, y, z)), "over time:", 8.0)
commander.go_to(x, y, z, 0.0, 8.0)
threadUtil.interruptibleSleep(8.25)
print("Waiting for drone", drone.swarmIndex, "to converge to position", ("({:.2f}, {:.2f}, {:.2f})".format(x, y, z)))
drone.waitForTargetPosition(x, y, z)
Logger.log("Ready!", drone.swarmIndex)
self.appController.updateSequence()
exceptionUtil.checkInterrupt()
def run(self):
# Update the order of the recent sequences
if self.sequenceIndex:
self.sequences.insert(0, self.sequences.pop(self.sequenceIndex))
self.appController.sequenceOrderUpdated.emit()
try:
Logger.log("SEQUENCE STARTING")
swarmController = self.appController.swarmController
# connect to required drones
numDrones = min(SequenceController.CURRENT.drones, self.appController.availableDrones)
self.appController.swarmController.connectSwarm(numDrones)
self.appController.sequenceUpdated.emit()
threadUtil.interruptibleSleep(0.25)
# ensure all drones have updated light house info & know their positions
uploadLighthouseData = self.appController.trajectoryEnabled or self.appController.positioningEnabled
swarmController.initializeSensors(uploadLighthouseData)
self.appController.sequenceUpdated.emit()
threadUtil.interruptibleSleep(0.25)
# get all drones in position
swarmController.parallel(self.uploadFlightData)
self.synchronizedTakeoff()
swarmController.parallel(self.setInitialPosition)
threadUtil.interruptibleSleep(0.25)
# kick off the actual sequence action4
self.runSequence()
# land all drones
self.landDrones()
# Wrap up
self.completeSequence()
except SequenceInterrupt:
Logger.log("ABORTING SEQUENCE")
self.abort()
return
except ConnectionAbortedError:
print("-- Drone connection failed, cancelling sequence --")
self.appController.connectionFailed.emit()
return
# Re-throw all other exceptions
except Exception:
raise
def initialize(self, address, disconnectCallback):
Logger.log("Connecting to address " + address, self.swarmIndex)
crazyflie = Crazyflie(ro_cache='./cache', rw_cache='./cache')
syncCrazyflie = SyncCrazyflie(address, cf=crazyflie)
crazyflie.connection_lost.add_callback(disconnectCallback)
syncCrazyflie.open_link()
self.crazyflie = syncCrazyflie
self.commander = syncCrazyflie.cf.high_level_commander
self.light_controller = syncCrazyflie.cf.light_controller
self.light_controller.set_color(0, 0, 0, 0.0, True)
threadUtil.interruptibleSleep(0.5)
self.state = DroneState.INITIALIZING
self.light_controller.set_color(255, 200, 0, 0.0, True)
def connectSwarm(self, numDrones=None):
toConnect = []
if numDrones and (isinstance(numDrones, int)
or numDrones.is_integer()):
totalCount = len(self.availableDrones)
desiredCount = totalCount if numDrones < 0 else numDrones
end = max(0, min(desiredCount, totalCount))
toConnect = self.availableDrones[:end]
Logger.log("Attempting to open " + str(len(toConnect)) +
" drone connections")
self.broadcaster = Broadcaster(self.channel)
self.broadcaster.open_link()
self.connectedDrones = self.parallel(self.connectToDrone, toConnect)
Logger.log("Successfully opened " + str(len(self.connectedDrones)) +
" drone connections")
def runSequence(self):
Logger.log("Starting flight paths!")
swarmController = self.appController.swarmController
commander = swarmController.broadcaster.high_level_commander
light_controller = swarmController.broadcaster.light_controller
duration = SequenceController.CURRENT.duration
# Start the automated trajectory
if self.appController.trajectoryEnabled:
commander.start_trajectory(Drone.TRAJECTORY_ID)
# Start the automated LED sequence
if self.appController.colorSequenceEnabled:
light_controller.set_effect(RingEffect.TIMING_EFFECT)
self.appController.startTimer.emit()
threadUtil.interruptibleSleep(duration + 0.25)
def uploadFlightData(self, drone):
sequence = SequenceController.CURRENT
track = sequence.getTrack(drone.swarmIndex)
exceptionUtil.checkInterrupt()
if track is not None:
Logger.log("Uploading trajectory & LED data", drone.swarmIndex)
if self.appController.trajectoryEnabled:
trajectoryData = bytearray(track['CompressedTrajectory'])
drone.writeTrajectory(trajectoryData)
exceptionUtil.checkInterrupt()
if self.appController.colorSequenceEnabled:
lightData = bytearray(track['LedTimings'])
drone.writeLedTimings(lightData)
exceptionUtil.checkInterrupt()
def updateSensors(self, uploadGeometry, geometryOne, geometryTwo):
Logger.log("Updating sensor & positioning data", self.swarmIndex)
self.crazyflie.cf.param.set_value('lighthouse.method', '0')
self.crazyflie.cf.param.set_value('lighthouse.systemType', '1')
# PID controller
self.crazyflie.cf.param.set_value('stabilizer.controller', '0')
self.crazyflie.cf.param.set_value('commander.enHighLevel', '1')
exceptionUtil.checkInterrupt()
if uploadGeometry:
self.writeBaseStationData(geometryOne, geometryTwo)
self.resetEstimator()
self.state = DroneState.CONNECTED
self.light_controller.set_color(0, 255, 0, 0.0, True)
exceptionUtil.checkInterrupt()
def synchronizedTakeoff(self):
if not (self.appController.trajectoryEnabled or self.appController.positioningEnabled):
return
Logger.log("Taking off")
swarmController = self.appController.swarmController
commander = swarmController.broadcaster.high_level_commander
light_controller = swarmController.broadcaster.light_controller
light_controller.set_color(0, 127, 255, 0.1, True)
commander.takeoff(SequenceController.MIN_HEIGHT, 4.0)
print("Taking off to height:", SequenceController.MIN_HEIGHT, ", over time:", 4.0)
for drone in swarmController.connectedDrones:
drone.state = DroneState.IN_FLIGHT
self.appController.sequenceUpdated.emit()
threadUtil.interruptibleSleep(4.0)
def completeSequence(self):
Logger.log("SEQUENCE COMPLETE")
self.appController.swarmController.disconnectSwarm()
self.appController.onSequenceCompleted()
self.appController.sequenceUpdated.emit()