import time import cflib.crtp from cflib.crazyflie.log import LogConfig from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm from cflib.crazyflie.syncLogger import SyncLogger from cflib.crazyflie.log import LogConfig from cflib.crazyflie.mem import MemoryElement from cflib.crazyflie.mem import Poly4D from cflib.crazyflie.syncCrazyflie import SyncCrazyflie URI1 = 'radio://0/80/2M/E7E7E7E701' URI2 = 'radio://0/80/2M/E7E7E7E702' figure8 = [ [1.050000, 0.000000, -0.000000, 0.000000, -0.000000, 0.830443, -0.276140, -0.384219, 0.180493, -0.000000, 0.000000, -0.000000, 0.000000, -1.356107, 0.688430, 0.587426, -0.329106, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.710000, 0.396058, 0.918033, 0.128965, -0.773546, 0.339704, 0.034310, -0.026417, -0.030049, -0.445604, -0.684403, 0.888433, 1.493630, -1.361618, -0.139316, 0.158875, 0.095799, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.620000, 0.922409, 0.405715, -0.582968, -0.092188, -0.114670, 0.101046, 0.075834, -0.037926, -0.291165, 0.967514, 0.421451, -1.086348, 0.545211, 0.030109, -0.050046, -0.068177, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.700000, 0.923174, -0.431533, -0.682975, 0.177173, 0.319468, -0.043852, -0.111269, 0.023166, 0.289869, 0.724722, -0.512011, -0.209623, -0.218710, 0.108797, 0.128756, -0.055461, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.560000, 0.405364, -0.834716, 0.158939, 0.288175, -0.373738, -0.054995, 0.036090, 0.078627, 0.450742, -0.385534, -0.954089, 0.128288, 0.442620, 0.055630, -0.060142, -0.076163, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.560000, 0.001062, -0.646270, -0.012560, -0.324065, 0.125327, 0.119738, 0.034567, -0.063130, 0.001593, -1.031457, 0.015159, 0.820816, -0.152665, -0.130729, -0.045679, 0.080444, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.700000, -0.402804, -0.820508, -0.132914, 0.236278, 0.235164, -0.053551, -0.088687, 0.031253, -0.449354, -0.411507, 0.902946, 0.185335, -0.239125, -0.041696, 0.016857, 0.016709, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.620000, -0.921641, -0.464596, 0.661875, 0.286582, -0.228921, -0.051987, 0.004669, 0.038463, -0.292459, 0.777682, 0.565788, -0.432472, -0.060568, -0.082048, -0.009439, 0.041158, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [0.710000, -0.923935, 0.447832, 0.627381, -0.259808, -0.042325, -0.032258, 0.001420, 0.005294, 0.288570, 0.873350, -0.515586, -0.730207, -0.026023, 0.288755, 0.215678, -0.148061, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa [1.053185, -0.398611, 0.850510, -0.144007, -0.485368, -0.079781, 0.176330, 0.234482, -0.153567, 0.447039, -0.532729, -0.855023, 0.878509, 0.775168, -0.391051, -0.713519, 0.391628, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa ] class Uploader: def __init__(self): self._is_done = False def upload(self, trajectory_mem): print('Uploading data') trajectory_mem.write_data(self._upload_done) while not self._is_done: time.sleep(0.2) def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True seq_args = { URI1: [figure8], URI2: [figure8], } # List of URIs, comment the one you do not want to fly uris = { URI1, URI2, } class Uploader: def __init__(self): self._is_done = False def upload(self, trajectory_mem): print('Uploading data') trajectory_mem.write_data(self._upload_done) while not self._is_done: time.sleep(0.2) def _upload_done(self, mem, addr): print('Data uploaded') self._is_done = True def wait_for_position_estimator(scf): print('Waiting for estimator to find position...') 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') var_y_history = [1000] * 10 var_x_history = [1000] * 10 var_z_history = [1000] * 10 threshold = 0.001 with SyncLogger(scf, log_config) as logger: for log_entry in logger: 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) # print("{} {} {}". # format(max_x - min_x, max_y - min_y, max_z - min_z)) if (max_x - min_x) < threshold and ( max_y - min_y) < threshold and ( max_z - min_z) < threshold: break def reset_estimator(scf): cf = scf.cf cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') wait_for_position_estimator(scf) def activate_high_level_commander(scf): scf.cf.param.set_value('commander.enHighLevel', '1') def activate_mellinger_controller(scf, use_mellinger): controller = 1 if use_mellinger: controller = 2 scf.cf.param.set_value('stabilizer.controller', controller) def upload_trajectory(scf, trajectory_id, trajectory): trajectory_mem = scf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] total_duration = 0 for row in trajectory: duration = row[0] x = Poly4D.Poly(row[1:9]) y = Poly4D.Poly(row[9:17]) z = Poly4D.Poly(row[17:25]) yaw = Poly4D.Poly(row[25:33]) trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) total_duration += duration Uploader().upload(trajectory_mem) scf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.poly4Ds)) return total_duration def run_shared_sequence(scf, trajectory_id, duration, seq_args): activate_mellinger_controller(scf, False) commander = cf.high_level_commander commander.takeoff(1.0, 2.0) time.sleep(3) relative = True commander.start_trajectory(trajectory_id, 1.0, relative) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) commander.stop() duration = upload_trajectory(cf,trajectory_id, figure8) #uris = { # 'radio://0/80/2M/E7E7E7E701', # 'radio://0/80/2M/E7E7E7E702', # Add more URIs if you want more copters in the swarm #} if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.parallel_safe(activate_high_level_commander) swarm.parallel_safe(reset_estimator) swarm.parallel_safe(run_shared_sequence)