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 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 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 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!")