Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
print("usage: {} [URI]".format(sys.argv[0]))
print(" URI - the URI to use to connect to the Crazyflie, default value is radio://0/80/2M/E7E7E7E7E7")
sys.exit(1)
uri = 'radio://0/80/2M/E7E7E7E7E7'
if len(sys.argv) > 1:
uri = sys.argv[1]
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(uri, cf=cf) as scf:
print("Setting positions")
for packet in yaml.load_all(sys.stdin, Loader=yaml.CLoader):
if not packet:
continue
while True:
for id, position in packet.items():
x = position['x']
y = position['y']
z = position['z']
print("Anchor {}, ({}, {}, {})".format(id, x, y, z))
LPP_SHORT_ANCHOR_POSITION = 0x01
position_pack = struct.pack("
def __init__(self, uri, velocity=0.2, threaded=False):
super().__init__(threaded)
# set the default velocity
self._velocity = velocity
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# the connection to the crazyflie
self._scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
# temp (or maybe will be permanent) state variable
self._is_open = False
self._running = False
self._send_rate = 50 # want to send messages at 5Hz NOTE: the minimum is 2 Hz
self._out_msg_queue = queue.Queue() # a queue for sending data between threads
self._write_handle = threading.Thread(target=self.command_loop)
self._write_handle.daemon = True
# since can only command velocities and not positions, the connection
# needs some awareness of the current position to be able to do
# the math necessary
self._current_position_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z]
# due to a "bug" in the crazyflie's position estimator with the flow
def flight_controller():
global scf
with SyncCrazyflie(uri) as _scf:
scf = _scf
# reset the kalman filter and find a new estimated position
reset_estimator(scf)
fly_along_path(scf, flight_path)
(power_conf, force, smart) = generate_config(args.power)
power_pack = generate_lpp_packet(power_conf, force, smart)
print('Sending anchor transmit power configuration to anchors (%i to %i) '
'using %s. Setting transmit power to %s%s. Anchors will reset'
'when configured.' % (
args.anchor_id_lower,
args.anchor_id_upper,
args.uri,
args.power, unit))
logging.basicConfig(level=logging.ERROR)
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(args.uri, cf=cf) as scf:
print('Starting transmission. Terminate with CTRL+C.')
while True:
for id in range(args.anchor_id_lower, args.anchor_id_upper + 1):
print('Anchor %i' % id)
for _ in range(7):
scf.cf.loc.send_short_lpp_packet(id, power_pack)
time.sleep(0.2)
time.sleep(0.5)