Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
2 crazyflie swarm take off in HL
# -*- coding: utf-8 -*-
"""
Simple high level commander take-off with two crayzyflies. The result should be this: https://youtu.be/I5WXYlIme0k
"""
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
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 run_shared_sequence(scf):
commander = scf.cf.high_level_commander
commander.takeoff(1.0, 2.0)
time.sleep(6)
commander.land(0.0, 2.0)
time.sleep(2)
commander.stop()
uris = {
'radio://0/30/2M/E7E7E7E702',
'radio://0/30/2M/E7E7E7E703',
# 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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment