def get_status(self):
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except USBError as e:
return "Cannot open Crazyflie. Permission problem?"\
" ({})".format(str(e))
except Exception as e:
return str(e)
return "Crazyradio version {}".format(self.cfusb.version)
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("^usb://", uri):
raise WrongUriType("Not a radio URI")
# Open the USB dongle
if not re.search("^usb://([0-9]+)$",
uri):
raise WrongUriType('Wrong radio URI format!')
uri_data = re.search("^usb://([0-9]+)$",
uri)
self.uri = uri
if self.cfusb is None:
self.cfusb = CfUsb(devid=int(uri_data.group(1)))
if self.cfusb.dev:
self.cfusb.set_crtp_to_usb(True)
# Wait for the blocking queues in the firmware to time out
time.sleep(1)
else:
self.cfusb = None
raise Exception("Could not open {}".format(self.uri))
else:
raise Exception("Link already open!")
# 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 = _UsbReceiveThread(self.cfusb, self.in_queue,
link_quality_callback,
link_error_callback)
self._thread.start()
self.link_error_callback = link_error_callback
def scan_interface(self, address):
""" Scan interface for Crazyflies """
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except Exception as e:
logger.warn("Exception while scanning for Crazyflie USB: {}".format(str(e)))
return []
else:
raise Exception("Cannot scan for links while the link is open!")
# FIXME: implements serial number in the Crazyradio driver!
#serial = "N/A"
found = self.cfusb.scan()
self.cfusb.close()
self.cfusb = None
return found
def get_status(self):
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except USBError as e:
return "Cannot open Crazyflie. Permission problem?"\
" ({})".format(str(e))
except Exception as e:
return str(e)
return "Crazyradio version {}".format(self.cfusb.version)
def scan_interface(self):
""" Scan interface for Crazyflies """
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except Exception as e:
logger.warn("Exception while scanning for Crazyflie USB: {}".format(str(e)))
return []
else:
raise Exception("Cannot scan for links while the link is open!")
# FIXME: implements serial number in the Crazyradio driver!
#serial = "N/A"
found = self.cfusb.scan()
self.cfusb.close()
self.cfusb = None
return found
class UsbDriver(CRTPDriver):
""" Crazyradio link driver """
def __init__(self):
""" Create the link driver """
CRTPDriver.__init__(self)
self.cfusb = 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("^usb://", uri):
raise WrongUriType("Not a radio URI")
# Open the USB dongle
if not re.search("^usb://([0-9]+)$",
uri):
raise WrongUriType('Wrong radio URI format!')
uri_data = re.search("^usb://([0-9]+)$",
uri)
self.uri = uri
if self.cfusb is None:
self.cfusb = CfUsb(devid=int(uri_data.group(1)))
if self.cfusb.dev:
self.cfusb.set_crtp_to_usb(True)
time.sleep(1) # Wait for the blocking queues in the firmware to time out
else:
self.cfusb = None
raise Exception("Could not open {}".format(self.uri))
else:
raise Exception("Link already open!")
# 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 = _UsbReceiveThread(self.cfusb, self.in_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.cfusb is None):
return
try:
dataOut = (pk.header,)
dataOut += pk.datat
self.cfusb.send_packet(dataOut)
except Queue.Full:
if self.link_error_callback:
self.link_error_callback("UsbDriver: Could not send packet"
" to Crazyflie")
def pause(self):
self._thread.stop()
self._thread = None
def restart(self):
if self._thread:
return
self._thread = _UsbReceiveThread(self.cfusb, self.in_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.cfusb:
self.cfusb.set_crtp_to_usb(False)
self.cfusb.close()
except Exception as e:
# If we pull out the dongle we will not make this call
logger.info("Could not close {}".format(e))
pass
self.cfusb = None
def scan_interface(self, address):
""" Scan interface for Crazyflies """
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except Exception as e:
logger.warn("Exception while scanning for Crazyflie USB: {}".format(str(e)))
return []
else:
raise Exception("Cannot scan for links while the link is open!")
# FIXME: implements serial number in the Crazyradio driver!
#serial = "N/A"
found = self.cfusb.scan()
self.cfusb.close()
self.cfusb = None
return found
def get_status(self):
return "No information available"
def get_name(self):
return "UsbCdc"
class UsbDriver(CRTPDriver):
""" Crazyradio link driver """
def __init__(self):
""" Create the link driver """
CRTPDriver.__init__(self)
self.cfusb = 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 = False
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('^usb://', uri):
raise WrongUriType('Not a radio URI')
# Open the USB dongle
if not re.search('^usb://([0-9]+)$',
uri):
raise WrongUriType('Wrong radio URI format!')
uri_data = re.search('^usb://([0-9]+)$',
uri)
self.uri = uri
if self.cfusb is None:
self.cfusb = CfUsb(devid=int(uri_data.group(1)))
if self.cfusb.dev:
self.cfusb.set_crtp_to_usb(True)
else:
self.cfusb = None
raise Exception('Could not open {}'.format(self.uri))
else:
raise Exception('Link already open!')
# 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 = _UsbReceiveThread(self.cfusb, self.in_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.cfusb is None):
return
try:
dataOut = (pk.header,)
dataOut += pk.datat
self.cfusb.send_packet(dataOut)
except queue.Full:
if self.link_error_callback:
self.link_error_callback(
'UsbDriver: Could not send packet to Crazyflie')
def pause(self):
self._thread.stop()
self._thread = None
def restart(self):
if self._thread:
return
self._thread = _UsbReceiveThread(self.cfusb, self.in_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.cfusb:
self.cfusb.set_crtp_to_usb(False)
self.cfusb.close()
except Exception as e:
# If we pull out the dongle we will not make this call
logger.info('Could not close {}'.format(e))
pass
self.cfusb = None
def scan_interface(self, address):
""" Scan interface for Crazyflies """
if self.cfusb is None:
try:
self.cfusb = CfUsb()
except Exception as e:
logger.warn(
'Exception while scanning for Crazyflie USB: {}'.format(
str(e)))
return []
else:
raise Exception('Cannot scan for links while the link is open!')
# FIXME: implements serial number in the Crazyradio driver!
# serial = "N/A"
found = self.cfusb.scan()
self.cfusb.close()
self.cfusb = None
return found
def get_status(self):
return 'No information available'
def get_name(self):
return 'UsbCdc'
