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')
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")