#Basado en swarmSequence.py #Solo con LPS #All credits to Copyright (C) 2019 Bitcraze AB. 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 #URIs URI1 = 'radio://0/80/2M/E7E7E7E7E1' URI2 = 'radio://0/80/2M/E7E7E7E7E2' URI3 = 'radio://0/80/2M/E7E7E7E7E3' URI4 = 'radio://0/80/2M/E7E7E7E7E4' URI5 = 'radio://0/80/2M/E7E7E7E7E5' URI6 = 'radio://0/80/2M/E7E7E7E7E6' URI7 = 'radio://0/80/2M/E7E7E7E7E7' URI8 = 'radio://0/80/2M/E7E7E7E7E8' URI9 = 'radio://0/80/2M/E7E7E7E7E9' URI10 = 'radio://0/80/2M/E7E7E7E7EA' URI11 = 'radio://0/80/2M/E7E7E7E7EB' # x y z time sequence1 = [ (0.0, 0.0, 1.5, 5.0), (0.5,0.5,0.5,2.0) ] seq_args = { URI6: [sequence1] } # List of URIs, comment the one you do not want to fly uris = { #URI1, #URI2, #URI3, #URI4, #URI5, URI6 #URI7, #URI8, #URI9, #URI10, #URI11 } 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 wait_for_param_download(scf): while not scf.cf.param.is_updated: time.sleep(1.0) print('Parameters downloaded for', scf.cf.link_uri) 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(cf) def take_off(cf, position): take_off_time = 5.0 sleep_time = 0.1 steps = int(take_off_time / sleep_time) vz = position[2] / take_off_time print(vz) for i in range(steps): cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) time.sleep(sleep_time) def land(cf, position): landing_time = 5.0 sleep_time = 0.1 steps = int(landing_time / sleep_time) vz = -position[2] / landing_time print(vz) for _ in range(steps): cf.commander.send_velocity_world_setpoint(0, 0, vz, 0) time.sleep(sleep_time) cf.commander.send_stop_setpoint() # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def run_sequence(scf, sequence): try: cf = scf.cf take_off(cf, sequence[0]) for position in sequence: print('Setting position {}'.format(position)) end_time = time.time() + position[3] while time.time() < end_time: cf.commander.send_position_setpoint(position[0], position[1], position[2], 0) time.sleep(0.1) land(cf, sequence[-1]) except Exception as e: print(e) if __name__ == '__main__': # logging.basicConfig(level=logging.DEBUG) cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: # If the copters are started in their correct positions this is # probably not needed. The Kalman filter will have time to converge # any way since it takes a while to start them all up and connect. We # keep the code here to illustrate how to do it. # swarm.parallel(reset_estimator) # The current values of all parameters are downloaded as a part of the # connections sequence. Since we have 10 copters this is clogging up # communication and we have to wait for it to finish before we start # flying. print('Waiting for parameters to be downloaded...') swarm.parallel(wait_for_param_download) swarm.parallel(run_sequence, args_dict=seq_args)