class _SharedRadio(Thread):
def __init__(self, devid: int):
Thread.__init__(self)
self._radio = Crazyradio(devid=devid)
self._devid = devid
self.version = self._radio.version
self._cmd_queue = Queue() # type: Queue[Tuple[int, _RadioCommands, Any]] # noqa
self._rsp_queues = {} # type: Dict[int, Queue[Any]]
self._next_instance_id = 0
self._lock = Semaphore(1)
self.start()
def open_instance(self) -> _SharedRadioInstance:
rsp_queue = Queue()
with self._lock:
instance_id = self._next_instance_id
self._rsp_queues[instance_id] = rsp_queue
self._next_instance_id += 1
return _SharedRadioInstance(instance_id,
self._cmd_queue,
rsp_queue,
self._radio.version)
def run(self):
while True:
command = self._cmd_queue.get()
if command[1] == _RadioCommands.STOP:
with self._lock:
del self._rsp_queues[command[0]]
if len(self._rsp_queues) == 0:
self._radio.close()
_RadioManager.remove(self._devid)
return
elif command[1] == _RadioCommands.SEND_PACKET:
channel, address, datarate, data = command[2]
self._radio.set_channel(channel)
self._radio.set_address(address)
self._radio.set_data_rate(datarate)
ack = self._radio.send_packet(data)
self._rsp_queues[command[0]].put(ack)
elif command[1] == _RadioCommands.SET_ARC:
self._radio.set_arc(command[2])
elif command[1] == _RadioCommands.SCAN_SELECTED:
datarate, address, selected, data = command[2]
self._radio.set_data_rate(datarate)
self._radio.set_address(address)
resp = self._radio.scan_selected(selected, data)
self._rsp_queues[command[0]].put(resp)
elif command[1] == _RadioCommands.SCAN_CHANNELS:
datarate, address, start, stop, packet = command[2]
self._radio.set_data_rate(datarate)
self._radio.set_address(address)
resp = self._radio.scan_channels(start, stop, packet)
self._rsp_queues[command[0]].put(resp)
def _send(self, cmd):
packet = (0xf3, 0xfe, cmd)
cr = Crazyradio(devid=self.devid)
cr.set_channel(self.channel)
cr.set_data_rate(self.datarate)
cr.set_address(self.address)
cr.set_arc(3)
success = False
for i in range(50):
res = cr.send_packet(packet)
if res.ack:
success = True
break
time.sleep(0.01)
cr.close()
if not success:
raise Exception('Failed to connect to Crazyflie at {}'.format(
self.uri))
import time
from cflib.drivers.crazyradio import Crazyradio
cr = Crazyradio(devid=1)
cr.set_channel(56)
cr.set_data_rate(cr.DR_2MPS)
while True:
# Send multicast packet to P2P port 7
cr.set_address((0xff, 0xe7, 0xe7, 0xe7, 0xe7))
# cr.set_ack_enable(False)
cr.send_packet((0xff, 0x80, 0x63, 0x00))
print('send')
time.sleep(0.01)
position[3])
t = T
if (abs(movement[0]/T) > VMAX or abs(movement[1]/T) > VMAX):
t = abs(movement[0]/VMAX) if abs(movement[0]/VMAX) > abs(movement[1]/VMAX) else abs(movement[1]/VMAX)
go_straight_d(cf, movement[0], movement[1], movement[2], t)
print('Moving: ({}, {}, {}) in time {}'.format(movement[0], movement[1], movement[2], t))
print('Now at: ({}, {}, {})'.format(position_internal[0], position_internal[1], position_internal[2]))
time.sleep(1)
for i in range(4):
position_internal[i] = position[i]
time.sleep(0.1)
if __name__ == '__main__':
cr = Crazyradio()
cr.send_packet((0xFF, 0x05, 0x0))
cr.close()
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
#time.sleep(0.5)
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) > 0:
with SyncCrazyflie('radio://0/80/2M/E7E7E7E7E9', cf=Crazyflie(rw_cache='./cache')) as scf:
class _RadioTransferThread(threading.Thread):
""" Thread that handles transfer for a single crazyradio hardware
Can handles transfers form more than one radio profile (ie. link to a copter)
"""
def __init__(self, radio_id):
threading.Thread.__init__(self)
self.cradio = Crazyradio(devid=radio_id)
if self.cradio.version >= 0.4:
self.cradio.set_arc(10)
else:
logger.warning("Radio version <0.4 will be obsoleted soon!")
self._num_profiles = 0
self.tx_queue = Queue.Queue()
self.sp = False
def add_profile(self):
self._num_profiles += 1
rx_queue = Queue.Queue()
return rx_queue
def remove_profile(self, handle):
# we don't need to to anything, the python garbage collector will take care of it
self._num_profiles -= 1
def num_profiles(self):
return self._num_profiles
def send_packet(self, profile, data):
self.tx_queue.put([profile, data])
return profile.handle.get()
def stop(self):
self.sp = True
self.tx_queue.put([None, None])
self.join()
def run(self):
#Simply service transfers requests
while not self.sp:
tx = self.tx_queue.get()
if self.sp:
break
ack = self._send_packet(tx[0], tx[1])
tx[0].handle.put(ack)
# Close the USB dongle
try:
if self.cradio:
self.cradio.close()
print("Closed radio")
except:
# If we pull out the dongle we will not make this call
pass
self.cradio = None
def _send_packet(self, profile, data):
"""
Send packet making sure the radio is configured for the
right transfers profile
"""
assert isinstance(profile, _RadioProfile)
if self.cradio.channel != profile.channel:
self.cradio.set_channel(profile.channel)
if self.cradio.data_rate != profile.rate:
self.cradio.set_data_rate(profile.rate)
if self.cradio.address != profile.address:
self.cradio.set_address(profile.address)
return self.cradio.send_packet(data)
from cflib.drivers.crazyradio import Crazyradio
cr = Crazyradio(devid=0)
cr.set_channel(56)
cr.set_power(cr.P_M18DBM)
cr.set_data_rate(cr.DR_2MPS)
while True:
# Send multicast packet to P2P port 7
cr.set_address((0xff, 0xe7, 0xe7, 0xe7, 0xe7))
cr.send_packet((0xff, 0x80, 0x64))
print('send')
class _RadioTransferThread(threading.Thread):
""" Thread that handles transfer for a single crazyradio hardware
Can handles transfers form more than one radio profile (ie. link to a copter)
"""
def __init__(self, radio_id):
threading.Thread.__init__(self)
self.cradio = Crazyradio(devid=radio_id)
if self.cradio.version >= 0.4:
self.cradio.set_arc(10)
else:
logger.warning("Radio version <0.4 will be obsoleted soon!")
self._num_profiles = 0
self.tx_queue = Queue.Queue()
self.sp = False
def add_profile(self):
self._num_profiles += 1
rx_queue = Queue.Queue()
return rx_queue
def remove_profile(self, handle):
# we don't need to to anything, the python garbage collector will take care of it
self._num_profiles -= 1
def num_profiles(self):
return self._num_profiles
def send_packet(self, profile, data):
self.tx_queue.put([profile, data])
return profile.handle.get()
def stop(self):
self.sp = True
self.tx_queue.put([None, None])
self.join()
def run(self):
#Simply service transfers requests
while not self.sp:
tx = self.tx_queue.get()
if self.sp:
break
ack = self._send_packet(tx[0], tx[1])
tx[0].handle.put(ack)
# Close the USB dongle
try:
if self.cradio:
self.cradio.close()
print("Closed radio")
except:
# If we pull out the dongle we will not make this call
pass
self.cradio = None
def _send_packet(self, profile, data):
"""
Send packet making sure the radio is configured for the
right transfers profile
"""
assert isinstance(profile, _RadioProfile)
if self.cradio.channel != profile.channel:
self.cradio.set_channel(profile.channel)
if self.cradio.data_rate != profile.rate:
self.cradio.set_data_rate(profile.rate)
if self.cradio.address != profile.address:
self.cradio.set_address(profile.address)
return self.cradio.send_packet(data)
import cflib.crtp
from cflib.drivers.crazyradio import Crazyradio
URI1 = '0xE7E7E7E7E0'
URI2 = '0xE7E7E7E7E1'
URI3 = '0xE7E7E7E7E2'
URI4 = 0xE7E7E7E7E3
URI5 = 0xE7E7E7E7E4
URI6 = 0xE7E7E7E7E5
URI7 = 0xE7E7E7E7E6
URI8 = '0xE7E7E7E7E7'
URI9 = 0xE7E7E7E7E8
URI10 = 0xE7E7E7E7E9
cflib.crtp.init_drivers(enable_debug_driver=False)
cr = Crazyradio()
cr.set_channel(100)
cr.set_data_rate(cr.DR_2MPS)
# cr.set_address(hex(0xE7E7E7E7E7))
cr.send_packet((0xff, 0xfe, 0xff)).ack # Init the reboot
# print cr.send_packet((0xff, 0xfe, 0xf0, 0)).ack # Reboot to Bootloader
while not cr.send_packet((0xff, 0xfe, 0xf0, 1)).ack: # Reboot to Firmware
#keep trying
print("trying again")
