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 onDisconnect(self, uri, errorMessage):
Logger.error(errorMessage)
if uri in self.droneMapping:
drone = self.droneMapping[uri]
drone.state = DroneState.DISCONNECTED
self.appController.droneDisconnected.emit(uri)
self.appController.sequenceUpdated.emit()
def __init__(self, mainWindow, appSettings):
super().__init__()
Logger.initialize()
self.clearSequence = False
self.sequencePlaying = False
self.appSettings = appSettings
self.mainWindow = mainWindow
self.sequenceController = SequenceController(self, appSettings)
self.initializePositioning()
self.initializeSwarm()
self.scanInProgress = False
self.sequenceTimer = QTimer()
self.sequenceTimer.timeout.connect(self.updateSequence)
self.sequenceTimer.setInterval(20) # in MS
self.elapsedTime = 0
self.sequenceProgress = 0
self.startTimestamp = None
self.killTimer = QTimer()
self.killTimer.timeout.connect(self.hardKill)
self.killTimer.setSingleShot(True)
self.startTimer.connect(self.startSequenceTimer)
self.sequenceFinished.connect(self.stopSequenceTimer)
self.droneDisconnected.connect(self.onDroneDisconnected)
self.sequenceOrderUpdated.connect(self.saveSequenceSettings)
self.connectionFailed.connect(self.onConnectionFailed)
self.scanForDrones()
def writeLedTimings(self, color_data):
mems = self.crazyflie.cf.mem.get_mems(
MemoryElement.TYPE_DRIVER_LEDTIMING)
if self.crazyflie is None or len(color_data) == 0:
Logger.error("Could not upload LED data", self.swarmIndex)
raise ValueError("Cannot write led timing data")
self.write(lambda: mems[0].write_raw(color_data, self.writeComplete))
exceptionUtil.checkInterrupt()
def waitForEstimator(self):
startTime = time.time()
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
log_config.add_variable('kalman.stateX', 'float')
log_config.add_variable('kalman.stateY', 'float')
log_config.add_variable('kalman.stateZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(self.crazyflie, log_config) as logger:
for log_entry in logger:
exceptionUtil.checkInterrupt()
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
x = data['kalman.stateX']
y = data['kalman.stateY']
z = data['kalman.stateZ']
has_converged = (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (max_z - min_z) < threshold
timed_out = (time.time() -
startTime) > Drone.ESTIMATOR_TIMEOUT_SEC
if has_converged:
print("Initial position for drone", self.swarmIndex,
"has converged, to position: ",
("({:.2f}, {:.2f}, {:.2f})".format(x, y, z)))
self.startPosition = (float(x), float(y), float(z))
break
elif timed_out:
Logger.error("Drone position sensor failed to update!",
self.swarmIndex)
self.setError()
raise DroneException(
"Drone did not achieve the target position")
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 __init__(self, appController, *args, **kwargs):
super().__init__(*args, **kwargs)
self.appController = appController
self.appController.sequenceSelected.connect(self.clearLog)
self.appController.sequenceStarted.connect(self.clearLog)
# self.appController.sequenceFinished.connect(self.clearLog)
self.appController.sequenceUpdated.connect(self.scrollBottom)
Logger.bind_handler(self.log)
self.layout = layoutUtil.createLayout(LayoutType.VERTICAL, self)
self.createTitle()
self.createLogPanel()
self.clearLog()
def writeTrajectory(self, data):
if self.crazyflie is None or len(data) == 0:
Logger.error("Could not upload trajectory data!", self.swarmIndex)
raise ValueError("Cannot write trajectory data")
trajectoryMemory = self.crazyflie.cf.mem.get_mems(
MemoryElement.TYPE_TRAJ)[0]
self.write(lambda: trajectoryMemory.write_raw(data, self.writeComplete,
self.writeFailed))
exceptionUtil.checkInterrupt()
self.crazyflie.cf.high_level_commander.define_trajectory_compressed(
Drone.TRAJECTORY_ID, 0)
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 waitForTargetPosition(self,
targetX,
targetY,
targetZ,
timeoutSeconds=None):
startTime = time.time()
log_config = LogConfig(name='Kalman Position', period_in_ms=50)
log_config.add_variable('kalman.stateX', 'float')
log_config.add_variable('kalman.stateY', 'float')
log_config.add_variable('kalman.stateZ', 'float')
threshold = 0.05
with SyncLogger(self.crazyflie, log_config) as logger:
for log_entry in logger:
exceptionUtil.checkInterrupt()
data = log_entry[1]
x = float(data['kalman.stateX'])
y = float(data['kalman.stateY'])
z = float(data['kalman.stateZ'])
has_converged = abs(targetX - x) < threshold and abs(
targetY - y) and abs(targetZ - z)
timed_out = (time.time() - startTime
) > timeoutSeconds if timeoutSeconds else False
if has_converged:
print("Target position for drone", self.swarmIndex,
"has converged. Target was:",
("({:.2f}, {:.2f}, {:.2f})".format(
targetX, targetY, targetZ)),
", converged to position: ",
("({:.2f}, {:.2f}, {:.2f})".format(x, y, z)))
break
elif timed_out:
Logger.error("Drone failed to reach target position!",
self.swarmIndex)
self.setError()
raise ConnectionAbortedError(
"Drone position was not able to converge!")
def completeSequence(self):
Logger.log("SEQUENCE COMPLETE")
self.appController.swarmController.disconnectSwarm()
self.appController.onSequenceCompleted()
self.appController.sequenceUpdated.emit()
