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)
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))?)?$", uri): raise WrongUriType('Wrong radio URI format!') uri_data = re.search("^radio://([0-9]+)((/([0-9]+))" "(/(250K|1M|2M))?)?$", 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(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 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) # 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_interface(self): """ 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 = [] self.cradio.set_arc(1) self.cradio.set_data_rate(self.cradio.DR_250KPS) 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()) 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) return "Crazyradio version {}".format(self.cradio.version) 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"
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")
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'