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)
class RadioDriver(CRTPDriver):
""" Crazyradio link driver """
def __init__(self):
""" Create the link driver """
CRTPDriver.__init__(self)
self.cradio = None
self.uri = ""
self.link_error_callback = None
self.link_quality_callback = None
self.in_queue = None
self.out_queue = None
self._thread = None
self.needs_resending = True
def connect(self, uri, link_quality_callback, link_error_callback):
"""
Connect the link driver to a specified URI of the format:
radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
The callback for linkQuality can be called at any moment from the
driver to report back the link quality in percentage. The
callback from linkError will be called when a error occurs with
an error message.
"""
# check if the URI is a radio URI
if not re.search("^radio://", uri):
raise WrongUriType("Not a radio URI")
# Open the USB dongle
if not re.search("^radio://([0-9]+)((/([0-9]+))"
"((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$", uri):
raise WrongUriType('Wrong radio URI format!')
uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
"((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$", uri)
self.uri = uri
channel = 2
if uri_data.group(4):
channel = int(uri_data.group(4))
datarate = Crazyradio.DR_2MPS
if uri_data.group(7) == "250K":
datarate = Crazyradio.DR_250KPS
if uri_data.group(7) == "1M":
datarate = Crazyradio.DR_1MPS
if uri_data.group(7) == "2M":
datarate = Crazyradio.DR_2MPS
if self.cradio is None:
self.cradio = Crazyradio(devid=int(uri_data.group(1)))
else:
raise Exception("Link already open!")
if self.cradio.version >= 0.4:
self.cradio.set_arc(10)
else:
logger.warning("Radio version <0.4 will be obsoleted soon!")
self.cradio.set_channel(channel)
self.cradio.set_data_rate(datarate)
if uri_data.group(9):
addr = str(uri_data.group(9))
new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
self.cradio.set_address(new_addr)
# Prepare the inter-thread communication queue
self.in_queue = queue.Queue()
# Limited size out queue to avoid "ReadBack" effect
self.out_queue = queue.Queue(1)
# Launch the comm thread
self._thread = _RadioDriverThread(self.cradio, self.in_queue,
self.out_queue,
link_quality_callback,
link_error_callback,
self)
self._thread.start()
self.link_error_callback = link_error_callback
def receive_packet(self, time=0):
"""
Receive a packet though the link. This call is blocking but will
timeout and return None if a timeout is supplied.
"""
if time == 0:
try:
return self.in_queue.get(False)
except queue.Empty:
return None
elif time < 0:
try:
return self.in_queue.get(True)
except queue.Empty:
return None
else:
try:
return self.in_queue.get(True, time)
except queue.Empty:
return None
def send_packet(self, pk):
""" Send the packet pk though the link """
# if self.out_queue.full():
# self.out_queue.get()
if (self.cradio is None):
return
try:
self.out_queue.put(pk, True, 2)
except queue.Full:
if self.link_error_callback:
self.link_error_callback("RadioDriver: Could not send packet"
" to copter")
def pause(self):
self._thread.stop()
self._thread = None
def restart(self):
if self._thread:
return
self._thread = _RadioDriverThread(self.cradio, self.in_queue,
self.out_queue,
self.link_quality_callback,
self.link_error_callback,
self)
self._thread.start()
def close(self):
""" Close the link. """
# Stop the comm thread
self._thread.stop()
# Close the USB dongle
try:
if self.cradio:
self.cradio.close()
except:
# If we pull out the dongle we will not make this call
pass
self.cradio = None
while not self.out_queue.empty():
self.out_queue.get()
# Clear callbacks
self.link_error_callback = None
self.link_quality_callback = None
def _scan_radio_channels(self, start=0, stop=125):
""" Scan for Crazyflies between the supplied channels. """
return list(self.cradio.scan_channels(start, stop, (0xff,)))
def scan_selected(self, links):
to_scan = ()
for l in links:
one_to_scan = {}
uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
"(/(250K|1M|2M))?)?$",
l)
one_to_scan["channel"] = int(uri_data.group(4))
datarate = Crazyradio.DR_2MPS
if uri_data.group(6) == "250K":
datarate = Crazyradio.DR_250KPS
if uri_data.group(6) == "1M":
datarate = Crazyradio.DR_1MPS
if uri_data.group(6) == "2M":
datarate = Crazyradio.DR_2MPS
one_to_scan["datarate"] = datarate
to_scan += (one_to_scan,)
found = self.cradio.scan_selected(to_scan, (0xFF, 0xFF, 0xFF))
ret = ()
for f in found:
dr_string = ""
if f["datarate"] == Crazyradio.DR_2MPS:
dr_string = "2M"
if f["datarate"] == Crazyradio.DR_250KPS:
dr_string = "250K"
if f["datarate"] == Crazyradio.DR_1MPS:
dr_string = "1M"
ret += ("radio://0/{}/{}".format(f["channel"], dr_string),)
return ret
def scan_interface(self, address):
""" Scan interface for Crazyflies """
if self.cradio is None:
try:
self.cradio = Crazyradio()
except Exception:
return []
else:
raise Exception("Cannot scann for links while the link is open!")
# FIXME: implements serial number in the Crazyradio driver!
serial = "N/A"
logger.info("v%s dongle with serial %s found", self.cradio.version,
serial)
found = []
if address is not None:
addr = "{:X}".format(address)
new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
self.cradio.set_address(new_addr)
self.cradio.set_arc(1)
self.cradio.set_data_rate(self.cradio.DR_250KPS)
if address is None or address == 0xE7E7E7E7E7:
found += [["radio://0/{}/250K".format(c), ""]
for c in self._scan_radio_channels()]
self.cradio.set_data_rate(self.cradio.DR_1MPS)
found += [["radio://0/{}/1M".format(c), ""]
for c in self._scan_radio_channels()]
self.cradio.set_data_rate(self.cradio.DR_2MPS)
found += [["radio://0/{}/2M".format(c), ""]
for c in self._scan_radio_channels()]
else:
found += [["radio://0/{}/250K/{:X}".format(c, address), ""]
for c in self._scan_radio_channels()]
self.cradio.set_data_rate(self.cradio.DR_1MPS)
found += [["radio://0/{}/1M/{:X}".format(c, address), ""]
for c in self._scan_radio_channels()]
self.cradio.set_data_rate(self.cradio.DR_2MPS)
found += [["radio://0/{}/2M/{:X}".format(c, address), ""]
for c in self._scan_radio_channels()]
self.cradio.close()
self.cradio = None
return found
def get_status(self):
if self.cradio is None:
try:
self.cradio = Crazyradio()
except USBError as e:
return "Cannot open Crazyradio. Permission problem?" \
" ({})".format(str(e))
except Exception as e:
return str(e)
ver = self.cradio.version
self.cradio.close()
self.cradio = None
return "Crazyradio version {}".format(ver)
def get_name(self):
return "radio"
class RadioDriver(CRTPDriver):
""" Crazyradio link driver """
def __init__(self):
""" Create the link driver """
CRTPDriver.__init__(self)
self.cradio = None
self.uri = ""
self.link_error_callback = None
self.link_quality_callback = None
self.in_queue = None
self.out_queue = None
self._thread = None
def connect(self, uri, link_quality_callback, link_error_callback):
"""
Connect the link driver to a specified URI of the format:
radio://<dongle nbr>/<radio channel>/[250K,1M,2M]
The callback for linkQuality can be called at any moment from the
driver to report back the link quality in percentage. The
callback from linkError will be called when a error occues with
an error message.
"""
# check if the URI is a radio URI
if not re.search("^radio://", uri):
raise WrongUriType("Not a radio URI")
# Open the USB dongle
if not re.search(
"^radio://([0-9]+)((/([0-9]+))((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$",
uri):
raise WrongUriType('Wrong radio URI format!')
uri_data = re.search(
"^radio://([0-9]+)((/([0-9]+))"
"((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$", uri)
self.uri = uri
channel = 2
if uri_data.group(4):
channel = int(uri_data.group(4))
datarate = Crazyradio.DR_2MPS
if uri_data.group(7) == "250K":
datarate = Crazyradio.DR_250KPS
if uri_data.group(7) == "1M":
datarate = Crazyradio.DR_1MPS
if uri_data.group(7) == "2M":
datarate = Crazyradio.DR_2MPS
if self.cradio is None:
self.cradio = Crazyradio(devid=int(uri_data.group(1)))
else:
raise Exception("Link already open!")
if self.cradio.version >= 0.4:
self.cradio.set_arc(10)
else:
logger.warning("Radio version <0.4 will be obsoleted soon!")
self.cradio.set_channel(channel)
self.cradio.set_data_rate(datarate)
if uri_data.group(9):
addr = str(uri_data.group(9))
new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
self.cradio.set_address(new_addr)
# Prepare the inter-thread communication queue
self.in_queue = Queue.Queue()
# Limited size out queue to avoid "ReadBack" effect
self.out_queue = Queue.Queue(50)
# Launch the comm thread
self._thread = _RadioDriverThread(self.cradio, self.in_queue,
self.out_queue,
link_quality_callback,
link_error_callback)
self._thread.start()
self.link_error_callback = link_error_callback
def receive_packet(self, time=0):
"""
Receive a packet though the link. This call is blocking but will
timeout and return None if a timeout is supplied.
"""
if time == 0:
try:
return self.in_queue.get(False)
except Queue.Empty:
return None
elif time < 0:
try:
return self.in_queue.get(True)
except Queue.Empty:
return None
else:
try:
return self.in_queue.get(True, time)
except Queue.Empty:
return None
def send_packet(self, pk):
""" Send the packet pk though the link """
# if self.out_queue.full():
# self.out_queue.get()
if (self.cradio is None):
return
try:
self.out_queue.put(pk, True, 2)
except Queue.Full:
if self.link_error_callback:
self.link_error_callback("RadioDriver: Could not send packet"
" to copter")
def pause(self):
self._thread.stop()
self._thread = None
def restart(self):
if self._thread:
return
self._thread = _RadioDriverThread(self.cradio, self.in_queue,
self.out_queue,
self.link_quality_callback,
self.link_error_callback)
self._thread.start()
def close(self):
""" Close the link. """
# Stop the comm thread
self._thread.stop()
# Close the USB dongle
try:
if self.cradio:
self.cradio.close()
except:
# If we pull out the dongle we will not make this call
pass
self.cradio = None
def _scan_radio_channels(self, start=0, stop=125):
""" Scan for Crazyflies between the supplied channels. """
return list(self.cradio.scan_channels(start, stop, (0xff, )))
def scan_selected(self, links):
to_scan = ()
for l in links:
one_to_scan = {}
uri_data = re.search(
"^radio://([0-9]+)((/([0-9]+))"
"(/(250K|1M|2M))?)?$", l)
one_to_scan["channel"] = int(uri_data.group(4))
datarate = Crazyradio.DR_2MPS
if uri_data.group(6) == "250K":
datarate = Crazyradio.DR_250KPS
if uri_data.group(6) == "1M":
datarate = Crazyradio.DR_1MPS
if uri_data.group(6) == "2M":
datarate = Crazyradio.DR_2MPS
one_to_scan["datarate"] = datarate
to_scan += (one_to_scan, )
found = self.cradio.scan_selected(to_scan, (0xFF, 0xFF, 0xFF))
ret = ()
for f in found:
dr_string = ""
if f["datarate"] == Crazyradio.DR_2MPS:
dr_string = "2M"
if f["datarate"] == Crazyradio.DR_250KPS:
dr_string = "250K"
if f["datarate"] == Crazyradio.DR_1MPS:
dr_string = "1M"
ret += ("radio://0/{}/{}".format(f["channel"], dr_string), )
return ret
def scan_interface(self, address):
""" Scan interface for Crazyflies """
if self.cradio is None:
try:
self.cradio = Crazyradio()
except Exception:
return []
else:
raise Exception("Cannot scann for links while the link is open!")
# FIXME: implements serial number in the Crazyradio driver!
serial = "N/A"
logger.info("v%s dongle with serial %s found", self.cradio.version,
serial)
found = []
if address != None:
addr = "{:X}".format(address)
new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
self.cradio.set_address(new_addr)
self.cradio.set_arc(1)
self.cradio.set_data_rate(self.cradio.DR_250KPS)
if address == None or address == 0xE7E7E7E7E7:
found += map(lambda c: ["radio://0/{}/250K".format(c), ""],
self._scan_radio_channels())
self.cradio.set_data_rate(self.cradio.DR_1MPS)
found += map(lambda c: ["radio://0/{}/1M".format(c), ""],
self._scan_radio_channels())
self.cradio.set_data_rate(self.cradio.DR_2MPS)
found += map(lambda c: ["radio://0/{}/2M".format(c), ""],
self._scan_radio_channels())
else:
found += map(
lambda c: ["radio://0/{}/250K/{:X}".format(c, address), ""],
self._scan_radio_channels())
self.cradio.set_data_rate(self.cradio.DR_1MPS)
found += map(
lambda c: ["radio://0/{}/1M/{:X}".format(c, address), ""],
self._scan_radio_channels())
self.cradio.set_data_rate(self.cradio.DR_2MPS)
found += map(
lambda c: ["radio://0/{}/2M/{:X}".format(c, address), ""],
self._scan_radio_channels())
self.cradio.close()
self.cradio = None
return found
def get_status(self):
if self.cradio is None:
try:
self.cradio = Crazyradio()
except USBError as e:
return "Cannot open Crazyradio. Permission problem?"\
" ({})".format(str(e))
except Exception as e:
return str(e)
ver = self.cradio.version
self.cradio.close()
self.cradio = None
return "Crazyradio version {}".format(ver)
def get_name(self):
return "radio"
