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 resetEstimator(self):
self.crazyflie.cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.25)
exceptionUtil.checkInterrupt()
self.crazyflie.cf.param.set_value('kalman.resetEstimation', '0')
self.startPosition = (0, 0, 0)
self.waitForEstimator()
def interruptibleSleep(duration, ignore=False, interval=0.25):
start = time.time()
elapsed = time.time() - start
while elapsed < duration:
sleepTime = max(min(interval, duration - elapsed), 0)
time.sleep(sleepTime)
exceptionUtil.checkInterrupt(ignore)
elapsed = time.time() - start
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 writeBaseStationData(self, geometryOne, geometryTwo):
geo_dict = {0: geometryOne, 1: geometryTwo}
helper = LighthouseMemHelper(self.crazyflie.cf)
writer = LighthouseConfigWriter(self.crazyflie.cf,
nr_of_base_stations=2)
self.write(lambda: helper.write_geos(geo_dict, self.writeComplete))
self.write(lambda: writer.write_and_store_config(self.writeComplete,
geos=geo_dict,
calibs=calibration.
CALIBRATION_DATA))
exceptionUtil.checkInterrupt()
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 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 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 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 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()