class Console: """ Crazyflie console is used to receive characters printed using printf from the firmware. """ def __init__(self, crazyflie): """ Initialize the console and register it to receive data from the copter. """ self.receivedChar = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming) #print("Console init finished") def incoming(self, packet): """ Callback for data received from the copter. """ #print("Console incoming") # This might be done prettier ;-) console_text = packet.data.decode('UTF-8') # leonana: add print for console information #print(console_text, end='', flush=True) self.receivedChar.call(console_text)
class PeriodicTimer: """Create a periodic timer that will periodicall call a callback""" def __init__(self, period, callback): self._callbacks = Caller() self._callbacks.add_callback(callback) self._started = False self._period = period self._timer = Timer(period, self._expired) self._timer.daemon = True def start(self): """Start the timer""" self._timer = Timer(self._period, self._expired) self._timer.daemon = True self._timer.start() self._started = True def stop(self): """Stop the timer""" self._timer.cancel() self._started = False def _expired(self): """Callback for the expired internal timer""" self._callbacks.call() if self._started: self.start()
class PluginHelper(): """Used for passing objects to tabs and toolboxes""" def __init__(self): self.cf = None self.menu = None self.logConfigReader = None self.referenceHeight = 0.400 self.hover_input_updated = Caller() self.useReferenceHeight = False self.inputType = 0 self.inputTimer = 0.000 self.sinewaveFrequency = 1.0 # 1s self.mainUI = None self.plotTab = None self.pose_logger = None self.connectivity_manager = None self.init_time = time.time() def send_hover_setpoint(self, vy, vx, yawrate, height): if self.useReferenceHeight: if (self.inputType == 0): # Step Input self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight) if (self.inputType == 1): # Sine Wave self.hover_input_updated.call(vy, vx, yawrate, self.sine_wave_generator()) if (self.inputType == 2): # Ramp self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight) print(self.sine_wave_generator()) else: self.hover_input_updated.call(vy, vx, yawrate, height) def sine_wave_generator(self): output = 0.25 * sin(2.00 * pi * self.sinewaveFrequency * (time.time() - self.init_time)) + 0.5 # output = output + self.inputTimer + 0.001 return output
class PluginHelper(): """Used for passing objects to tabs and toolboxes""" def __init__(self): self.cf = None self.menu = None self.logConfigReader = None self.referenceHeight = 0.400 self.hover_input_updated = Caller() self.useReferenceHeight = False self.inputType = 0 self.inputTimer = 0.000 self.sinewaveFrequency = 1.0 # 1s def send_hover_setpoint(self, vy, vx, yawrate, height): if self.useReferenceHeight: if (self.inputType == 0): # Step Input self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight) if (self.inputType == 1): # Sine Wave self.hover_input_updated.call(vy, vx, yawrate, self.sine_wave_generator()) if (self.inputType == 2): # Ramp self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight) #else: #self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight) else: self.hover_input_updated.call(vy, vx, yawrate, height) def sine_wave_generator(self): output = 0.2 * sin(2.00 * pi * self.sinewaveFrequency * self.inputTimer) output = output + self.referenceHeight self.inputTimer = self.inputTimer + 0.001 return output
class Appchannel: def __init__(self, crazyflie): self._cf = crazyflie self.packet_received = Caller() self._cf.add_port_callback(CRTPPort.PLATFORM, self._incoming) def send_packet(self, data): packet = CRTPPacket() packet.port = CRTPPort.PLATFORM packet.channel = cflib.crazyflie.platformservice.APP_CHANNEL packet.data = data self._cf.send_packet(packet) def _incoming(self, packet: CRTPPacket): if packet.channel == cflib.crazyflie.platformservice.APP_CHANNEL: self.packet_received.call(packet.data)
class Console: """ Crazyflie console is used to receive characters printed using printf from the firmware. """ def __init__(self, crazyflie): """ Initialize the console and register it to receive data from the copter. """ self.receivedChar = Caller() """ This member variable is used to setup a callback that will be called when text is received from the CONSOLE port of CRTP (0). Example: ```python [...] def log_console(self, text): self.log_file.write(text) [...] self.cf.console.receivedChar.add_callback(self.log_console) ``` """ self.cf = crazyflie self.cf.add_port_callback(CRTPPort.CONSOLE, self._incoming) def _incoming(self, packet): """ Callback for data received from the copter. """ # This might be done prettier ;-) console_text = packet.data.decode('UTF-8') self.receivedChar.call(console_text)
class Console: """ Crazyflie console is used to receive characters printed using printf from the firmware. """ def __init__(self, crazyflie): """ Initialize the console and register it to receive data from the copter. """ self.receivedChar = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming) def incoming(self, packet): """ Callback for data received from the copter. """ # This might be done prettier ;-) console_text = packet.data.decode("UTF-8") self.receivedChar.call(console_text)
class Log(): """Create log configuration""" # These codes can be decoded using os.stderror, but # some of the text messages will look very strange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: 'No more memory available', errno.ENOEXEC: 'Command not found', errno.ENOENT: 'No such block id', errno.E2BIG: 'Block too large', errno.EEXIST: 'Block already exists' } def __init__(self, crazyflie=None): self.log_blocks = [] # Called with newly created blocks self.block_added_cb = Caller() self.cf = crazyflie self.toc = None self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb) self.toc_updated = Caller() self.state = IDLE self.fake_toc_crc = 0xDEADBEEF self._refresh_callback = None self._toc_cache = None def add_config(self, logconf): """Add a log configuration to the logging framework. When doing this the contents of the log configuration will be validated and listeners for new log configurations will be notified. When validating the configuration the variables are checked against the TOC to see that they actually exist. If they don't then the configuration cannot be used. Since a valid TOC is required, a Crazyflie has to be connected when calling this method, otherwise it will fail.""" if not self.cf.link: logger.error('Cannot add configs without being connected to a ' 'Crazyflie!') return # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. for name in logconf.default_fetch_as: var = self.toc.get_element_by_complete_name(name) if not var: logger.warning('%s not in TOC, this block cannot be used!', name) logconf.valid = False raise KeyError('Variable {} not in TOC'.format(name)) # Now that we know what type this variable has, add it to the log # config again with the correct type logconf.add_variable(name, var.ctype) # Now check that all the added variables are in the TOC and that # the total size constraint of a data packet with logging data is # not size = 0 for var in logconf.variables: size += LogTocElement.get_size_from_id(var.fetch_as) # Check that we are able to find the variable in the TOC so # we can return error already now and not when the config is sent if var.is_toc_variable(): if (self.toc.get_element_by_complete_name(var.name) is None): logger.warning( 'Log: %s not in TOC, this block cannot be used!', var.name) logconf.valid = False raise KeyError('Variable {} not in TOC'.format(var.name)) if (size <= MAX_LOG_DATA_PACKET_SIZE and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf self.log_blocks.append(logconf) self.block_added_cb.call(logconf) else: logconf.valid = False raise AttributeError( 'The log configuration is too large or has an invalid ' 'parameter') def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" self._toc_cache = toc_cache self._refresh_callback = refresh_done_callback self.toc = None pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING, ) self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, )) def _find_block(self, id): for block in self.log_blocks: if block.id == id: return block return None def _new_packet_cb(self, packet): """Callback for newly arrived packets with TOC information""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if (chan == CHAN_SETTINGS): id = payload[0] error_status = payload[1] block = self._find_block(id) if (cmd == CMD_CREATE_BLOCK): if (block is not None): if error_status == 0 or error_status == errno.EEXIST: if not block.added: logger.debug('Have successfully added id=%d', id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, id, block.period) self.cf.send_packet( pk, expected_reply=(CMD_START_LOGGING, id)) block.added = True else: msg = self._err_codes[error_status] logger.warning('Error %d when adding id=%d (%s)', error_status, id, msg) block.err_no = error_status block.added_cb.call(False) block.error_cb.call(block, msg) else: logger.warning('No LogEntry to assign block to !!!') if (cmd == CMD_START_LOGGING): if (error_status == 0x00): logger.info('Have successfully started logging for id=%d', id) if block: block.started = True else: msg = self._err_codes[error_status] logger.warning('Error %d when starting id=%d (%s)', error_status, id, msg) if block: block.err_no = error_status block.started_cb.call(self, False) # This is a temporary fix, we are adding a new issue # for this. For some reason we get an error back after # the block has been started and added. This will show # an error in the UI, but everything is still working. # block.error_cb.call(block, msg) if (cmd == CMD_STOP_LOGGING): if (error_status == 0x00): logger.info('Have successfully stopped logging for id=%d', id) if block: block.started = False if (cmd == CMD_DELETE_BLOCK): # Accept deletion of a block that isn't added. This could # happen due to timing (i.e add/start/delete in fast sequence) if error_status == 0x00 or error_status == errno.ENOENT: logger.info('Have successfully deleted id=%d', id) if block: block.started = False block.added = False if (cmd == CMD_RESET_LOGGING): # Guard against multiple responses due to re-sending if not self.toc: logger.debug('Logging reset, continue with TOC download') self.log_blocks = [] self.toc = Toc() toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, self.toc, self._refresh_callback, self._toc_cache) toc_fetcher.start() if (chan == CHAN_LOGDATA): chan = packet.channel id = packet.data[0] block = self._find_block(id) timestamps = struct.unpack('<BBB', packet.data[1:4]) timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16) logdata = packet.data[4:] if (block is not None): block.unpack_log_data(logdata, timestamp) else: logger.warning('Error no LogEntry to handle id=%d', id)
class Memory(): """Access memories on the Crazyflie""" # These codes can be decoded using os.stderror, but # some of the text messages will look very strange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: 'No more memory available', errno.ENOEXEC: 'Command not found', errno.ENOENT: 'No such block id', errno.E2BIG: 'Block too large', errno.EEXIST: 'Block already exists' } def __init__(self, crazyflie=None): """Instantiate class and connect callbacks""" self.mems = [] # Called when new memories have been added self.mem_added_cb = Caller() # Called when new data has been read self.mem_read_cb = Caller() self.mem_write_cb = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self._refresh_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 self._ow_mem_fetch_index = 0 self._elem_data = () self._read_requests = {} self._read_requests_lock = Lock() self._write_requests = {} self._write_requests_lock = Lock() self._ow_mems_left_to_update = [] self._getting_count = False def _mem_update_done(self, mem): """ Callback from each individual memory (only 1-wire) when reading of header/elements are done """ if mem.id in self._ow_mems_left_to_update: self._ow_mems_left_to_update.remove(mem.id) logger.info(mem) if len(self._ow_mems_left_to_update) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None def get_mem(self, id): """Fetch the memory with the supplied id""" for m in self.mems: if m.id == id: return m return None def get_mems(self, type): """Fetch all the memories of the supplied type""" ret = () for m in self.mems: if m.type == type: ret += (m, ) return ret def ow_search(self, vid=0xBC, pid=None, name=None): """Search for specific memory id/name and return it""" for m in self.get_mems(MemoryElement.TYPE_1W): if pid and m.pid == pid or name and m.name == name: return m return None def write(self, memory, addr, data, flush_queue=False): """Write the specified data to the given memory at the given address""" wreq = _WriteRequest(memory, addr, data, self.cf) if memory.id not in self._write_requests: self._write_requests[memory.id] = [] # Workaround until we secure the uplink and change messages for # mems to non-blocking self._write_requests_lock.acquire() if flush_queue: self._write_requests[memory.id] = self._write_requests[ memory.id][:1] self._write_requests[memory.id].insert(len(self._write_requests), wreq) if len(self._write_requests[memory.id]) == 1: wreq.start() self._write_requests_lock.release() return True def read(self, memory, addr, length): """ Read the specified amount of bytes from the given memory at the given address """ if memory.id in self._read_requests: logger.warning('There is already a read operation ongoing for ' 'memory id {}'.format(memory.id)) return False rreq = _ReadRequest(memory, addr, length, self.cf) self._read_requests[memory.id] = rreq rreq.start() return True def refresh(self, refresh_done_callback): """Start fetching all the detected memories""" self._refresh_callback = refresh_done_callback self._fetch_id = 0 for m in self.mems: try: self.mem_read_cb.remove_callback(m.new_data) m.disconnect() except Exception as e: logger.info( 'Error when removing memory after update: {}'.format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info('Requesting number of memories') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR, ) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR, )) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = payload[0] logger.info('{} memories found'.format(self.nbr_of_mems)) # Start requesting information about the memories, # if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info('Requesting first id') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory # (the I2C). logger.error( '-------->Got good count, but no info on mem!') self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = payload[0] # Type - 1 byte mem_type = payload[1] # Size 4 bytes (as addr) mem_size = struct.unpack('I', payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) mem_addr = '' for m in mem_addr_raw: mem_addr += '{:02X}'.format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_DRIVER_LED: mem = LEDDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info( 'Requesting information about memory {}'.format( self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info( 'Done getting all the memories, start reading the OWs') ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise # we are done for ow_mem in ows: ow_mem.update(self._mem_update_done) if len(ows) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) logger.info('WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format( id, addr, status)) # Find the read request if id in self._write_requests: self._write_requests_lock.acquire() wreq = self._write_requests[id][0] if status == 0: if wreq.write_done(addr): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) self.mem_write_cb.call(wreq.mem, wreq.addr) # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: self._write_requests[id][0].start() else: logger.info('Status {}: write resending...'.format(status)) wreq.resend() self._write_requests_lock.release() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) data = struct.unpack('B' * len(payload[5:]), payload[5:]) logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, ' 'data={}'.format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info('READING: We are still interested in request for ' 'mem {}'.format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: logger.info('Status {}: resending...'.format(status)) rreq.resend()
class JoystickReader(object): """ Thread that will read input from devices/joysticks and send control-set points to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): self._input_device = None self.min_thrust = 0 self.max_thrust = 0 self._thrust_slew_rate = 0 self.thrust_slew_enabled = False self.thrust_slew_limit = 0 self.has_pressure_sensor = False self.max_rp_angle = 0 self.max_yaw_rate = 0 self._old_thrust = 0 self._old_raw_thrust = 0 self._old_alt_hold = False self.springy_throttle = True self.trim_roll = Config().get("trim_roll") self.trim_pitch = Config().get("trim_pitch") self._input_map = None self._mux = [ NoMux(self), TakeOverSelectiveMux(self), TakeOverMux(self) ] # Set NoMux as default self._selected_mux = self._mux[0] if Config().get("flightmode") is "Normal": self.max_yaw_rate = Config().get("normal_max_yaw") self.max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("normal_min_thrust") self.max_thrust = Config().get("normal_max_thrust") self.thrust_slew_limit = Config().get("normal_slew_limit") self.thrust_slew_rate = Config().get("normal_slew_rate") else: self.max_yaw_rate = Config().get("max_yaw") self.max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("min_thrust") self.max_thrust = Config().get("max_thrust") self.thrust_slew_limit = Config().get("slew_limit") self.thrust_slew_rate = Config().get("slew_rate") self._dev_blacklist = None if len(Config().get("input_device_blacklist")) > 0: self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(cfclient.module_path + "/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager().configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() self.alt1_updated = Caller() self.alt2_updated = Caller() # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting) self.limiting_updated = Caller() def _get_device_from_name(self, device_name): """Get the raw device from a name""" for d in readers.devices(): if d.name == device_name: return d return None def set_alt_hold_available(self, available): """Set if altitude hold is available or not (depending on HW)""" self.has_pressure_sensor = available def enable_alt_hold(self, althold): """Enable or disable altitude hold""" self._old_alt_hold = althold def _do_device_discovery(self): devs = self.available_devices() # This is done so that devs can easily get access # to limits without creating lots of extra code for d in devs: d.input = self if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def available_mux(self): return self._mux def set_mux(self, name=None, mux=None): old_mux = self._selected_mux if name: for m in self._mux: if m.name == name: self._selected_mux = m elif mux: self._selected_mux = mux old_mux.close() logger.info("Selected MUX: {}".format(self._selected_mux.name)) def available_devices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = readers.devices() devs += interfaces.devices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev.name))): dev.input = self approved_devs.append(dev) return approved_devs def enableRawReading(self, device_name): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ if self._input_device: self._input_device.close() self._input_device = None for d in readers.devices(): if d.name == device_name: self._input_device = d # Set the mapping to None to get raw values self._input_device.input_map = None self._input_device.open() def get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in list(device_config_mapping.keys()): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def stop_raw_reading(self): """Disable raw reading of input device.""" if self._input_device: self._input_device.close() self._input_device = None def read_raw_values(self): """ Read raw values from the input device.""" [axes, buttons, mapped_values] = self._input_device.read(include_raw=True) dict_axes = {} dict_buttons = {} for i, a in enumerate(axes): dict_axes[i] = a for i, b in enumerate(buttons): dict_buttons[i] = b return [dict_axes, dict_buttons, mapped_values] def set_raw_input_map(self, input_map): """Set an input device map""" # TODO: Will not work! if self._input_device: self._input_device.input_map = input_map def set_input_map(self, device_name, input_map_name): """Load and set an input device map with the given name""" settings = ConfigManager().get_settings(input_map_name) if settings: self.springy_throttle = settings["springythrottle"] self._input_map = ConfigManager().get_config(input_map_name) self._get_device_from_name(device_name).input_map = self._input_map self._get_device_from_name(device_name).input_map_name = input_map_name Config().get("device_config_mapping")[device_name] = input_map_name def start_input(self, device_name, role="Device", config_name=None): """ Start reading input from the device with name device_name using config config_name. Returns True if device supports mapping, otherwise False """ try: # device_id = self._available_devices[device_name] # Check if we supplied a new map, if not use the preferred one device = self._get_device_from_name(device_name) self._selected_mux.add_device(device, role) # Update the UI with the limiting for this device self.limiting_updated.call(device.limit_rp, device.limit_yaw, device.limit_thrust) self._read_timer.start() return device.supports_mapping except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) if not self._input_device: self.device_error.call( "Could not find device {}".format(device_name)) return False def resume_input(self): self._selected_mux.resume() self._read_timer.start() def pause_input(self, device_name=None): """Stop reading from the input device.""" self._read_timer.stop() self._selected_mux.pause() def _set_thrust_slew_rate(self, rate): self._thrust_slew_rate = rate if rate > 0: self.thrust_slew_enabled = True else: self.thrust_slew_enabled = False def _get_thrust_slew_rate(self): return self._thrust_slew_rate def read_input(self): """Read input data from the selected device""" try: data = self._selected_mux.read() if data: if data.toggled.althold: try: self.althold_updated.call(str(data.althold)) except Exception as e: logger.warning( "Exception while doing callback from input-device " "for althold: {}".format(e)) if data.toggled.estop: try: self.emergency_stop_updated.call(data.estop) except Exception as e: logger.warning("Exception while doing callback from" "input-device for estop: {}".format(e)) if data.toggled.alt1: try: self.alt1_updated.call(data.alt1) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt1: {}".format(e)) if data.toggled.alt2: try: self.alt2_updated.call(data.alt2) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt2: {}".format(e)) # Update the user roll/pitch trim from device if data.toggled.pitchNeg and data.pitchNeg: self.trim_pitch -= 1 if data.toggled.pitchPos and data.pitchPos: self.trim_pitch += 1 if data.toggled.rollNeg and data.rollNeg: self.trim_roll -= 1 if data.toggled.rollPos and data.rollPos: self.trim_roll += 1 if data.toggled.pitchNeg or data.toggled.pitchPos or \ data.toggled.rollNeg or data.toggled.rollPos: self.rp_trim_updated.call(self.trim_roll, self.trim_pitch) # Thrust might be <0 here, make sure it's not otherwise we'll # get an error. if data.thrust < 0: data.thrust = 0 if data.thrust > 0xFFFF: data.thrust = 0xFFFF # If we are using alt hold the data is not in a percentage if not data.althold: data.thrust = JoystickReader.p2t(data.thrust) self.input_updated.call(data.roll + self.trim_roll, data.pitch + self.trim_pitch, data.yaw, data.thrust) else: self.input_updated.call(0, 0, 0, 0) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self.input_updated.call(0, 0, 0, 0) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater(self.cf, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self.values = {} def request_update_of_all_params(self): """Request an update of all the parameters in the TOC""" for group in self.toc.toc: for name in self.toc.toc[group]: complete_name = '%s.%s' % (group, name) self.request_param_update(complete_name) def _check_if_all_updated(self): """Check if all parameters from the TOC has at least been fetched once""" for g in self.toc.toc: if g not in self.values: return False for n in self.toc.toc[g]: if n not in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: s = struct.unpack(element.pytype, pk.data[1:])[0] s = s.__str__() complete_name = '%s.%s' % (element.group, element.name) # Save the value for synchronous access if element.group not in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( complete_name, s) if element.group in self.group_update_callbacks: self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) # Once all the parameters are updated call the # callback for "everything updated" (after all the param # updated callbacks) if self._check_if_all_updated() and not self.is_updated: self.is_updated = True self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) def remove_update_callback(self, group, name=None, cb=None): """Remove the supplied callback for a group or a group.name""" if not cb: return if not name: if group in self.group_update_callbacks: self.group_update_callbacks[group].remove_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname in self.param_update_callbacks: self.param_update_callbacks[paramname].remove_callback(cb) def add_update_callback(self, group=None, name=None, cb=None): """ Add a callback for a specific parameter name. This callback will be executed when a new value is read from the Crazyflie. """ if not group and not name: self.all_update_callback.add_callback(cb) elif not name: if group not in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ element = self.toc.get_element_by_complete_name(complete_name) if not element: logger.warning("Cannot set value for [%s], it's not in the TOC!", complete_name) raise KeyError('{} not in param TOC'.format(complete_name)) elif element.access == ParamTocElement.RO_ACCESS: logger.debug('[%s] is read only, no trying to set value', complete_name) raise AttributeError('{} is read-only!'.format(complete_name)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) pk.data = struct.pack('<B', varid) pk.data += struct.pack(element.pytype, eval(value)) self.param_updater.request_param_setvalue(pk)
class LogEntry(object): """Representation of one log configuration that enables logging from the Crazyflie""" block_idCounter = 1 def __init__(self, crazyflie, logconf): """Initialize the entry""" self.data_received = Caller() self.error = Caller() self.started_cb = Caller() self.added_cb = Caller() self.err_no = 0 self.logconf = logconf self.block_id = LogEntry.block_idCounter LogEntry.block_idCounter += 1 % 255 self.cf = crazyflie self.period = logconf.getPeriod() / 10 self.period_in_ms = logconf.getPeriod() self._added = False self._started = False def _set_added(self, added): self._added = added self.added_cb.call(added) def _get_added(self): return self._added def _set_started(self, started): self._started = started self.started_cb.call(started) def _get_started(self): return self._started added = property(_get_added, _set_added) started = property(_get_started, _set_started) def start(self): """Start the logging for this entry""" if (self.cf.link is not None): if (self._added is False): logger.debug("First time block is started, add block") pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_CREATE_BLOCK, self.block_id) for var in self.logconf.getVariables(): if (var.isTocVariable() is False): # Memory location logger.debug("Logging to raw memory %d, 0x%04X", var.getStoredFetchAs(), var.getAddress()) pk.data += struct.pack('<B', var.getStoredFetchAs()) pk.data += struct.pack('<I', var.getAddress()) else: # Item in TOC logger.debug("Adding %s with id=%d and type=0x%02X", var.getName(), self.cf.log.toc.get_element_id( var.getName()), var.getStoredFetchAs()) pk.data += struct.pack('<B', var.getStoredFetchAs()) pk.data += struct.pack('<B', self.cf.log.toc. get_element_id(var.getName())) logger.debug("Adding log block id {}".format(self.block_id)) self.cf.send_packet(pk) else: logger.debug("Block already registered, starting logging" " for %d", self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.block_id, self.period) self.cf.send_packet(pk) def stop(self): """Stop the logging for this entry""" if (self.cf.link is not None): if (self.block_id is None): logger.warning("Stopping block, but no block registered") else: logger.debug("Sending stop logging for block %d", self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.block_id) self.cf.send_packet(pk) def close(self): """Delete this entry in the Crazyflie""" if (self.cf.link is not None): if (self.block_id is None): logger.warning("Delete block, but no block registered") else: logger.debug("LogEntry: Sending delete logging for block %d" % self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.block_id) self.cf.send_packet(pk) self.block_id = None # Wait until we get confirmation of delete def unpack_log_data(self, log_data, timestamp): """Unpack received logging data so it represent real values according to the configuration in the entry""" ret_data = {} data_index = 0 for var in self.logconf.getVariables(): size = LogTocElement.get_size_from_id(var.getFetchAs()) name = var.getName() unpackstring = LogTocElement.get_unpack_string_from_id( var.getFetchAs()) value = struct.unpack(unpackstring, log_data[data_index:data_index + size])[0] data_index += size ret_data[name] = value self.data_received.call(ret_data, timestamp)
class FreeFallRecovery(QtCore.QObject): """ Class to recover from detected freefall""" sigRecoveryTimedOut = pyqtSignal() def __init__(self, parent=None): super(QtCore.QObject, self).__init__(parent) self.parent = parent self.useBaro = False # Using barometer for current recovery self.useBaroNext = False # Use barometer for next recovery self.falling = False # Free fall Recovery active self.kill = False self.boostMSec = 45 # Max thrust for first 30ms. State also keeps track of mode, see step() self.nr = 0 # step nr during non baro mode self.falloff = FallOff(self) # why the hell do we mix signals like this? self.auto_input_updated = Caller() self.althold_updated = Caller() self.timerOut = QtCore.QTimer() def setUseBaro(self, on): """ If we should use the barometer (ie hover mode) to help recover or not for the next fall""" msg = " next " if self.falling else " " if self.useBaroNext != on: logger.info("Using Barometer for" + msg + "freefall recovery" if on else "Not using Baro for" + msg + "freefall recovery") self.useBaroNext = on def step(self, rollTrimmed, pitchTrimmed, yaw, thrust): """Here we handle our recover; called by the joy driver @ 100hz. We mostly pass everything through, but we modify the throttle""" if self.useBaro > 0: # Barometer Mode if self.useBaro == 1: # Loop Hover mode thrust = int(round(-0.5 * 32767 + 32767)) elif self.useBaro == 2: # Set hover mode self.useBaro -= 1 self.enableBaro() thrust = int(round(-0.5 * 32767 + 32767)) else: # Boost mode self.useBaro -= 1 thrust = JoystickReader.p2t(min(99, 70 + self.useBaro * 1.25)) logger.info("Thrust: %d", thrust) else: # Regular mode self.nr += 1 thrust = JoystickReader.p2t(self.falloff.f( self.nr)) #if thrust>0.05 else 0 self.auto_input_updated.call(rollTrimmed, pitchTrimmed, yaw, thrust) def startRecovery(self): if self.kill: logger.info("Cannot start freefall recovery due to killswitch") else: logger.info("Starting freefall recovery") self.falling = True self.useBaro = self.boostMSec = 30 if self.useBaroNext else 0 # Start the timeout and the handler if self.useBaro: self.althold_updated.call("altHold.altHoldErrMax", 260.0) self.timerOut.singleShot( 3000, self.setTimedOut) # 6 second for recovery with barometer #self.enableBaro() else: self.nr = 0 self.timerOut.singleShot( 2000, self.setTimedOut ) # 2 second for recovery without barometer def setKillSwitch(self, on): self.kill = on if on: self.abort() def abort(self): """ We have landed, so stop recovering""" if self.cleanUp(): self.sigRecoveryTimedOut.emit() logger.info("Aborted Recovery") def setLanded(self): """ We have landed, so stop recovering""" if self.cleanUp(): logger.info("Recovery stopped due to landing") def setTimedOut(self): """ Recovering times out before we detected a landing""" if self.cleanUp(): logger.info("Recovery stopped due to timeout") self.sigRecoveryTimedOut.emit() def cleanUp(self): if self.falling: self.falling = False self.timerOut.stop() if self.useBaro: self.disableBaro() return True return False def enableBaro(self): # Make change in altitude stronger self.althold_updated.call("altHold.altHoldErrMax", 260.0) self.althold_updated.call("altHold.altHoldChangeSens", 50.0) self.althold_updated.call("flightmode.althold", True) def disableBaro(self): self.althold_updated.call("flightmode.althold", False) self.althold_updated.call("altHold.altHoldErrMax", 1.0) self.althold_updated.call("altHold.altHoldChangeSens", 200.0)
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater( self.cf, self._useV2, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self._initialized = Event() self.values = {} def request_update_of_all_params(self): """Request an update of all the parameters in the TOC""" for group in self.toc.toc: for name in self.toc.toc[group]: complete_name = '%s.%s' % (group, name) self.request_param_update(complete_name) def _check_if_all_updated(self): """Check if all parameters from the TOC has at least been fetched once""" for g in self.toc.toc: if g not in self.values: return False for n in self.toc.toc[g]: if n not in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: s = struct.unpack(element.pytype, pk.data[1:])[0] s = s.__str__() complete_name = '%s.%s' % (element.group, element.name) # Save the value for synchronous access if element.group not in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s # Once all the parameters are updated call the # callback for "everything updated" if self._check_if_all_updated() and not self.is_updated: self.is_updated = True self._initialized.set() self.all_updated.call() logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( complete_name, s) if element.group in self.group_update_callbacks: self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) else: logger.debug('Variable id [%d] not found in TOC', var_id) def remove_update_callback(self, group, name=None, cb=None): """Remove the supplied callback for a group or a group.name""" if not cb: return if not name: if group in self.group_update_callbacks: self.group_update_callbacks[group].remove_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname in self.param_update_callbacks: self.param_update_callbacks[paramname].remove_callback(cb) def add_update_callback(self, group=None, name=None, cb=None): """ Add a callback for a specific parameter name. This callback will be executed when a new value is read from the Crazyflie. """ if not group and not name: self.all_update_callback.add_callback(cb) elif not name: if group not in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ def refresh_done(): extended_elements = list() for group in self.toc.toc: for element in self.toc.toc[group].values(): if element.is_extended(): extended_elements.append(element) if len(extended_elements) > 0: extended_type_fetcher = _ExtendedTypeFetcher(self.cf, self.toc) extended_type_fetcher.start() extended_type_fetcher.set_callback(refresh_done_callback) extended_type_fetcher.request_extended_types(extended_elements) else: refresh_done_callback() self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False self._initialized.clear() # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value_raw(self, complete_name, type, value): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) # This gives us the type for the struct.pack pytype = ParamTocElement.types[type][1][1] pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f'<B{len_array}sB{pytype}', 0, char_array, type, value) # We will not get an update callback when using raw (MISC_CHANNEL) # so just send. self.cf.send_packet(pk) def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') element = self.toc.get_element_by_complete_name(complete_name) if not element: logger.warning("Cannot set value for [%s], it's not in the TOC!", complete_name) raise KeyError('{} not in param TOC'.format(complete_name)) elif element.access == ParamTocElement.RO_ACCESS: logger.debug('[%s] is read only, no trying to set value', complete_name) raise AttributeError('{} is read-only!'.format(complete_name)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) if self._useV2: pk.data = struct.pack('<H', varid) else: pk.data = struct.pack('<B', varid) try: value_nr = eval(value) except TypeError: value_nr = value pk.data += struct.pack(element.pytype, value_nr) self.param_updater.request_param_setvalue(pk) def get_value(self, complete_name, timeout=60): """ Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') [group, name] = complete_name.split('.') return self.values[group][name] def get_default_value(self, complete_name, callback): """ Get the default value of the specified parameter. The supplied callback will be called with the name of the parameter as well as the default value. None if there is an error. @param complete_name The 'group.name' name of the parameter to store @param callback The callback should take `complete_name` and default value as argument """ element = self.toc.get_element_by_complete_name(complete_name) def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_GET_DEFAULT_VALUE: if pk.data[3] == errno.ENOENT: callback(complete_name, None) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) return default_value, = struct.unpack(element.pytype, pk.data[3:]) callback(complete_name, default_value) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack('<BH', MISC_GET_DEFAULT_VALUE, element.ident) self.param_updater.send_param_misc(pk) def persistent_clear(self, complete_name, callback=None): """ Clear the current value of the specified persistent parameter from eeprom. The supplied callback will be called with `True` as an argument on success and with `False` as an argument on failure. @param complete_name The 'group.name' name of the parameter to store @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) if not element.is_persistent(): raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_CLEAR: callback(complete_name, pk.data[3] == 0) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) if callback is not None: self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack('<BH', MISC_PERSISTENT_CLEAR, element.ident) self.param_updater.send_param_misc(pk) def persistent_store(self, complete_name, callback=None): """ Store the current value of the specified persistent parameter to eeprom. The supplied callback will be called with `True` as an argument on success, and with `False` as an argument on failure. @param complete_name The 'group.name' name of the parameter to store @param callback Optional callback should take `complete_name` and boolean status as arguments """ element = self.toc.get_element_by_complete_name(complete_name) if not element.is_persistent(): raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE: callback(complete_name, pk.data[3] == 0) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) if callback is not None: self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack('<BH', MISC_PERSISTENT_STORE, element.ident) self.param_updater.send_param_misc(pk) def persistent_get_state(self, complete_name, callback): """ Get the state of the specified persistent parameter. The state will be returned in the supplied callback. The state is represented as a namedtuple with members: `is_stored`, `default_value` and `stored_value`. The state is `None` if the parameter is not persistent or if something goes wrong. | Member | Description | | ----------------- | ----------------------------------------------- | | `is_stored` | `True` if the value is stored to eeprom | | `default_value` | The default value supplied by the firmware | | `stored_value` | Value stored in eeprom, None if `not is_stored` | @param complete_name The 'group.name' name of the parameter to store @param callback Callback, takes `complete_name` and PersistentParamState namedtuple as arg """ element = self.toc.get_element_by_complete_name(complete_name) if not element.is_persistent(): raise AttributeError(f"Param '{complete_name}' is not persistent") def new_packet_cb(pk): if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_GET_STATE: if pk.data[3] == errno.ENOENT: callback(complete_name, None) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) return is_stored = pk.data[3] == 1 if not is_stored: default_value, = struct.unpack(element.pytype, pk.data[4:]) else: # Remove little-endian indicator ('<') just_type = element.pytype[1:] default_value, stored_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) callback(complete_name, PersistentParamState( is_stored, default_value, stored_value if is_stored else None ) ) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb) pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack('<BH', MISC_PERSISTENT_GET_STATE, element.ident) self.param_updater.send_param_misc(pk)
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater( self.cf, self._useV2, self._param_updated, crazyflie.on_params_flushed) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self.values = {} def request_update_of_all_params(self): """Request an update of all the parameters in the TOC""" for group in self.toc.toc: for name in self.toc.toc[group]: complete_name = '%s.%s' % (group, name) self.request_param_update(complete_name) def _check_if_all_updated(self): """Check if all parameters from the TOC has at least been fetched once""" for g in self.toc.toc: if g not in self.values: return False for n in self.toc.toc[g]: if n not in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: s = struct.unpack(element.pytype, pk.data[1:])[0] s = s.__str__() complete_name = '%s.%s' % (element.group, element.name) # Save the value for synchronous access if element.group not in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( complete_name, s) if element.group in self.group_update_callbacks: self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) # Once all the parameters are updated call the # callback for "everything updated" (after all the param # updated callbacks) if self._check_if_all_updated() and not self.is_updated: self.is_updated = True self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) def remove_update_callback(self, group, name=None, cb=None): """Remove the supplied callback for a group or a group.name""" if not cb: return if not name: if group in self.group_update_callbacks: self.group_update_callbacks[group].remove_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname in self.param_update_callbacks: self.param_update_callbacks[paramname].remove_callback(cb) def add_update_callback(self, group=None, name=None, cb=None): """ Add a callback for a specific parameter name. This callback will be executed when a new value is read from the Crazyflie. """ if not group and not name: self.all_update_callback.add_callback(cb) elif not name: if group not in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ element = self.toc.get_element_by_complete_name(complete_name) if not element: logger.warning("Cannot set value for [%s], it's not in the TOC!", complete_name) raise KeyError('{} not in param TOC'.format(complete_name)) elif element.access == ParamTocElement.RO_ACCESS: logger.debug('[%s] is read only, no trying to set value', complete_name) raise AttributeError('{} is read-only!'.format(complete_name)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) if self._useV2: pk.data = struct.pack('<H', varid) else: pk.data = struct.pack('<B', varid) try: value_nr = eval(value) except TypeError: value_nr = value pk.data += struct.pack(element.pytype, value_nr) # print("Id for parameter: ", complete_name, " = ", varid) self.param_updater.request_param_setvalue(pk)
class CallerTest(unittest.TestCase): def setUp(self): self.callback_count = 0 self.sut = Caller() def test_that_callback_is_added(self): # Fixture # Test self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_callback_is_added_only_one_time(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_multiple_callbacks_are_added(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback2) # Assert self.sut.call() self.assertEqual(2, self.callback_count) def test_that_callback_is_removed(self): # Fixture self.sut.add_callback(self._callback) # Test self.sut.remove_callback(self._callback) # Assert self.sut.call() self.assertEqual(0, self.callback_count) def test_that_callback_is_called_with_arguments(self): # Fixture self.sut.add_callback(self._callback_with_args) # Test self.sut.call('The token') # Assert self.assertEqual('The token', self.callback_token) def _callback(self): self.callback_count += 1 def _callback2(self): self.callback_count += 1 def _callback_with_args(self, token): self.callback_token = token
class Memory(): """Access memories on the Crazyflie""" # These codes can be decoded using os.stderror, but # some of the text messages will look very stange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: "No more memory available", errno.ENOEXEC: "Command not found", errno.ENOENT: "No such block id", errno.E2BIG: "Block too large", errno.EEXIST: "Block already exists" } def __init__(self, crazyflie=None): """Instantiate class and connect callbacks""" self.mems = [] # Called when new memories have been added self.mem_added_cb = Caller() # Called when new data has been read self.mem_read_cb = Caller() self.mem_write_cb = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self._refresh_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 self._ow_mem_fetch_index = 0 self._elem_data = () self._read_requests = {} self._write_requests = {} self._ow_mems_left_to_update = [] self._getting_count = False def _mem_update_done(self, mem): """Callback from each individual memory (only 1-wire) when reading of header/elements are done""" if mem.id in self._ow_mems_left_to_update: self._ow_mems_left_to_update.remove(mem.id) logger.info(mem) if len(self._ow_mems_left_to_update) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None def get_mem(self, id): """Fetch the memory with the supplied id""" for m in self.mems: if m.id == id: return m return None def get_mems(self, type): """Fetch all the memories of the supplied type""" ret = () for m in self.mems: if m.type == type: ret += (m, ) return ret def write(self, memory, addr, data): """Write the specified data to the given memory at the given address""" if memory.id in self._write_requests: logger.warning("There is already a write operation ongoing for memory id {}".format(memory.id)) return False wreq = _WriteRequest(memory, addr, data, self.cf) self._write_requests[memory.id] = wreq wreq.start() return True def read(self, memory, addr, length): """Read the specified amount of bytes from the given memory at the given address""" if memory.id in self._read_requests: logger.warning("There is already a read operation ongoing for memory id {}".format(memory.id)) return False rreq = _ReadRequest(memory, addr, length, self.cf) self._read_requests[memory.id] = rreq rreq.start() return True def refresh(self, refresh_done_callback): """Start fetching all the detected memories""" self._refresh_callback = refresh_done_callback self._fetch_id = 0 for m in self.mems: try: self.mem_read_cb.remove_callback(m.new_data) m.disconnect() except Exception as e: logger.info("Error when removing memory after update: {}".format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info("Requesting number of memories") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR, ) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) #logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload))) if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = ord(payload[0]) logger.info("{} memories found".format(self.nbr_of_mems)) # Start requesting information about the memories, if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info("Requesting first id") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory (the I2C). logger.error("-------->Got good count, but no info on mem!") self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = ord(payload[0]) # Type - 1 byte mem_type = ord(payload[1]) # Size 4 bytes (as addr) mem_size = struct.unpack("I", payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack("B"*8, payload[6:14]) mem_addr = "" for m in mem_addr_raw: mem_addr += "{:02X}".format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) #logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info("Requesting information about memory {}".format(self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info("Done getting all the memories, start reading the OWs") ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise we are done for ow_mem in self.get_mems(MemoryElement.TYPE_1W): ow_mem.update(self._mem_update_done) if len (self.get_mems(MemoryElement.TYPE_1W)) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status)) # Find the read request if id in self._write_requests: wreq = self._write_requests[id] if status == 0: if wreq.write_done(addr): self._write_requests.pop(id, None) self.mem_write_cb.call(wreq.mem, wreq.addr) else: wreq.resend() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) data = struct.unpack("B"*len(payload[5:]), payload[5:]) logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info("READING: We are still interested in request for mem {}".format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: rreq.resend()
class Log(): """Create log configuration""" # These codes can be decoded using os.stderror, but # some of the text messages will look very stange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: "No more memory available", errno.ENOEXEC: "Command not found", errno.ENOENT: "No such block id", errno.E2BIG: "Block too large" } def __init__(self, crazyflie=None): self.log_blocks = [] # Called with newly created blocks self.block_added_cb = Caller() self.cf = crazyflie self.toc = None self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb) self.toc_updated = Caller() self.state = IDLE self.fake_toc_crc = 0xDEADBEEF def add_config(self, logconf): """Add a log configuration to the logging framework. When doing this the contents of the log configuration will be validated and listeners for new log configurations will be notified. When validating the configuration the variables are checked against the TOC to see that they actually exist. If they don't then the configuration cannot be used. Since a valid TOC is required, a Crazyflie has to be connected when calling this method, otherwise it will fail.""" if not self.cf.link: logger.error("Cannot add configs without being connected to a " "Crazyflie!") return # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. for name in logconf.default_fetch_as: var = self.toc.get_element_by_complete_name(name) if not var: logger.warning("%s not in TOC, this block cannot be" " used!", name) logconf.valid = False return # Now that we know what type this variable has, add it to the log # config again with the correct type logconf.add_variable(name, var.ctype) # Now check that all the added variables are in the TOC and that # the total size constraint of a data packet with logging data is # not size = 0 for var in logconf.variables: size += LogTocElement.get_size_from_id(var.fetch_as) # Check that we are able to find the variable in the TOC so # we can return error already now and not when the config is sent if var.is_toc_variable(): if (self.toc.get_element_by_complete_name(var.name) is None): logger.warning( "Log: %s not in TOC, this block cannot be" " used!", var.name) logconf.valid = False return if (size <= MAX_LOG_DATA_PACKET_SIZE and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf self.log_blocks.append(logconf) self.block_added_cb.call(logconf) else: logconf.valid = False def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING, ) self.cf.send_packet(pk, expect_answer=True) self.log_blocks = [] self.toc = Toc() toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _find_block(self, id): for block in self.log_blocks: if block.id == id: return block return None def _new_packet_cb(self, packet): """Callback for newly arrived packets with TOC information""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) if (chan == CHAN_SETTINGS): id = ord(payload[0]) error_status = ord(payload[1]) block = self._find_block(id) if (cmd == CMD_CREATE_BLOCK): if (block is not None): if (error_status == 0): # No error logger.debug("Have successfully added id=%d", id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, id, block.period) self.cf.send_packet(pk, expect_answer=True) block.added = True else: msg = self._err_codes[error_status] logger.warning("Error %d when adding id=%d (%s)", error_status, id, msg) block.err_no = error_status block.added_cb.call(False) block.error_cb.call(block, msg) else: logger.warning("No LogEntry to assign block to !!!") if (cmd == CMD_START_LOGGING): if (error_status == 0x00): logger.info( "Have successfully started logging for block=%d", id) if block: block.started = True else: msg = self._err_codes[error_status] logger.warning("Error %d when starting id=%d (%s)", error_status, id, msg) if block: block.err_no = error_status block.started_cb.call(False) block.error_cb.call(block, msg) if (cmd == CMD_STOP_LOGGING): if (error_status == 0x00): logger.info( "Have successfully stopped logging for block=%d", id) if block: block.started = False if (cmd == CMD_DELETE_BLOCK): if (error_status == 0x00): logger.info("Have successfully deleted block=%d", id) if block: block.started = False block.added = False if (chan == CHAN_LOGDATA): chan = packet.channel id = packet.datal[0] block = self._find_block(id) timestamps = struct.unpack("<BBB", packet.data[1:4]) timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16) logdata = packet.data[4:] if (block is not None): block.unpack_log_data(logdata, timestamp) else: logger.warning("Error no LogEntry to handle id=%d", id)
class PoseLogger: LOG_NAME_ESTIMATE_X = 'stateEstimate.x' LOG_NAME_ESTIMATE_Y = 'stateEstimate.y' LOG_NAME_ESTIMATE_Z = 'stateEstimate.z' LOG_NAME_ESTIMATE_ROLL = 'stateEstimate.roll' LOG_NAME_ESTIMATE_PITCH = 'stateEstimate.pitch' LOG_NAME_ESTIMATE_YAW = 'stateEstimate.yaw' NO_POSE = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) def __init__(self, cf: Crazyflie) -> None: self._cf = cf self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self.data_received_cb = Caller() self.error_cb = Caller() # The pose is an array containing # X, Y, Z, roll, pitch, yaw # roll, pitch and yaw in degrees self.pose = self.NO_POSE @property def position(self): """Get the position part of the full pose""" return self.pose[0:3] @property def rpy(self): """Get the roll, pitch and yaw of the full pose in degrees""" return self.pose[3:6] @property def rpy_rad(self): """Get the roll, pitch and yaw of the full pose in radians""" return [ math.radians(self.pose[3]), math.radians(self.pose[4]), math.radians(self.pose[5]) ] def _connected(self, link_uri) -> None: logConf = LogConfig("Pose", 40) logConf.add_variable(self.LOG_NAME_ESTIMATE_X, "float") logConf.add_variable(self.LOG_NAME_ESTIMATE_Y, "float") logConf.add_variable(self.LOG_NAME_ESTIMATE_Z, "float") logConf.add_variable(self.LOG_NAME_ESTIMATE_ROLL, "float") logConf.add_variable(self.LOG_NAME_ESTIMATE_PITCH, "float") logConf.add_variable(self.LOG_NAME_ESTIMATE_YAW, "float") try: self._cf.log.add_config(logConf) logConf.data_received_cb.add_callback(self._data_received) logConf.error_cb.add_callback(self._error) logConf.start() except KeyError as e: logger.warning(str(e)) except AttributeError as e: logger.warning(str(e)) def _disconnected(self, link_uri) -> None: self.pose = self.NO_POSE def _data_received(self, timestamp, data, logconf) -> None: self.pose = ( data[self.LOG_NAME_ESTIMATE_X], data[self.LOG_NAME_ESTIMATE_Y], data[self.LOG_NAME_ESTIMATE_Z], data[self.LOG_NAME_ESTIMATE_ROLL], data[self.LOG_NAME_ESTIMATE_PITCH], data[self.LOG_NAME_ESTIMATE_YAW], ) self.data_received_cb.call(self, self.pose) def _error(self, log_conf, msg) -> None: self.error_cb.call(self, msg)
class Localization(): """ Handle localization-related data communication with the Crazyflie """ # Implemented channels POSITION_CH = 0 GENERIC_CH = 1 # Location message types for generig channel RANGE_STREAM_REPORT = 0x00 RANGE_STREAM_REPORT_FP16 = 0x01 LPS_SHORT_LPP_PACKET = 0x02 def __init__(self, crazyflie=None): """ Initialize the Extpos object. """ self._cf = crazyflie self.receivedLocationPacket = Caller() self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming) def _incoming(self, packet): """ Callback for data received from the copter. """ if len(packet.data) < 1: logger.warning('Localization packet received with incorrect' + 'length (length is {})'.format(len(packet.data))) return pk_type = struct.unpack('<B', packet.data[:1])[0] data = packet.data[1:] # Decoding the known packet types # TODO: more generic decoding scheme? decoded_data = None if pk_type == self.RANGE_STREAM_REPORT: if len(data) % 5 != 0: logger.error('Wrong range stream report data lenght') return decoded_data = {} raw_data = data for i in range(int(len(data) / 5)): anchor_id, distance = struct.unpack('<Bf', raw_data[:5]) decoded_data[anchor_id] = distance raw_data = raw_data[5:] pk = LocalizationPacket(pk_type, data, decoded_data) self.receivedLocationPacket.call(pk) def send_extpos(self, pos): """ Send the current Crazyflie X, Y, Z position. This is going to be forwarded to the Crazyflie's position estimator. """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.POSITION_CH pk.data = struct.pack('<fff', pos[0], pos[1], pos[2]) self._cf.send_packet(pk) def send_short_lpp_packet(self, dest_id, data): """ Send ultra-wide-band LPP packet to dest_id """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data self._cf.send_packet(pk)
class Memory: """Access memories on the Crazyflie""" # These codes can be decoded using os.stderror, but # some of the text messages will look very stange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: "No more memory available", errno.ENOEXEC: "Command not found", errno.ENOENT: "No such block id", errno.E2BIG: "Block too large", errno.EEXIST: "Block already exists", } def __init__(self, crazyflie=None): """Instantiate class and connect callbacks""" self.mems = [] # Called when new memories have been added self.mem_added_cb = Caller() # Called when new data has been read self.mem_read_cb = Caller() self.mem_write_cb = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self._refresh_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 self._ow_mem_fetch_index = 0 self._elem_data = () self._read_requests = {} self._write_requests = {} self._ow_mems_left_to_update = [] self._getting_count = False def _mem_update_done(self, mem): """Callback from each individual memory (only 1-wire) when reading of header/elements are done""" if mem.id in self._ow_mems_left_to_update: self._ow_mems_left_to_update.remove(mem.id) logger.info(mem) if len(self._ow_mems_left_to_update) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None def get_mem(self, id): """Fetch the memory with the supplied id""" for m in self.mems: if m.id == id: return m return None def get_mems(self, type): """Fetch all the memories of the supplied type""" ret = () for m in self.mems: if m.type == type: ret += (m,) return ret def write(self, memory, addr, data): """Write the specified data to the given memory at the given address""" if memory.id in self._write_requests: logger.warning("There is already a write operation ongoing for memory id {}".format(memory.id)) return False wreq = _WriteRequest(memory, addr, data, self.cf) self._write_requests[memory.id] = wreq wreq.start() return True def read(self, memory, addr, length): """Read the specified amount of bytes from the given memory at the given address""" if memory.id in self._read_requests: logger.warning("There is already a read operation ongoing for memory id {}".format(memory.id)) return False rreq = _ReadRequest(memory, addr, length, self.cf) self._read_requests[memory.id] = rreq rreq.start() return True def refresh(self, refresh_done_callback): """Start fetching all the detected memories""" self._refresh_callback = refresh_done_callback self._fetch_id = 0 for m in self.mems: try: self.mem_read_cb.remove_callback(m.new_data) m.disconnect() except Exception as e: logger.info("Error when removing memory after update: {}".format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info("Requesting number of memories") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR,) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) # logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload))) if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = ord(payload[0]) logger.info("{} memories found".format(self.nbr_of_mems)) # Start requesting information about the memories, if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info("Requesting first id") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory (the I2C). logger.error("-------->Got good count, but no info on mem!") self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = ord(payload[0]) # Type - 1 byte mem_type = ord(payload[1]) # Size 4 bytes (as addr) mem_size = struct.unpack("I", payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack("B" * 8, payload[6:14]) mem_addr = "" for m in mem_addr_raw: mem_addr += "{:02X}".format(m) if not self.get_mem(mem_id): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info("Requesting information about memory {}".format(self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info("Done getting all the memories, start reading the OWs") ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise we are done for ow_mem in self.get_mems(MemoryElement.TYPE_1W): ow_mem.update(self._mem_update_done) if len(self.get_mems(MemoryElement.TYPE_1W)) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status)) # Find the read request if id in self._write_requests: wreq = self._write_requests[id] if status == 0: if wreq.write_done(addr): self._write_requests.pop(id, None) self.mem_write_cb.call(wreq.mem, wreq.addr) else: wreq.resend() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) data = struct.unpack("B" * len(payload[5:]), payload[5:]) logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info("READING: We are still interested in request for mem {}".format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: rreq.resend()
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): self._input_device = None self._min_thrust = 0 self._max_thrust = 0 self._thrust_slew_rate = 0 self._thrust_slew_enabled = False self._thrust_slew_limit = 0 self._has_pressure_sensor = False self._old_thrust = 0 self._old_raw_thrust = 0 self._old_alt_hold = False self._springy_throttle = True self._prev_values = {} self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") self._input_map = None self._mux = [ NoMux(self), SelectiveMux(self), TakeOverMux(self), MixMux(self), TakeOverSelectiveMux(self) ] # Set NoMux as default self._selected_mux = self._mux[0] if Config().get("flightmode") is "Normal": self.set_yaw_limit(Config().get("normal_max_yaw")) self.set_rp_limit(Config().get("normal_max_rp")) # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("normal_min_thrust"), Config().get("normal_max_thrust")) self.set_thrust_slew_limiting(Config().get("normal_slew_rate"), Config().get("normal_slew_limit")) else: self.set_yaw_limit(Config().get("max_yaw")) self.set_rp_limit(Config().get("max_rp")) # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("min_thrust"), Config().get("max_thrust")) self.set_thrust_slew_limiting(Config().get("slew_rate"), Config().get("slew_limit")) self._dev_blacklist = None if len(Config().get("input_device_blacklist")) > 0: self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager().configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() self.alt1_updated = Caller() self.alt2_updated = Caller() # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting) self.limiting_updated = Caller() def set_alt_hold_available(self, available): """Set if altitude hold is available or not (depending on HW)""" self._has_pressure_sensor = available def enable_alt_hold(self, althold): """Enable or disable altitude hold""" self._old_alt_hold = althold def _do_device_discovery(self): devs = self.available_devices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def available_mux(self): available = [] for m in self._mux: available.append(m.name) return available def set_mux(self, name=None, mux=None): if name: for m in self._mux: if m.name == name: self._selected_mux = m elif mux: self._selected_mux = mux logger.info("Selected MUX: {}".format(self._selected_mux.name)) def get_mux_supported_dev_count(self): return self._selected_mux.get_supported_dev_count() def available_devices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = readers.devices() devs += interfaces.devices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev.name))): approved_devs.append(dev) return approved_devs def enableRawReading(self, device_name): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ if self._input_device: self._input_device.close() self._input_device = None for d in readers.devices(): if d.name == device_name: self._input_device = d # Set the mapping to None to get raw values self._input_device.input_map = None self._input_device.open() def get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in device_config_mapping.keys(): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def stop_raw_reading(self): """Disable raw reading of input device.""" if self._input_device: self._input_device.close() self._input_device = None def read_raw_values(self): """ Read raw values from the input device.""" [axes, buttons, mapped_values] = self._input_device.read(include_raw=True) dict_axes = {} dict_buttons = {} for i, a in enumerate(axes): dict_axes[i] = a for i, b in enumerate(buttons): dict_buttons[i] = b return [dict_axes, dict_buttons, mapped_values] def set_raw_input_map(self, input_map): """Set an input device map""" if self._input_device: self._input_device.input_map = input_map def set_input_map(self, device_name, input_map_name): """Load and set an input device map with the given name""" settings = ConfigManager().get_settings(input_map_name) if settings: self._springy_throttle = settings["springythrottle"] self._input_map = ConfigManager().get_config(input_map_name) if self._input_device: self._input_device.input_map = self._input_map Config().get("device_config_mapping")[device_name] = input_map_name def get_device_name(self): """Get the name of the current open device""" if self._input_device: return self._input_device.name return None def start_input(self, device_name, config_name=None): """ Start reading input from the device with name device_name using config config_name. Returns True if device supports mapping, otherwise False """ try: #device_id = self._available_devices[device_name] # Check if we supplied a new map, if not use the preferred one for d in readers.devices(): if d.name == device_name: self._input_device = d if not config_name: config_name = self.get_saved_device_mapping( device_name) self.set_input_map(device_name, config_name) self._input_device.open() self._input_device.input_map = self._input_map self._input_device.input_map_name = config_name self._selected_mux.add_device(self._input_device, None) # Update the UI with the limiting for this device self.limiting_updated.call(self._input_device.limit_rp, self._input_device.limit_yaw, self._input_device.limit_thrust) self._read_timer.start() return self._input_device.supports_mapping except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) if not self._input_device: self.device_error.call( "Could not find device {}".format(device_name)) return False def stop_input(self, device_name=None): """Stop reading from the input device.""" if device_name: for d in readers.devices(): if d.name == device_name: d.close() else: for d in readers.devices(): d.close() #if self._input_device: # self._input_device.close() # self._input_device = None def set_yaw_limit(self, max_yaw_rate): """Set a new max yaw rate value.""" for m in self._mux: m.max_yaw_rate = max_yaw_rate def set_rp_limit(self, max_rp_angle): """Set a new max roll/pitch value.""" for m in self._mux: m.max_rp_angle = max_rp_angle def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" for m in self._mux: m.thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate) m.thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit) if thrust_slew_rate > 0: m.thrust_slew_enabled = True else: m.thrust_slew_enabled = False def set_thrust_limits(self, min_thrust, max_thrust): """Set a new min/max thrust limit.""" for m in self._mux: m.min_thrust = JoystickReader.p2t(min_thrust) m.max_thrust = JoystickReader.p2t(max_thrust) def set_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" for m in self._mux: m.trim_roll = trim_roll def set_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" for m in self._mux: m.trim_pitch = trim_pitch def _calc_rp_trim(self, key_neg, key_pos, data): if self._check_toggle(key_neg, data) and not data[key_neg]: return -1.0 if self._check_toggle(key_pos, data) and not data[key_pos]: return 1.0 return 0.0 def _check_toggle(self, key, data): if not key in self._prev_values: self._prev_values[key] = data[key] elif self._prev_values[key] != data[key]: self._prev_values[key] = data[key] return True return False def read_input(self): """Read input data from the selected device""" try: [roll, pitch, yaw, thrust] = self._selected_mux.read() #if trim_roll != 0 or trim_pitch != 0: # self._trim_roll += trim_roll # self._trim_pitch += trim_pitch # self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) #trimmed_roll = roll + self._trim_roll #trimmed_pitch = pitch + self._trim_pitch # Thrust might be <0 here, make sure it's not otherwise we'll get an error. if thrust < 0: thrust = 0 if thrust > 65535: thrust = 65535 self.input_updated.call(roll, pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self.input_updated.call(0, 0, 0, 0) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) @staticmethod def deadband(value, threshold): if abs(value) < threshold: value = 0 elif value > 0: value -= threshold elif value < 0: value += threshold return value / (1 - threshold)
class LogEntry: """Representation of one log configuration that enables logging from the Crazyflie""" block_idCounter = 1 def __init__(self, crazyflie, logconf): """Initialize the entry""" self.data_received = Caller() self.error = Caller() self.logconf = logconf self.block_id = LogEntry.block_idCounter LogEntry.block_idCounter += 1 self.cf = crazyflie self.period = logconf.getPeriod() / 10 self.block_created = False def start(self): """Start the logging for this entry""" if (self.cf.link is not None): if (self.block_created is False): logger.debug("First time block is started, add block") self.block_created = True pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_CREATE_BLOCK, self.block_id) for var in self.logconf.getVariables(): if (var.isTocVariable() is False): # Memory location logger.debug("Logging to raw memory %d, 0x%04X", var.getStoredFetchAs(), var.getAddress()) pk.data += struct.pack('<B', var.getStoredFetchAs()) pk.data += struct.pack('<I', var.getAddress()) else: # Item in TOC logger.debug("Adding %s with id=%d and type=0x%02X", var.getName(), self.cf.log.toc.get_element_id( var.getName()), var.getStoredFetchAs()) pk.data += struct.pack('<B', var.getStoredFetchAs()) pk.data += struct.pack('<B', self.cf.log.toc. get_element_id(var.getName())) logger.debug("Adding log block id {}".format(self.block_id)) self.cf.send_packet(pk) else: logger.debug("Block already registered, starting logging" " for %d", self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.block_id, self.period) self.cf.send_packet(pk) def stop(self): """Stop the logging for this entry""" if (self.cf.link is not None): if (self.block_id is None): logger.warning("Stopping block, but no block registered") else: logger.debug("Sending stop logging for block %d", self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.block_id) self.cf.send_packet(pk) def close(self): """Delete this entry in the Crazyflie""" if (self.cf.link is not None): if (self.block_id is None): logger.warning("Delete block, but no block registered") else: logger.debug("LogEntry: Sending delete logging for block %d" % self.block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.block_id) self.cf.send_packet(pk) self.block_id = None # Wait until we get confirmation of delete def unpack_log_data(self, log_data): """Unpack received logging data so it represent real values according to the configuration in the entry""" ret_data = {} data_index = 0 for var in self.logconf.getVariables(): size = LogTocElement.get_size_from_id(var.getFetchAs()) name = var.getName() unpackstring = LogTocElement.get_unpack_string_from_id( var.getFetchAs()) value = struct.unpack(unpackstring, log_data[data_index:data_index + size])[0] data_index += size ret_data[name] = value self.data_received.call(ret_data)
class Localization(): """ Handle localization-related data communication with the Crazyflie """ # Implemented channels POSITION_CH = 0 GENERIC_CH = 1 # Location message types for generig channel RANGE_STREAM_REPORT = 0x00 RANGE_STREAM_REPORT_FP16 = 0x01 LPS_SHORT_LPP_PACKET = 0x02 def __init__(self, crazyflie=None): """ Initialize the Extpos object. """ self._cf = crazyflie self.receivedLocationPacket = Caller() self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming) def _incoming(self, packet): """ Callback for data received from the copter. """ if len(packet.data) < 1: logger.warning('Localization packet received with incorrect' + 'length (length is {})'.format(len(packet.data))) return pk_type = struct.unpack('<B', packet.data[:1])[0] data = packet.data[1:] # Decoding the known packet types # TODO: more generic decoding scheme? decoded_data = None if pk_type == self.RANGE_STREAM_REPORT: if len(data) % 5 != 0: logger.error('Wrong range stream report data lenght') return decoded_data = {} raw_data = data for i in range(int(len(data) / 5)): anchor_id, distance = struct.unpack('<Bf', raw_data[:5]) decoded_data[anchor_id] = distance raw_data = raw_data[5:] pk = LocalizationPacket(pk_type, data, decoded_data) self.receivedLocationPacket.call(pk) def send_extpos(self, pos): """ Send the current Crazyflie X, Y, Z position. This is going to be forwarded to the Crazyflie's position estimator. """crazyflie pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.POSITION_CH pk.data = struct.pack('<fff', pos[0], pos[1], pos[2]) self._cf.send_packet(pk) ## print("here\n") def send_short_lpp_packet(self, dest_id, data): """ Send ultra-wide-band LPP packet to dest_id """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data self._cf.send_packet(pk)
class Log(): """Create log configuration""" # These codes can be decoded using os.stderror, but # some of the text messages will look very strange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: 'No more memory available', errno.ENOEXEC: 'Command not found', errno.ENOENT: 'No such block id', errno.E2BIG: 'Block too large', errno.EEXIST: 'Block already exists' } def __init__(self, crazyflie=None): self.log_blocks = [] # Called with newly created blocks self.block_added_cb = Caller() self.cf = crazyflie self.toc = None self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb) self.toc_updated = Caller() self.state = IDLE self.fake_toc_crc = 0xDEADBEEF self._refresh_callback = None self._toc_cache = None self._config_id_counter = 1 def add_config(self, logconf): """Add a log configuration to the logging framework. When doing this the contents of the log configuration will be validated and listeners for new log configurations will be notified. When validating the configuration the variables are checked against the TOC to see that they actually exist. If they don't then the configuration cannot be used. Since a valid TOC is required, a Crazyflie has to be connected when calling this method, otherwise it will fail.""" if not self.cf.link: logger.error('Cannot add configs without being connected to a ' 'Crazyflie!') return # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. for name in logconf.default_fetch_as: var = self.toc.get_element_by_complete_name(name) if not var: logger.warning( '%s not in TOC, this block cannot be used!', name) logconf.valid = False raise KeyError('Variable {} not in TOC'.format(name)) # Now that we know what type this variable has, add it to the log # config again with the correct type logconf.add_variable(name, var.ctype) # Now check that all the added variables are in the TOC and that # the total size constraint of a data packet with logging data is # not size = 0 for var in logconf.variables: size += LogTocElement.get_size_from_id(var.fetch_as) # Check that we are able to find the variable in the TOC so # we can return error already now and not when the config is sent if var.is_toc_variable(): if (self.toc.get_element_by_complete_name(var.name) is None): logger.warning( 'Log: %s not in TOC, this block cannot be used!', var.name) logconf.valid = False raise KeyError('Variable {} not in TOC'.format(var.name)) if (size <= MAX_LOG_DATA_PACKET_SIZE and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf logconf.id = self._config_id_counter self._config_id_counter = (self._config_id_counter + 1) % 255 self.log_blocks.append(logconf) self.block_added_cb.call(logconf) else: logconf.valid = False raise AttributeError( 'The log configuration is too large or has an invalid ' 'parameter') def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" self._toc_cache = toc_cache self._refresh_callback = refresh_done_callback self.toc = None pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING,) self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING,)) def _find_block(self, id): for block in self.log_blocks: if block.id == id: return block return None def _new_packet_cb(self, packet): """Callback for newly arrived packets with TOC information""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if (chan == CHAN_SETTINGS): id = payload[0] error_status = payload[1] block = self._find_block(id) if (cmd == CMD_CREATE_BLOCK): if (block is not None): if error_status == 0 or error_status == errno.EEXIST: if not block.added: logger.debug('Have successfully added id=%d', id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, id, block.period) self.cf.send_packet(pk, expected_reply=( CMD_START_LOGGING, id)) block.added = True else: msg = self._err_codes[error_status] logger.warning('Error %d when adding id=%d (%s)', error_status, id, msg) block.err_no = error_status block.added_cb.call(False) block.error_cb.call(block, msg) else: logger.warning('No LogEntry to assign block to !!!') if (cmd == CMD_START_LOGGING): if (error_status == 0x00): logger.info('Have successfully started logging for id=%d', id) if block: block.started = True else: msg = self._err_codes[error_status] logger.warning('Error %d when starting id=%d (%s)', error_status, id, msg) if block: block.err_no = error_status block.started_cb.call(self, False) # This is a temporary fix, we are adding a new issue # for this. For some reason we get an error back after # the block has been started and added. This will show # an error in the UI, but everything is still working. # block.error_cb.call(block, msg) if (cmd == CMD_STOP_LOGGING): if (error_status == 0x00): logger.info('Have successfully stopped logging for id=%d', id) if block: block.started = False if (cmd == CMD_DELETE_BLOCK): # Accept deletion of a block that isn't added. This could # happen due to timing (i.e add/start/delete in fast sequence) if error_status == 0x00 or error_status == errno.ENOENT: logger.info('Have successfully deleted id=%d', id) if block: block.started = False block.added = False if (cmd == CMD_RESET_LOGGING): # Guard against multiple responses due to re-sending if not self.toc: logger.debug('Logging reset, continue with TOC download') self.log_blocks = [] self.toc = Toc() toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, self.toc, self._refresh_callback, self._toc_cache) toc_fetcher.start() if (chan == CHAN_LOGDATA): chan = packet.channel id = packet.data[0] block = self._find_block(id) timestamps = struct.unpack('<BBB', packet.data[1:4]) timestamp = ( timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16) logdata = packet.data[4:] if (block is not None): block.unpack_log_data(logdata, timestamp) else: logger.warning('Error no LogEntry to handle id=%d', id)
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True, cf=None): # TODO: Should be OS dependant self.inputdevice = AiController(cf) self._min_thrust = 0 self._max_thrust = 0 self._thrust_slew_rate = 0 self._thrust_slew_enabled = False self._thrust_slew_limit = 0 self._emergency_stop = False self._has_pressure_sensor = False self._old_thrust = 0 self._old_alt_hold = False self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") if (Config().get("flightmode") is "Normal"): self._max_yaw_rate = Config().get("normal_max_yaw") self._max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("normal_min_thrust"), Config().get("normal_max_thrust")) self.set_thrust_slew_limiting(Config().get("normal_slew_rate"), Config().get("normal_slew_limit")) else: self._max_yaw_rate = Config().get("max_yaw") self._max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("min_thrust"), Config().get("max_thrust")) self.set_thrust_slew_limiting(Config().get("slew_rate"), Config().get("slew_limit")) self._dev_blacklist = None if (len(Config().get("input_device_blacklist")) > 0): self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager().configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() def setAltHoldAvailable(self, available): self._has_pressure_sensor = available def setAltHold(self, althold): self._old_alt_hold = althold def _do_device_discovery(self): devs = self.getAvailableDevices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def getAvailableDevices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = self.inputdevice.getAvailableDevices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev["name"]))): self._available_devices[dev["name"]] = dev["id"] approved_devs.append(dev) return approved_devs def enableRawReading(self, deviceId): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ self.inputdevice.enableRawReading(deviceId) def disableRawReading(self): """Disable raw reading of input device.""" self.inputdevice.disableRawReading() def readRawValues(self): """ Read raw values from the input device.""" return self.inputdevice.readRawValues() def start_input(self, device_name, config_name): """ Start reading input from the device with name device_name using config config_name """ try: device_id = self._available_devices[device_name] self.inputdevice.start_input( device_id, ConfigManager().get_config(config_name)) self._read_timer.start() except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) def stop_input(self): """Stop reading from the input device.""" self._read_timer.stop() def set_yaw_limit(self, max_yaw_rate): """Set a new max yaw rate value.""" self._max_yaw_rate = max_yaw_rate def set_rp_limit(self, max_rp_angle): """Set a new max roll/pitch value.""" self._max_rp_angle = max_rp_angle def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate) self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit) if (thrust_slew_rate > 0): self._thrust_slew_enabled = True else: self._thrust_slew_enabled = False def set_thrust_limits(self, min_thrust, max_thrust): """Set a new min/max thrust limit.""" self._min_thrust = JoystickReader.p2t(min_thrust) self._max_thrust = JoystickReader.p2t(max_thrust) def set_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" self._trim_roll = trim_roll def set_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" self._trim_pitch = trim_pitch def read_input(self): """Read input data from the selected device""" try: data = self.inputdevice.read_input() roll = data["roll"] #* self._max_rp_angle pitch = data["pitch"] #* self._max_rp_angle thrust = data["thrust"] yaw = data["yaw"] raw_thrust = data["thrust"] emergency_stop = data["estop"] trim_roll = data["rollcal"] trim_pitch = data["pitchcal"] althold = data["althold"] #Deadband the roll and pitch roll = JoystickReader.deadband(roll, 0.2) * self._max_rp_angle pitch = JoystickReader.deadband(pitch, 0.2) * self._max_rp_angle if (self._old_alt_hold != althold): self.althold_updated.call(str(althold)) self._old_alt_hold = althold if self._emergency_stop != emergency_stop: self._emergency_stop = emergency_stop self.emergency_stop_updated.call(self._emergency_stop) # Thust limiting (slew, minimum and emergency stop) if althold and self._has_pressure_sensor: thrust = int( round( JoystickReader.deadband(thrust, 0.2) * 32767 + 32767)) #Convert to uint16 else: if raw_thrust < 0.15 or emergency_stop: thrust = 0 else: thrust = self._min_thrust + thrust * (self._max_thrust - self._min_thrust) if (self._thrust_slew_enabled == True and self._thrust_slew_limit > thrust and not emergency_stop): if self._old_thrust > self._thrust_slew_limit: self._old_thrust = self._thrust_slew_limit if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)): thrust = self._old_thrust - self._thrust_slew_rate / 100 if raw_thrust < 0 or thrust < self._min_thrust: thrust = 0 self._old_thrust = thrust # Yaw deadband # TODO: Add to input device config? yaw = JoystickReader.deadband(yaw, 0.2) * self._max_yaw_rate if trim_roll != 0 or trim_pitch != 0: self._trim_roll += trim_roll self._trim_pitch += trim_pitch self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) @staticmethod def deadband(value, threshold): if abs(value) < threshold: value = 0 elif value > 0: value -= threshold elif value < 0: value += threshold return value / (1 - threshold)
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): # TODO: Should be OS dependant self.inputdevice = PyGameReader() self.maxRPAngle = 0 self.thrustDownSlew = 0 self.thrustSlewEnabled = False self.slewEnableLimit = 0 self.maxYawRate = 0 self.detectAxis = False self.emergencyStop = False self.oldThrust = 0 self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") # TODO: The polling interval should be set from config file self.readTimer = PeriodicTimer(0.01, self.readInput) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() self._available_devices = {} # Check if user config exists, otherwise copy files if not os.path.isdir(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() def _do_device_discovery(self): devs = self.getAvailableDevices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def getAvailableDevices(self): """List all available input devices.""" devs = self.inputdevice.getAvailableDevices() for d in devs: self._available_devices[d["name"]] = d["id"] return devs def enableRawReading(self, deviceId): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ self.inputdevice.enableRawReading(deviceId) def disableRawReading(self): """Disable raw reading of input device.""" self.inputdevice.disableRawReading() def readRawValues(self): """ Read raw values from the input device.""" return self.inputdevice.readRawValues() def start_input(self, device_name, config_name): """ Start reading input from the device with name device_name using config config_name """ try: device_id = self._available_devices[device_name] self.inputdevice.start_input( device_id, ConfigManager().get_config(config_name)) self.readTimer.start() except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) def stop_input(self): """Stop reading from the input device.""" self.readTimer.stop() def set_yaw_limit(self, maxRate): """Set a new max yaw rate value.""" self.maxYawRate = maxRate def set_rp_limit(self, maxAngle): """Set a new max roll/pitch value.""" self.maxRPAngle = maxAngle def set_thrust_slew_limiting(self, thrustDownSlew, slewLimit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" self.thrustDownSlew = thrustDownSlew self.slewEnableLimit = slewLimit if (thrustDownSlew > 0): self.thrustSlewEnabled = True else: self.thrustSlewEnabled = False def set_thrust_limits(self, minThrust, maxThrust): """Set a new min/max thrust limit.""" self.minThrust = minThrust self.maxThrust = maxThrust def _update_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" self._trim_roll = trim_roll def _update_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" self._trim_pitch = trim_pitch def readInput(self): """Read input data from the selected device""" try: data = self.inputdevice.readInput() roll = data["roll"] * self.maxRPAngle pitch = data["pitch"] * self.maxRPAngle thrust = data["thrust"] yaw = data["yaw"] raw_thrust = data["thrust"] emergency_stop = data["estop"] trim_roll = data["rollcal"] trim_pitch = data["pitchcal"] if self.emergencyStop != emergency_stop: self.emergencyStop = emergency_stop self.emergency_stop_updated.call(self.emergencyStop) # Thust limiting (slew, minimum and emergency stop) if raw_thrust < 0.05 or emergency_stop: thrust = 0 else: thrust = self.minThrust + thrust * (self.maxThrust - self.minThrust) if (self.thrustSlewEnabled == True and self.slewEnableLimit > thrust and not emergency_stop): if self.oldThrust > self.slewEnableLimit: self.oldThrust = self.slewEnableLimit if thrust < (self.oldThrust - (self.thrustDownSlew / 100)): thrust = self.oldThrust - self.thrustDownSlew / 100 if raw_thrust < 0 or thrust < self.minThrust: thrust = 0 self.oldThrust = thrust # Yaw deadband # TODO: Add to input device config? if yaw < -0.2 or yaw > 0.2: if yaw < 0: yaw = (yaw + 0.2) * self.maxYawRate * 1.25 else: yaw = (yaw - 0.2) * self.maxYawRate * 1.25 else: self.yaw = 0 if trim_roll != 0 or trim_pitch != 0: self._trim_roll += trim_roll self._trim_pitch += trim_pitch self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call( "Error reading from input device\n\n%s" % traceback.format_exc()) self.readTimer.stop()
class Crazyflie(): """The Crazyflie class""" def __init__(self, link=None, ro_cache=None, rw_cache=None): """ Create the objects from this module and register callbacks. ro_cache -- Path to read-only cache (string) rw_cache -- Path to read-write cache (string) """ # Called on disconnect, no matter the reason self.disconnected = Caller() # Called on unintentional disconnect only self.connection_lost = Caller() # Called when the first packet in a new link is received self.link_established = Caller() # Called when the user requests a connection self.connection_requested = Caller() # Called when the link is established and the TOCs (that are not # cached) have been downloaded self.connected = Caller() # Called if establishing of the link fails (i.e times out) self.connection_failed = Caller() # Called for every packet received self.packet_received = Caller() # Called for every packet sent self.packet_sent = Caller() # Called when the link driver updates the link quality measurement self.link_quality_updated = Caller() self.state = State.DISCONNECTED self.link = link self._toc_cache = TocCache(ro_cache=ro_cache, rw_cache=rw_cache) self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) self.incoming.start() self.commander = Commander(self) self.high_level_commander = HighLevelCommander(self) self.loc = Localization(self) self.extpos = Extpos(self) self.log = Log(self) self.console = Console(self) self.param = Param(self) self.mem = Memory(self) self.platform = PlatformService(self) self.link_uri = '' # Used for retry when no reply was sent back self.packet_received.add_callback(self._check_for_initial_packet_cb) self.packet_received.add_callback(self._check_for_answers) self._answer_patterns = {} self._send_lock = Lock() self.connected_ts = None # Connect callbacks to logger self.disconnected.add_callback( lambda uri: logger.info('Callback->Disconnected from [%s]', uri)) self.disconnected.add_callback(self._disconnected) self.link_established.add_callback( lambda uri: logger.info('Callback->Connected to [%s]', uri)) self.connection_lost.add_callback(lambda uri, errmsg: logger.info( 'Callback->Connection lost to [%s]: %s', uri, errmsg)) self.connection_failed.add_callback(lambda uri, errmsg: logger.info( 'Callback->Connected failed to [%s]: %s', uri, errmsg)) self.connection_requested.add_callback(lambda uri: logger.info( 'Callback->Connection initialized[%s]', uri)) self.connected.add_callback(lambda uri: logger.info( 'Callback->Connection setup finished [%s]', uri)) def _disconnected(self, link_uri): """ Callback when disconnected.""" self.connected_ts = None def _start_connection_setup(self): """Start the connection setup by refreshing the TOCs""" logger.info('We are connected[%s], request connection setup', self.link_uri) self.platform.fetch_platform_informations(self._platform_info_fetched) def _platform_info_fetched(self): self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache) def _param_toc_updated_cb(self): """Called when the param TOC has been fully updated""" logger.info('Param TOC finished updating') self.connected_ts = datetime.datetime.now() self.connected.call(self.link_uri) # Trigger the update for all the parameters self.param.request_update_of_all_params() def _mems_updated_cb(self): """Called when the memories have been identified""" logger.info('Memories finished updating') self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache) def _log_toc_updated_cb(self): """Called when the log TOC has been fully updated""" logger.info('Log TOC finished updating') self.mem.refresh(self._mems_updated_cb) def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED or self.state == State.SETUP_FINISHED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection (download log/parameter TOC). """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver(link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) self._start_connection_setup() except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self._answer_patterns = {} self.disconnected.call(self.link_uri) """Check if the communication link is open or not.""" def is_connected(self): return self.connected_ts is not None def add_port_callback(self, port, cb): """Add a callback to cb on port""" self.incoming.add_port_callback(port, cb) def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) def _no_answer_do_retry(self, pk, pattern): """Resend packets that we have not gotten answers to""" logger.info('Resending for pattern %s', pattern) # Set the timer to None before trying to send again self.send_packet(pk, expected_reply=pattern, resend=True) def _check_for_answers(self, pk): """ Callback called for every packet received to check if we are waiting for an answer on this port. If so, then cancel the retry timer. """ longest_match = () if len(self._answer_patterns) > 0: data = (pk.header, ) + tuple(pk.data) for p in list(self._answer_patterns.keys()): logger.debug('Looking for pattern match on %s vs %s', p, data) if len(p) <= len(data): if p == data[0:len(p)]: match = data[0:len(p)] if len(match) >= len(longest_match): logger.debug('Found new longest match %s', match) longest_match = match if len(longest_match) > 0: self._answer_patterns[longest_match].cancel() del self._answer_patterns[longest_match] def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ Send a packet through the link interface. pk -- Packet to send expect_answer -- True if a packet from the Crazyflie is expected to be sent back, otherwise false """ # print("Send packet in port: %x"%(pk.port)) self._send_lock.acquire() if self.link is not None: if len(expected_reply ) > 0 and not resend and self.link.needs_resending: pattern = (pk.header, ) + expected_reply logger.debug( 'Sending packet and expecting the %s pattern back', pattern) new_timer = Timer( timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() elif resend: # Check if we have gotten an answer, if not try again pattern = expected_reply if pattern in self._answer_patterns: logger.debug('We want to resend and the pattern is there') if self._answer_patterns[pattern]: new_timer = Timer( timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() else: logger.debug('Resend requested, but no pattern found: %s', self._answer_patterns) self.link.send_packet(pk) self.packet_sent.call(pk) self._send_lock.release()
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): # TODO: Should be OS dependant self.SF = sensorFusion() self.inputdevice = PyGameReader() self.pointerDevice = psmove.PSMove() self.PointerYaw = 0 self.kalmanPitch = KalmanFilter() self.kalmanRoll = KalmanFilter() self.viscousModeThrust = 67 self._emergency_landing = False self.auto = False self._min_thrust = 0 self._max_thrust = 0 self._maxAltitude = 0 self.currentAltitude = 0 self.minAltitude = 0 self._thrust_slew_rate = 0 self._thrust_slew_enabled = False self._thrust_slew_limit = 0 self._emergency_stop = False self._has_pressure_sensor = False self._canSwitch = True self._old_thrust = 0 self._old_alt_hold = False self._old_flip_type = -1 self._flip_time_start = -float("inf") self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") self._trim_yaw = 0.0 if (Config().get("flightmode") is "Normal"): self._max_yaw_rate = Config().get("normal_max_yaw") self._max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("normal_min_thrust"), Config().get("normal_max_thrust")) self.set_thrust_slew_limiting(Config().get("normal_slew_rate"), Config().get("normal_slew_limit")) else: self._max_yaw_rate = Config().get("max_yaw") self._max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits(Config().get("min_thrust"), Config().get("max_thrust")) self.set_thrust_slew_limiting(Config().get("slew_rate"), Config().get("slew_limit")) self._dev_blacklist = None if (len(Config().get("input_device_blacklist")) > 0): self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager().configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.switch_mode_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() self.auto_input_updated = Caller() self.pointer_input_updated = Caller() def setViscousModeThrust(self, thrust): if thrust >= 0: self.viscousModeThrust = thrust def setEmergencyLanding(self, emergencyLanding): self._emergency_landing = emergencyLanding def setAltHoldAvailable(self, available): self._has_pressure_sensor = available def setAuto(self, auto): self.auto = auto def setAltHold(self, althold): self._old_alt_hold = althold def _do_device_discovery(self): devs = self.getAvailableDevices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def getAvailableDevices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = self.inputdevice.getAvailableDevices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev["name"]))): self._available_devices[dev["name"]] = dev["id"] approved_devs.append(dev) return approved_devs def enableRawReading(self, deviceId): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ self.inputdevice.enableRawReading(deviceId) def disableRawReading(self): """Disable raw reading of input device.""" self.inputdevice.disableRawReading() def readRawValues(self): """ Read raw values from the input device.""" return self.inputdevice.readRawValues() def start_input(self, device_name, config_name): """ Start reading input from the device with name device_name using config config_name """ try: device_id = self._available_devices[device_name] self.inputdevice.start_input( device_id, ConfigManager().get_config(config_name)) self._read_timer.start() except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) def stop_input(self): """Stop reading from the input device.""" self._read_timer.stop() def set_yaw_limit(self, max_yaw_rate): """Set a new max yaw rate value.""" self._max_yaw_rate = max_yaw_rate def set_rp_limit(self, max_rp_angle): """Set a new max roll/pitch value.""" self._max_rp_angle = max_rp_angle def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate) self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit) if (thrust_slew_rate > 0): self._thrust_slew_enabled = True else: self._thrust_slew_enabled = False def set_thrust_limits(self, min_thrust, max_thrust): """Set a new min/max thrust limit.""" self._min_thrust = JoystickReader.p2t(min_thrust) self._max_thrust = JoystickReader.p2t(max_thrust) def set_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" self._trim_roll = trim_roll def set_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" self._trim_pitch = trim_pitch def setMaxAltitude(self, maxAltitude): self._maxAltitude = maxAltitude def setCurrentAltitude(self, altitude): if altitude < self.minAltitude or self.minAltitude == 0: self.minAltitude = altitude self.currentAltitude = altitude def read_input(self): """Read input data from the selected device""" if self.pointerDevice is not None: #DT = 0.001 if self.pointerDevice.poll(): buttons = self.pointerDevice.get_buttons() if buttons & psmove.Btn_MOVE: self.pointerDevice.set_leds(0, 255, 0) self.pointerDevice.update_leds() self.SF = sensorFusion() ''' trigger_value = self.move.get_trigger() self.move.set_leds(trigger_value, 0, 0) self.move.update_leds() ''' ax, ay, az = self.pointerDevice.get_accelerometer_frame( psmove.Frame_SecondHalf) gx, gy, gz = self.pointerDevice.get_gyroscope_frame( psmove.Frame_SecondHalf) gx = gx * 180 / math.pi gy = gy * 180 / math.pi gz = gz * 180 / math.pi #print "A: %5.2f %5.2f %5.2f " % ( ax , ay , az ) #print "G: %8.2f %8.2f %8.2f " % ( gx , gy , gz ) self.SF.sensfusion6UpdateQ(gx, gy, gz, ax, ay, az, 1 / 100) roll, pitch, yaw = self.SF.sensfusion6GetEulerRPY() self.PointerYaw = -yaw ''' # Read sensor acc_x = self.pointerDevice.ax acc_y = self.pointerDevice.ay acc_z = self.pointerDevice.az gyr_x = self.pointerDevice.gx gyr_y = self.pointerDevice.gy gyr_z = self.pointerDevice.gz #// Calculate pitch and roll based only on acceleration. acc_pitch = math.atan2(acc_x, -acc_z) acc_roll = -math.atan2(acc_y, -acc_z) # Perform filtering self.kalmanPitch.kalman_innovate(acc_pitch, gyr_x, DT) self.kalmanRoll.kalman_innovate(acc_roll, gyr_y, DT) # The angle is stored in state 1 pitch = self.kalmanPitch.x1 roll = self.kalmanRoll.x1 cosRoll = math.cos(roll) sinRoll = math.sin(roll) cosPitch = math.cos(pitch) sinPitch = math.sin(pitch) magX = self.pointerDevice.mx * cosPitch + self.pointerDevice.mz * sinPitch magY = self.pointerDevice.mx * sinRoll * sinPitch + self.pointerDevice.my * cosRoll - self.pointerDevice.mz * sinRoll * cosPitch norm = math.sqrt(magX*magX + magY*magY) magHeadingX = magX/norm magHeadingY = -magY/norm #Absolute Yaw #self.PointerYaw = self.pointerDevice.mz #roll = self.pointerDevice.gy - self.pointerDevice.gy/GYROSCOPE_SENSITIVITY*dt #pitch = self.pointerDevice.gx - self.pointerDevice.gx/GYROSCOPE_SENSITIVITY*dt DT = 0.001 yaw = self.pointerDevice.gz - self.pointerDevice.gz/GYROSCOPE_SENSITIVITY*DT self.PointerYaw -= yaw*DT if self.PointerYaw >= 180: self.PointerYaw = -180 elif self.PointerYaw <= -180: self.PointerYaw = 180 if self.pointerDevice.get_buttons() & psmove.Btn_MOVE: #psmove.Btn_T: self.pointerDevice.set_leds(0, 255, 0) self.pointerDevice.update_leds() self.PointerYaw = 0 ''' if self.PointerYaw >= 0: self.pointerDevice.set_leds( int(255 * self.PointerYaw / 180), 255, 0) else: self.pointerDevice.set_leds( 0, 255, int(255 * math.fabs(self.PointerYaw) / 180)) self.pointerDevice.update_leds() self.pointer_input_updated.call(self.PointerYaw, False) try: data = self.inputdevice.read_input() roll = data["roll"] * self._max_rp_angle pitch = data["pitch"] * self._max_rp_angle thrust = data["thrust"] yaw = data["yaw"] raw_thrust = data["thrust"] emergency_stop = data["estop"] trim_roll = data["rollcal"] trim_pitch = data["pitchcal"] althold = data["althold"] flipleft = data["flipleft"] flipright = data["flipright"] viscousMode = data["viscousMode"] switchMode = data["switchmode"] if switchMode and self._canSwitch: self._canSwitch = False self.switch_mode_updated.call() elif not switchMode: self._canSwitch = True if (self._old_alt_hold != althold): self.althold_updated.call(althold) self._old_alt_hold = althold if self._emergency_stop != emergency_stop: self._emergency_stop = emergency_stop self.emergency_stop_updated.call(self._emergency_stop) if self.auto: self.auto_input_updated.call(trim_roll, trim_pitch, yaw, thrust) else: # Altitude Hold Mode Toggled if (self._old_alt_hold != althold): self.althold_updated.call(althold) self._old_alt_hold = althold # Disable hover mode if enabled if self._emergency_stop: if self._has_pressure_sensor: if self._old_alt_hold: self.althold_updated.call(False) self._old_alt_hold = False althold = False ''' modalità in cui il quad rimane fermo in altitudine e può salire o scendere in base a come si utilizza il joystick thrust up (>0) => sale (costantemente) thrust down (<0) => scende (costantemente) ''' if viscousMode: viscous_thrust = self.p2t(self.viscousModeThrust) if raw_thrust > 0 and raw_thrust <= 0.5: raw_thrust = 1 elif raw_thrust > 0.5: raw_thrust = 2 elif raw_thrust >= -0.5 and raw_thrust < 0: raw_thrust = -0.5 elif raw_thrust < -0.5: raw_thrust = -1 ''' if (self.currentAltitude - self.minAltitude) == self._maxAltitude: raw_thrust = 0 elif (self.currentAltitude - self.minAltitude) > self._maxAltitude: raw_thrust = -0.2 ''' thrust = int(round(viscous_thrust + raw_thrust * self.p2t(10))) # Thust limiting (slew, minimum and emergency stop) elif (althold and self._has_pressure_sensor) or (flipleft or flipright): thrust = int( round( JoystickReader.deadband(thrust, 0.2) * 32767 + 32767)) #Convert to uint16 else: if raw_thrust < 0.05 or emergency_stop: thrust = 0 else: thrust = self._min_thrust + thrust * (self._max_thrust - self._min_thrust) if (self._thrust_slew_enabled == True and self._thrust_slew_limit > thrust and not emergency_stop): #if (self._thrust_slew_enabled == True and not emergency_stop): if self._old_thrust > self._thrust_slew_limit: self._old_thrust = self._thrust_slew_limit if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)): thrust = self._old_thrust - self._thrust_slew_rate / 100 if raw_thrust < 0 or thrust < self._min_thrust: thrust = 0 #if trim_pitch > 0: # thrust += self.p2t(1) #if trim_pitch < 0: # thrust -= self.p2t(1) if self._emergency_landing: thrust = self._old_thrust - self.p2t(10) * 0.2 if thrust < 0: thrust = 0 self._old_thrust = thrust # Yaw deadband # TODO: Add to input device config? yaw = JoystickReader.deadband(yaw, 0.2) * self._max_yaw_rate if trim_roll != 0 or trim_pitch != 0: self._trim_roll += trim_roll self._trim_pitch += trim_pitch self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) if (flipleft or flipright) and self._flip_time_start < 0: self._flip_time_start = time.time() #ricavo il momento in cui inizia il flip if flipleft: self._old_flip_type = 0 if flipright: self._old_flip_type = 1 #if (flipleft and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 0: if flipleft and self._old_flip_type == 0: thrust = self.p2t( 70 ) #faccio in modo che il robot rimanga nella posizione corrente roll = 1600 #elif (flipright and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 1: elif flipright and self._old_flip_type == 1: #thrust = self.p2t(70) #roll = 30 #self.input_updated.call(roll, 0, yaw, thrust) #time.sleep(0.04) thrust = self.p2t(50) roll = -1000 self.input_updated.call(roll, 0, yaw, thrust) #time.sleep(FLIP_TIME) ''' ####### ## 1 ## ####### thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente roll = 30 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 2 ## ####### thrust = self.p2t(50) roll = -1600 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 3 ## ####### thrust = 0 roll = 0 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.0004) ####### ## 4 ## ####### thrust = self.p2t(50) roll = -1600 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 5 ## ####### thrust = self.p2t(70) roll = 30 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 6 ## ####### thrust = self.p2t(53) roll = 0 self.input_updated.call(roll, 0, yaw, thrust) return; ''' trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch if not flipleft and not flipright and not self.flipTimeControl( self._flip_time_start): self._old_flip_type = -1 self._flip_time_start = -float("inf") #resetto _flip_time_start self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch #yaw = yaw + self._trim_yaw self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self._read_timer.stop() def update_trim_yaw_signal(self, yaw): self._trim_yaw = yaw @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) @staticmethod def deadband(value, threshold): if abs(value) < threshold: value = 0 elif value > 0: value -= threshold elif value < 0: value += threshold return value / (1 - threshold) @staticmethod def flipTimeControl(startTime): return (time.time() - startTime >= 0 and time.time() - startTime <= FLIP_TIME)
class Drone: """Represents a CrazyFlie drone.""" def __init__(self, drone_id: str, arena: Arena, radio_id: int = 0, channel: int = 80, address: str = "E7E7E7E7E7", data_rate: str = "2M"): """ Initializes the drone with the given uri.""" # Initialize public variables self.id: str = drone_id self.var_x: float = 0 self.var_y: float = 0 self.var_z: float = 0 self.pos_x: float = 0 self.pos_y: float = 0 self.pos_z: float = 0 self.pitch: float = 0 self.roll: float = 0 self.yaw: float = 0 self.battery_voltage: float = 0 self.is_connected: bool = False self.status: DroneState = DroneState.OFFLINE self.link_uri: str = "radio://" + str(radio_id) + "/" + str( channel) + "/" + data_rate + "/" + address # Initialize limits self._max_velocity: float = 1.0 self._min_duration: float = 1.0 self._max_yaw_rotations: float = 1.0 self._arena = arena # Event to asynchronously wait for the connection self._connect_event = threading.Event() # Initialize the crazyflie self._cf = Crazyflie(rw_cache='./cache') # Initialize the callbacks self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) # Initialize events self.drone_lost = Caller() # Define the log configuration self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500) self._log_config_1.add_variable('kalman.varPX', 'float') self._log_config_1.add_variable('kalman.varPY', 'float') self._log_config_1.add_variable('kalman.varPZ', 'float') self._log_config_1.add_variable('pm.vbat', 'float') self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=100) self._log_config_2.add_variable('kalman.stateX', 'float') self._log_config_2.add_variable('kalman.stateY', 'float') self._log_config_2.add_variable('kalman.stateZ', 'float') self._log_config_3 = LogConfig(name='DroneLog_3', period_in_ms=500) self._log_config_3.add_variable('stabilizer.pitch', 'float') self._log_config_3.add_variable('stabilizer.roll', 'float') self._log_config_3.add_variable('stabilizer.yaw', 'float') def connect(self, synchronous: bool = False): """Connects to the Crazyflie.""" self._connect_crazyflie() if synchronous: self._connect_event.wait() def disconnect(self): """Disconnects from the Crazyflie and stops all logging.""" self._disconnect_crazyflie() def enable_high_level_commander(self): """Enables the drones high level commander.""" self._cf.param.set_value('commander.enHighLevel', '1') time.sleep(0.1) def disable_motion_tracking(self): """Disables to motion control (x/y) from the flow-deck.""" self._cf.param.set_value('motion.disable', '1') time.sleep(0.1) def get_status(self) -> str: """Gets various information of the drone.""" return { "id": self.id, "var_x": self.var_x, "var_y": self.var_y, "var_z": self.var_z, "x": self._arena.transform_x_inverse(self.pos_x), "y": self._arena.transform_y_inverse(self.pos_y), "z": self.pos_z, "pitch": self.pitch, "roll": self.roll, "yaw": self.yaw, "status": self.status.name, "battery_voltage": self.battery_voltage, "battery_percentage:": (self.battery_voltage - 3.4) / (4.18 - 3.4) * 100 } def reset_estimator(self) -> bool: """Resets the position estimates.""" self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2.0) # TODO: wait_for_position_estimator(cf) return True def takeoff(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) self.reset_estimator() duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.takeoff(absolute_height, duration) self.status = DroneState.STARTING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def land(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.land(absolute_height, duration) self.status = DroneState.LANDING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def go_to(self, x: float, y: float, z: float, yaw: float, velocity: float, relative: bool = False, synchronous: bool = False) -> float: x = self._sanitize_x(x, relative) y = self._sanitize_y(y, relative) z = self._sanitize_z(z, relative) yaw = self._sanitize_yaw(yaw) distance = self._calculate_distance(x, y, z, relative) duration = self._convert_velocity_to_time(distance, velocity) self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative) self.status = DroneState.NAVIGATING if synchronous: time.sleep(duration) return { "duration": duration, "target_x": self._arena.transform_x_inverse(x), "target_y": self._arena.transform_y_inverse(y), "target_z": z, "target_yaw": yaw, "relative": relative } def stop(self): self._cf.high_level_commander.stop() self.status = DroneState.IDLE def _connect_crazyflie(self): print('Connecting to %s' % self.link_uri) self._cf.open_link(self.link_uri) def _disconnect_crazyflie(self): print('Disconnecting from %s' % self.link_uri) # Stop the loggers self._log_config_1.stop() self._log_config_2.stop() self._log_config_3.stop() # Shutdown the rotors self._shutdown() # Disconnect self._cf.close_link() def _connected(self, link_uri): """This callback is called when the Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) # Setup parameters self.disable_motion_tracking() # Add the logger self._cf.log.add_config(self._log_config_1) self._cf.log.add_config(self._log_config_2) self._cf.log.add_config(self._log_config_3) # This callback will receive the data self._log_config_1.data_received_cb.add_callback( self._log_config_1_data) self._log_config_2.data_received_cb.add_callback( self._log_config_2_data) self._log_config_3.data_received_cb.add_callback( self._log_config_3_data) # This callback will be called on errors self._log_config_1.error_cb.add_callback(self._log_config_error) self._log_config_2.error_cb.add_callback(self._log_config_error) self._log_config_3.error_cb.add_callback(self._log_config_error) # Start the logging self._log_config_1.start() self._log_config_2.start() self._log_config_3.start() # Set the connected event self._connect_event.set() self.is_connected = True self.status = DroneState.IDLE def _connection_failed(self, link_uri, msg): """Callback when the initial connection fails.""" print('Connection to %s failed: %s' % (link_uri, msg)) # Set the connected event self._connect_event.set() def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected.""" print('Disconnected from %s' % link_uri) self.is_connected = False self.status = DroneState.OFFLINE def _connection_lost(self, link_uri, msg): """Callback when the connection is lost after a connection has been made.""" print('Connection to %s lost: %s' % (link_uri, msg)) self.drone_lost.call(self) self._connect_event.set() self.is_connected = False self.status = DroneState.OFFLINE def _log_config_error(self, logconf, msg): """Callback from the log API when an error occurs.""" print('Error when logging %s: %s' % (logconf.name, msg)) def _log_config_1_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.var_x = data['kalman.varPX'] self.var_y = data['kalman.varPY'] self.var_z = data['kalman.varPZ'] self.battery_voltage = data['pm.vbat'] def _log_config_2_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.pos_x = data['kalman.stateX'] self.pos_y = data['kalman.stateY'] self.pos_z = data['kalman.stateZ'] def _log_config_3_data(self, timestamp, data, logconf): self.pitch = data['stabilizer.pitch'] self.roll = data['stabilizer.roll'] self.yaw = data['stabilizer.yaw'] def _unlock(self): # Unlock startup thrust protection (only needed for low lewel commands) self._cf.commander.send_setpoint(0, 0, 0, 0) def _shutdown(self): self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime): """Keeps the drone at the given setpoint for the given amount of time.""" while keeptime > 0: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) keeptime -= 0.1 time.sleep(0.1) def _convert_velocity_to_time(self, distance: float, velocity: float) -> float: """Converts a distance and a velocity to a time.""" duration = distance / self._sanitize_velocity(velocity) return self._sanitize_duration(duration) def _calculate_distance(self, x: float, y: float, z: float, relative: bool = False) -> float: """Calculates the distance from the drone or the zero position (relative) to a given point in space.""" start_x = 0 if relative else self.pos_x start_y = 0 if relative else self.pos_y start_z = 0 if relative else self.pos_z return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2) def _sanitize_velocity(self, velocity: float) -> float: return min(velocity, self._max_velocity) def _sanitize_duration(self, duration: float) -> float: return max(duration, self._min_duration) def _sanitize_yaw(self, yaw: float) -> float: return yaw % (2 * self._max_yaw_rotations * math.pi) def _sanitize_x(self, x: float, relative: bool) -> float: target_x = (self.pos_x + x) if relative else x sanitized_x = self._sanitize_number(target_x, self._arena.min_x, self._arena.max_x) return self._arena.transform_x(sanitized_x) def _sanitize_y(self, y: float, relative: bool) -> float: target_y = (self.pos_y + y) if relative else y sanitized_y = self._sanitize_number(target_y, self._arena.min_y, self._arena.max_y) return self._arena.transform_y(sanitized_y) def _sanitize_z(self, z: float, relative: bool) -> float: return self._sanitize_number(z, self._arena.min_z, self._arena.max_z) def _sanitize_number(self, value: float, min_value: float, max_value: float) -> float: return min(max(value, min_value), max_value)
class JoystickReader(object): """ Thread that will read input from devices/joysticks and send control-set points to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): self._input_device = None self.min_thrust = 0 self.max_thrust = 0 self._thrust_slew_rate = 0 self.thrust_slew_enabled = False self.thrust_slew_limit = 0 self.has_pressure_sensor = False self.max_rp_angle = 0 self.max_yaw_rate = 0 self._old_thrust = 0 self._old_raw_thrust = 0 self._old_alt_hold = False self.springy_throttle = True self.trim_roll = Config().get("trim_roll") self.trim_pitch = Config().get("trim_pitch") self._input_map = None self._mux = [NoMux(self), TakeOverSelectiveMux(self), TakeOverMux(self)] # Set NoMux as default self._selected_mux = self._mux[0] if Config().get("flightmode") is "Normal": self.max_yaw_rate = Config().get("normal_max_yaw") self.max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("normal_min_thrust") self.max_thrust = Config().get("normal_max_thrust") self.thrust_slew_limit = Config().get("normal_slew_limit") self.thrust_slew_rate = Config().get("normal_slew_rate") else: self.max_yaw_rate = Config().get("max_yaw") self.max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("min_thrust") self.max_thrust = Config().get("max_thrust") self.thrust_slew_limit = Config().get("slew_limit") self.thrust_slew_rate = Config().get("slew_rate") self._dev_blacklist = None if len(Config().get("input_device_blacklist")) > 0: self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob( cfclient.module_path + "/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager(). configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() self.alt1_updated = Caller() self.alt2_updated = Caller() # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting) self.limiting_updated = Caller() def _get_device_from_name(self, device_name): """Get the raw device from a name""" for d in readers.devices(): if d.name == device_name: return d return None def set_alt_hold_available(self, available): """Set if altitude hold is available or not (depending on HW)""" self.has_pressure_sensor = available def enable_alt_hold(self, althold): """Enable or disable altitude hold""" self._old_alt_hold = althold def _do_device_discovery(self): devs = self.available_devices() # This is done so that devs can easily get access # to limits without creating lots of extra code for d in devs: d.input = self if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def available_mux(self): return self._mux def set_mux(self, name=None, mux=None): old_mux = self._selected_mux if name: for m in self._mux: if m.name == name: self._selected_mux = m elif mux: self._selected_mux = mux old_mux.close() logger.info("Selected MUX: {}".format(self._selected_mux.name)) def available_devices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = readers.devices() devs += interfaces.devices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev.name))): dev.input = self approved_devs.append(dev) return approved_devs def enableRawReading(self, device_name): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ if self._input_device: self._input_device.close() self._input_device = None for d in readers.devices(): if d.name == device_name: self._input_device = d # Set the mapping to None to get raw values self._input_device.input_map = None self._input_device.open() def get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in list(device_config_mapping.keys()): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def stop_raw_reading(self): """Disable raw reading of input device.""" if self._input_device: self._input_device.close() self._input_device = None def read_raw_values(self): """ Read raw values from the input device.""" [axes, buttons, mapped_values] = self._input_device.read( include_raw=True) dict_axes = {} dict_buttons = {} for i, a in enumerate(axes): dict_axes[i] = a for i, b in enumerate(buttons): dict_buttons[i] = b return [dict_axes, dict_buttons, mapped_values] def set_raw_input_map(self, input_map): """Set an input device map""" # TODO: Will not work! if self._input_device: self._input_device.input_map = input_map def set_input_map(self, device_name, input_map_name): """Load and set an input device map with the given name""" settings = ConfigManager().get_settings(input_map_name) if settings: self.springy_throttle = settings["springythrottle"] self._input_map = ConfigManager().get_config(input_map_name) self._get_device_from_name(device_name).input_map = self._input_map self._get_device_from_name(device_name).input_map_name = input_map_name Config().get("device_config_mapping")[device_name] = input_map_name def start_input(self, device_name, role="Device", config_name=None): """ Start reading input from the device with name device_name using config config_name. Returns True if device supports mapping, otherwise False """ try: # device_id = self._available_devices[device_name] # Check if we supplied a new map, if not use the preferred one device = self._get_device_from_name(device_name) self._selected_mux.add_device(device, role) # Update the UI with the limiting for this device self.limiting_updated.call(device.limit_rp, device.limit_yaw, device.limit_thrust) self._read_timer.start() return device.supports_mapping except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) if not self._input_device: self.device_error.call( "Could not find device {}".format(device_name)) return False def resume_input(self): self._selected_mux.resume() self._read_timer.start() def pause_input(self, device_name=None): """Stop reading from the input device.""" self._read_timer.stop() self._selected_mux.pause() def _set_thrust_slew_rate(self, rate): self._thrust_slew_rate = rate if rate > 0: self.thrust_slew_enabled = True else: self.thrust_slew_enabled = False def _get_thrust_slew_rate(self): return self._thrust_slew_rate def read_input(self): """Read input data from the selected device""" try: data = self._selected_mux.read() if data: if data.toggled.althold: try: self.althold_updated.call(str(data.althold)) except Exception as e: logger.warning( "Exception while doing callback from input-device " "for althold: {}".format(e)) if data.toggled.estop: try: self.emergency_stop_updated.call(data.estop) except Exception as e: logger.warning("Exception while doing callback from" "input-device for estop: {}".format(e)) if data.toggled.alt1: try: self.alt1_updated.call(data.alt1) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt1: {}".format(e)) if data.toggled.alt2: try: self.alt2_updated.call(data.alt2) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt2: {}".format(e)) # Update the user roll/pitch trim from device if data.toggled.pitchNeg and data.pitchNeg: self.trim_pitch -= 1 if data.toggled.pitchPos and data.pitchPos: self.trim_pitch += 1 if data.toggled.rollNeg and data.rollNeg: self.trim_roll -= 1 if data.toggled.rollPos and data.rollPos: self.trim_roll += 1 if data.toggled.pitchNeg or data.toggled.pitchPos or \ data.toggled.rollNeg or data.toggled.rollPos: self.rp_trim_updated.call(self.trim_roll, self.trim_pitch) # Thrust might be <0 here, make sure it's not otherwise we'll # get an error. if data.thrust < 0: data.thrust = 0 if data.thrust > 0xFFFF: data.thrust = 0xFFFF # If we are using alt hold the data is not in a percentage if not data.althold: data.thrust = JoystickReader.p2t(data.thrust) self.input_updated.call(data.roll + self.trim_roll, data.pitch + self.trim_pitch, data.yaw, data.thrust) else: self.input_updated.call(0, 0, 0, 0) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self.input_updated.call(0, 0, 0, 0) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class LogConfig(object): """Representation of one log configuration that enables logging from the Crazyflie""" _config_id_counter = 1 def __init__(self, name, period_in_ms): """Initialize the entry""" self.data_received_cb = Caller() self.error_cb = Caller() self.started_cb = Caller() self.added_cb = Caller() self.err_no = 0 self.id = LogConfig._config_id_counter LogConfig._config_id_counter = (LogConfig._config_id_counter + 1) % 255 self.cf = None self.period = int(period_in_ms / 10) self.period_in_ms = period_in_ms self._added = False self._started = False self.valid = False self.variables = [] self.default_fetch_as = [] self.name = name def add_variable(self, name, fetch_as=None): """Add a new variable to the configuration. name - Complete name of the variable in the form group.name fetch_as - String representation of the type the variable should be fetched as (i.e uint8_t, float, FP16, etc) If no fetch_as type is supplied, then the stored as type will be used (i.e the type of the fetched variable is the same as it's stored in the Crazyflie).""" if fetch_as: self.variables.append(LogVariable(name, fetch_as)) else: # We cannot determine the default type until we have connected. So # save the name and we will add these once we are connected. self.default_fetch_as.append(name) def add_memory(self, name, fetch_as, stored_as, address): """Add a raw memory position to log. name - Arbitrary name of the variable fetch_as - String representation of the type of the data the memory should be fetch as (i.e uint8_t, float, FP16) stored_as - String representation of the type the data is stored as in the Crazyflie address - The address of the data """ self.variables.append( LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as, address)) def _set_added(self, added): if added != self._added: self.added_cb.call(self, added) self._added = added def _get_added(self): return self._added def _set_started(self, started): if started != self._started: self.started_cb.call(self, started) self._started = started def _get_started(self): return self._started added = property(_get_added, _set_added) started = property(_get_started, _set_started) def create(self): """Save the log configuration in the Crazyflie""" pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_CREATE_BLOCK, self.id) for var in self.variables: if (var.is_toc_variable() is False): # Memory location logger.debug('Logging to raw memory %d, 0x%04X', var.get_storage_and_fetch_byte(), var.address) pk.data.append( struct.pack('<B', var.get_storage_and_fetch_byte())) pk.data.append(struct.pack('<I', var.address)) else: # Item in TOC logger.debug('Adding %s with id=%d and type=0x%02X', var.name, self.cf.log.toc.get_element_id(var.name), var.get_storage_and_fetch_byte()) pk.data.append(var.get_storage_and_fetch_byte()) pk.data.append(self.cf.log.toc.get_element_id(var.name)) logger.debug('Adding log block id {}'.format(self.id)) self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id)) def start(self): """Start the logging for this entry""" if (self.cf.link is not None): if (self._added is False): self.create() logger.debug('First time block is started, add block') else: logger.debug( 'Block already registered, starting logging' ' for id=%d', self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.id, self.period) self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id)) def stop(self): """Stop the logging for this entry""" if (self.cf.link is not None): if (self.id is None): logger.warning('Stopping block, but no block registered') else: logger.debug('Sending stop logging for block id=%d', self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.id) self.cf.send_packet(pk, expected_reply=(CMD_STOP_LOGGING, self.id)) def delete(self): """Delete this entry in the Crazyflie""" if (self.cf.link is not None): if (self.id is None): logger.warning('Delete block, but no block registered') else: logger.debug( 'LogEntry: Sending delete logging for block id=%d' % self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.id) self.cf.send_packet(pk, expected_reply=(CMD_DELETE_BLOCK, self.id)) def unpack_log_data(self, log_data, timestamp): """Unpack received logging data so it represent real values according to the configuration in the entry""" ret_data = {} data_index = 0 for var in self.variables: size = LogTocElement.get_size_from_id(var.fetch_as) name = var.name unpackstring = LogTocElement.get_unpack_string_from_id( var.fetch_as) value = struct.unpack(unpackstring, log_data[data_index:data_index + size])[0] data_index += size ret_data[name] = value self.data_received_cb.call(timestamp, ret_data, self)
class LogEntry: blockIdCounter = 1 def __init__(self, crazyflie, logconf): self.dataReceived = Caller() self.error = Caller() self.logconf = logconf self.blockId = LogEntry.blockIdCounter LogEntry.blockIdCounter += 1 self.cf = crazyflie self.period = logconf.getPeriod() / 10 self.blockCreated = False def start(self): if (self.cf.link is not None): if (self.blockCreated is False): logger.debug("First time block is started, add block") self.blockCreated = True pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_CREATE_BLOCK, self.blockId) for v in self.logconf.getVariables(): if (v.isTocVariable() is False): # Memory location logger.debug("Logging to raw memory %d, 0x%04X", v.getStoredFetchAs(), v.getAddress()) pk.data += struct.pack('<B', v.getStoredFetchAs()) pk.data += struct.pack('<I', v.getAddress()) else: # Item in TOC logger.debug("Adding %s with id=%d and type=0x%02X", v.getName(), self.cf.log.toc.get_element_id( v.getName()), v.getStoredFetchAs()) pk.data += struct.pack('<B', v.getStoredFetchAs()) pk.data += struct.pack('<B', self.cf.log.toc. get_element_id(v.getName())) logger.debug("Adding log block id {}".format(self.blockId)) self.cf.send_packet(pk) else: logger.debug("Block already registered, starting logging" " for %d", self.blockId) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.blockId, self.period) self.cf.send_packet(pk) def stop(self): if (self.cf.link is not None): if (self.blockId is None): logger.warning("Stopping block, but no block registered") else: logger.debug("Sending stop logging for block %d", self.blockId) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.blockId) self.cf.send_packet(pk) def close(self): if (self.cf.link is not None): if (self.blockId is None): logger.warning("Delete block, but no block registered") else: logger.debug("LogEntry: Sending delete logging for block %d" % self.blockId) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.blockId) self.cf.send_packet(pk) self.blockId = None # Wait until we get confirmation of delete def unpack_log_data(self, logData): retData = {} dataIndex = 0 for v in self.logconf.getVariables(): size = LogTocElement.get_size_from_id(v.getFetchAs()) name = v.getName() unpackstring = LogTocElement.get_unpack_string_from_id( v.getFetchAs()) value = struct.unpack(unpackstring, logData[dataIndex:dataIndex + size])[0] dataIndex += size retData[name] = value self.dataReceived.call(retData)
class JoystickReader(object): """ Thread that will read input from devices/joysticks and send control-set points to the Crazyflie """ inputConfig = [] ASSISTED_CONTROL_ALTHOLD = 0 ASSISTED_CONTROL_POSHOLD = 1 ASSISTED_CONTROL_HEIGHTHOLD = 2 ASSISTED_CONTROL_HOVER = 3 def __init__(self, do_device_discovery=True): self._input_device = None self._mux = [NoMux(self), TakeOverSelectiveMux(self), TakeOverMux(self)] # Set NoMux as default self._selected_mux = self._mux[0] self.min_thrust = 0 self.max_thrust = 0 self._thrust_slew_rate = 0 self.thrust_slew_enabled = False self.thrust_slew_limit = 0 self.has_pressure_sensor = False self._hover_max_height = MAX_TARGET_HEIGHT self.max_rp_angle = 0 self.max_yaw_rate = 0 try: self.set_assisted_control(Config().get("assistedControl")) except KeyError: self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD) self._old_thrust = 0 self._old_raw_thrust = 0 self.springy_throttle = True self._target_height = INITAL_TAGET_HEIGHT self.trim_roll = Config().get("trim_roll") self.trim_pitch = Config().get("trim_pitch") self._rp_dead_band = 0.1 self._input_map = None if Config().get("flightmode") is "Normal": self.max_yaw_rate = Config().get("normal_max_yaw") self.max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("normal_min_thrust") self.max_thrust = Config().get("normal_max_thrust") self.thrust_slew_limit = Config().get("normal_slew_limit") self.thrust_slew_rate = Config().get("normal_slew_rate") else: self.max_yaw_rate = Config().get("max_yaw") self.max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("min_thrust") self.max_thrust = Config().get("max_thrust") self.thrust_slew_limit = Config().get("slew_limit") self.thrust_slew_rate = Config().get("slew_rate") self._dev_blacklist = None if len(Config().get("input_device_blacklist")) > 0: self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob( cfclient.module_path + "/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager(). configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.assisted_input_updated = Caller() self.heighthold_input_updated = Caller() self.hover_input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.assisted_control_updated = Caller() self.alt1_updated = Caller() self.alt2_updated = Caller() # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting) self.limiting_updated = Caller() def _get_device_from_name(self, device_name): """Get the raw device from a name""" for d in readers.devices(): if d.name == device_name: return d return None def set_hover_max_height(self, height): self._hover_max_height = height def set_alt_hold_available(self, available): """Set if altitude hold is available or not (depending on HW)""" self.has_pressure_sensor = available def _do_device_discovery(self): devs = self.available_devices() # This is done so that devs can easily get access # to limits without creating lots of extra code for d in devs: d.input = self if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def available_mux(self): return self._mux def set_mux(self, name=None, mux=None): old_mux = self._selected_mux if name: for m in self._mux: if m.name == name: self._selected_mux = m elif mux: self._selected_mux = mux old_mux.close() logger.info("Selected MUX: {}".format(self._selected_mux.name)) def set_assisted_control(self, mode): self._assisted_control = mode def get_assisted_control(self): return self._assisted_control def available_devices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = readers.devices() devs += interfaces.devices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev.name))): dev.input = self approved_devs.append(dev) return approved_devs def enableRawReading(self, device_name): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ if self._input_device: self._input_device.close() self._input_device = None for d in readers.devices(): if d.name == device_name: self._input_device = d # Set the mapping to None to get raw values self._input_device.input_map = None self._input_device.open() def get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in list(device_config_mapping.keys()): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def stop_raw_reading(self): """Disable raw reading of input device.""" if self._input_device: self._input_device.close() self._input_device = None def read_raw_values(self): """ Read raw values from the input device.""" [axes, buttons, mapped_values] = self._input_device.read( include_raw=True) dict_axes = {} dict_buttons = {} for i, a in enumerate(axes): dict_axes[i] = a for i, b in enumerate(buttons): dict_buttons[i] = b return [dict_axes, dict_buttons, mapped_values] def set_raw_input_map(self, input_map): """Set an input device map""" # TODO: Will not work! if self._input_device: self._input_device.input_map = input_map def set_input_map(self, device_name, input_map_name): """Load and set an input device map with the given name""" dev = self._get_device_from_name(device_name) settings = ConfigManager().get_settings(input_map_name) if settings: self.springy_throttle = settings["springythrottle"] self._rp_dead_band = settings["rp_dead_band"] self._input_map = ConfigManager().get_config(input_map_name) dev.input_map = self._input_map dev.input_map_name = input_map_name Config().get("device_config_mapping")[device_name] = input_map_name dev.set_dead_band(self._rp_dead_band) def start_input(self, device_name, role="Device", config_name=None): """ Start reading input from the device with name device_name using config config_name. Returns True if device supports mapping, otherwise False """ try: # device_id = self._available_devices[device_name] # Check if we supplied a new map, if not use the preferred one device = self._get_device_from_name(device_name) self._selected_mux.add_device(device, role) # Update the UI with the limiting for this device self.limiting_updated.call(device.limit_rp, device.limit_yaw, device.limit_thrust) self._read_timer.start() return device.supports_mapping except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) if not self._input_device: self.device_error.call( "Could not find device {}".format(device_name)) return False def resume_input(self): self._selected_mux.resume() self._read_timer.start() def pause_input(self, device_name=None): """Stop reading from the input device.""" self._read_timer.stop() self._selected_mux.pause() def _set_thrust_slew_rate(self, rate): self._thrust_slew_rate = rate if rate > 0: self.thrust_slew_enabled = True else: self.thrust_slew_enabled = False def _get_thrust_slew_rate(self): return self._thrust_slew_rate def read_input(self): """Read input data from the selected device""" try: data = self._selected_mux.read() if data: if data.toggled.assistedControl: if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_POSHOLD or \ self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HOVER: if data.assistedControl and self._assisted_control != \ JoystickReader.ASSISTED_CONTROL_HOVER: for d in self._selected_mux.devices(): d.limit_thrust = False d.limit_rp = False elif data.assistedControl: for d in self._selected_mux.devices(): d.limit_thrust = True d.limit_rp = False else: for d in self._selected_mux.devices(): d.limit_thrust = True d.limit_rp = True if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_ALTHOLD: self.assisted_control_updated.call( data.assistedControl) if ((self._assisted_control == JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD) or (self._assisted_control == JoystickReader.ASSISTED_CONTROL_HOVER)): try: self.assisted_control_updated.call( data.assistedControl) if not data.assistedControl: # Reset height controller state to initial # target height both in the UI and in the # Crazyflie. # TODO: Implement a proper state update of the # input layer self.heighthold_input_updated.\ call(0, 0, 0, INITAL_TAGET_HEIGHT) self.hover_input_updated.\ call(0, 0, 0, INITAL_TAGET_HEIGHT) except Exception as e: logger.warning( "Exception while doing callback from " "input-device for assited " "control: {}".format(e)) if data.toggled.estop: try: self.emergency_stop_updated.call(data.estop) except Exception as e: logger.warning("Exception while doing callback from" "input-device for estop: {}".format(e)) if data.toggled.alt1: try: self.alt1_updated.call(data.alt1) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt1: {}".format(e)) if data.toggled.alt2: try: self.alt2_updated.call(data.alt2) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt2: {}".format(e)) # Reset height target when height-hold is not selected if not data.assistedControl or \ (self._assisted_control != JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD and self._assisted_control != JoystickReader.ASSISTED_CONTROL_HOVER): self._target_height = INITAL_TAGET_HEIGHT if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_POSHOLD \ and data.assistedControl: vx = data.roll vy = data.pitch vz = data.thrust yawrate = data.yaw # The odd use of vx and vy is to map forward on the # physical joystick to positiv X-axis self.assisted_input_updated.call(vy, -vx, vz, yawrate) elif self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HOVER \ and data.assistedControl: vx = data.roll vy = data.pitch # Scale thrust to a value between -1.0 to 1.0 vz = (data.thrust - 32767) / 32767.0 # Integrate velosity setpoint self._target_height += vz * INPUT_READ_PERIOD # Cap target height if self._target_height > self._hover_max_height: self._target_height = self._hover_max_height if self._target_height < MIN_HOVER_HEIGHT: self._target_height = MIN_HOVER_HEIGHT yawrate = data.yaw # The odd use of vx and vy is to map forward on the # physical joystick to positiv X-axis self.hover_input_updated.call(vy, -vx, yawrate, self._target_height) else: # Update the user roll/pitch trim from device if data.toggled.pitchNeg and data.pitchNeg: self.trim_pitch -= .2 if data.toggled.pitchPos and data.pitchPos: self.trim_pitch += .2 if data.toggled.rollNeg and data.rollNeg: self.trim_roll -= .2 if data.toggled.rollPos and data.rollPos: self.trim_roll += .2 if data.toggled.pitchNeg or data.toggled.pitchPos or \ data.toggled.rollNeg or data.toggled.rollPos: self.rp_trim_updated.call(self.trim_roll, self.trim_pitch) if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD \ and data.assistedControl: roll = data.roll + self.trim_roll pitch = data.pitch + self.trim_pitch yawrate = data.yaw # Scale thrust to a value between -1.0 to 1.0 vz = (data.thrust - 32767) / 32767.0 # Integrate velosity setpoint self._target_height += vz * INPUT_READ_PERIOD # Cap target height if self._target_height > self._hover_max_height: self._target_height = self._hover_max_height if self._target_height < MIN_TARGET_HEIGHT: self._target_height = MIN_TARGET_HEIGHT self.heighthold_input_updated.call(roll, -pitch, yawrate, self._target_height) else: # Using alt hold the data is not in a percentage if not data.assistedControl: data.thrust = JoystickReader.p2t(data.thrust) # Thrust might be <0 here, make sure it's not otherwise # we'll get an error. if data.thrust < 0: data.thrust = 0 if data.thrust > 0xFFFF: data.thrust = 0xFFFF self.input_updated.call(data.roll + self.trim_roll, data.pitch + self.trim_pitch, data.yaw, data.thrust) else: self.input_updated.call(0, 0, 0, 0) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self.input_updated.call(0, 0, 0, 0) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class Crazyflie(): """The Crazyflie class""" def __init__(self, link=None): """ Create the objects from this module and register callbacks. """ # Called on disconnect, no matter the reason self.disconnected = Caller() # Called on unintentional disconnect only self.connection_lost = Caller() # Called when the first packet in a new link is received self.link_established = Caller() # Called when the user requests a connection self.connection_requested = Caller() # Called when the link is established self.connected = Caller() # Called if establishing of the link fails (i.e times out) self.connection_failed = Caller() # Called for every packet received self.packet_received = Caller() # Called for every packet sent self.packet_sent = Caller() # Called when the link driver updates the link quality measurement self.link_quality_updated = Caller() self.state = State.DISCONNECTED self.link = link # Thread handling incoming packets self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) self.incoming.start() # Thread handling outgoing packets self.outgoing = _OutgoingPacketHandler(self) self.outgoing.setDaemon(True) self.outgoing.start() self.commander = Commander(self) self.link_uri = '' # Used for retry when no reply was sent back self.packet_received.add_callback(self._check_for_initial_packet_cb) self._send_lock = Lock() self.connected_ts = None # Connect callbacks to logger self.disconnected.add_callback( lambda uri: logger.info('Callback->Disconnected from [%s]', uri)) self.disconnected.add_callback(self._disconnected) self.link_established.add_callback( lambda uri: logger.info('Callback->Connected to [%s]', uri)) self.connection_lost.add_callback(lambda uri, errmsg: logger.info( 'Callback->Connection lost to [%s]: %s', uri, errmsg)) self.connection_failed.add_callback(lambda uri, errmsg: logger.info( 'Callback->Connected failed to [%s]: %s', uri, errmsg)) self.connection_requested.add_callback(lambda uri: logger.info( 'Callback->Connection initialized[%s]', uri)) self.connected.add_callback(lambda uri: logger.info( 'Callback->Connection setup finished [%s]', uri)) def _disconnected(self, link_uri): """ Callback when disconnected.""" self.connected_ts = None def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver(link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) logger.info('Connection setup finished') self.connected_ts = datetime.datetime.now() self.connected.call(self.link_uri) except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self.disconnected.call(self.link_uri) def is_connected(self): """Check if the communication link is open or not.""" return self.connected_ts is not None def send_packet(self, pk): """ Send a packet through the link interface. pk -- Packet to send """ self._send_lock.acquire() if self.link is not None: self.link.send_packet(pk) # self.packet_sent.call(pk) self._send_lock.release()
class Memory(): """Access memories on the Crazyflie""" # These codes can be decoded using os.stderror, but # some of the text messages will look very strange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: 'No more memory available', errno.ENOEXEC: 'Command not found', errno.ENOENT: 'No such block id', errno.E2BIG: 'Block too large', errno.EEXIST: 'Block already exists' } def __init__(self, crazyflie=None): """Instantiate class and connect callbacks""" self.mems = [] # Called when new memories have been added self.mem_added_cb = Caller() # Called when new data has been read self.mem_read_cb = Caller() self.mem_write_cb = Caller() self.cf = crazyflie self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self.cf.disconnected.add_callback(self._disconnected) self._refresh_callback = None self._fetch_id = 0 self.nbr_of_mems = 0 self._ow_mem_fetch_index = 0 self._elem_data = () self._read_requests = {} self._read_requests_lock = Lock() self._write_requests = {} self._write_requests_lock = Lock() self._ow_mems_left_to_update = [] self._getting_count = False def _mem_update_done(self, mem): """ Callback from each individual memory (only 1-wire) when reading of header/elements are done """ if mem.id in self._ow_mems_left_to_update: self._ow_mems_left_to_update.remove(mem.id) logger.info(mem) if len(self._ow_mems_left_to_update) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None def get_mem(self, id): """Fetch the memory with the supplied id""" for m in self.mems: if m.id == id: return m return None def get_mems(self, type): """Fetch all the memories of the supplied type""" ret = () for m in self.mems: if m.type == type: ret += (m,) return ret def ow_search(self, vid=0xBC, pid=None, name=None): """Search for specific memory id/name and return it""" for m in self.get_mems(MemoryElement.TYPE_1W): if pid and m.pid == pid or name and m.name == name: return m return None def write(self, memory, addr, data, flush_queue=False): """Write the specified data to the given memory at the given address""" wreq = _WriteRequest(memory, addr, data, self.cf) if memory.id not in self._write_requests: self._write_requests[memory.id] = [] # Workaround until we secure the uplink and change messages for # mems to non-blocking self._write_requests_lock.acquire() if flush_queue: self._write_requests[memory.id] = self._write_requests[ memory.id][:1] self._write_requests[memory.id].insert(len(self._write_requests), wreq) if len(self._write_requests[memory.id]) == 1: wreq.start() self._write_requests_lock.release() return True def read(self, memory, addr, length): """ Read the specified amount of bytes from the given memory at the given address """ if memory.id in self._read_requests: logger.warning('There is already a read operation ongoing for ' 'memory id {}'.format(memory.id)) return False rreq = _ReadRequest(memory, addr, length, self.cf) self._read_requests[memory.id] = rreq rreq.start() return True def refresh(self, refresh_done_callback): """Start fetching all the detected memories""" self._refresh_callback = refresh_done_callback self._fetch_id = 0 for m in self.mems: try: self.mem_read_cb.remove_callback(m.new_data) m.disconnect() except Exception as e: logger.info( 'Error when removing memory after update: {}'.format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info('Requesting number of memories') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR,) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _disconnected(self, uri): """The link to the Crazyflie has been broken. Reset state""" for m in self.mems: try: m.disconnect() except Exception as e: logger.info( 'Error when resetting after disconnect: {}'.format(e)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = payload[0] logger.info('{} memories found'.format(self.nbr_of_mems)) # Start requesting information about the memories, # if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info('Requesting first id') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=( CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory # (the I2C). logger.error( '-------->Got good count, but no info on mem!') self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = payload[0] # Type - 1 byte mem_type = payload[1] # Size 4 bytes (as addr) mem_size = struct.unpack('I', payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) mem_addr = '' for m in mem_addr_raw: mem_addr += '{:02X}'.format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_DRIVER_LED: mem = LEDDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_LOCO: mem = LocoMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info( 'Requesting information about memory {}'.format( self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=( CMD_INFO_DETAILS, self._fetch_id)) else: logger.info( 'Done getting all the memories, start reading the OWs') ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise # we are done for ow_mem in ows: ow_mem.update(self._mem_update_done) if len(ows) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) logger.info( 'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format( id, addr, status)) # Find the read request if id in self._write_requests: self._write_requests_lock.acquire() wreq = self._write_requests[id][0] if status == 0: if wreq.write_done(addr): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) self.mem_write_cb.call(wreq.mem, wreq.addr) # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: self._write_requests[id][0].start() else: logger.info('Status {}: write resending...'.format(status)) wreq.resend() self._write_requests_lock.release() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) data = struct.unpack('B' * len(payload[5:]), payload[5:]) logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, ' 'data={}'.format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info( 'READING: We are still interested in request for ' 'mem {}'.format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: logger.info('Status {}: resending...'.format(status)) rreq.resend()
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True): # TODO: Should be OS dependant self.SF = sensorFusion() self.inputdevice = PyGameReader() self.pointerDevice = psmove.PSMove() self.PointerYaw = 0 self.kalmanPitch = KalmanFilter() self.kalmanRoll = KalmanFilter() self.viscousModeThrust = 67 self._emergency_landing = False self.auto = False self._min_thrust = 0 self._max_thrust = 0 self._maxAltitude = 0 self.currentAltitude = 0 self.minAltitude = 0 self._thrust_slew_rate = 0 self._thrust_slew_enabled = False self._thrust_slew_limit = 0 self._emergency_stop = False self._has_pressure_sensor = False self._canSwitch = True self._old_thrust = 0 self._old_alt_hold = False self._old_flip_type = -1 self._flip_time_start = -float("inf"); self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") self._trim_yaw = 0.0 if (Config().get("flightmode") is "Normal"): self._max_yaw_rate = Config().get("normal_max_yaw") self._max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits( Config().get("normal_min_thrust"), Config().get("normal_max_thrust")) self.set_thrust_slew_limiting( Config().get("normal_slew_rate"), Config().get("normal_slew_limit")) else: self._max_yaw_rate = Config().get("max_yaw") self._max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits( Config().get("min_thrust"), Config().get("max_thrust")) self.set_thrust_slew_limiting( Config().get("slew_rate"), Config().get("slew_limit")) self._dev_blacklist = None if (len(Config().get("input_device_blacklist")) > 0): self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager(). configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.switch_mode_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() self.auto_input_updated = Caller() self.pointer_input_updated = Caller() def setViscousModeThrust(self, thrust): if thrust >= 0: self.viscousModeThrust = thrust def setEmergencyLanding(self, emergencyLanding): self._emergency_landing = emergencyLanding def setAltHoldAvailable(self, available): self._has_pressure_sensor = available def setAuto(self, auto): self.auto = auto def setAltHold(self, althold): self._old_alt_hold = althold def _do_device_discovery(self): devs = self.getAvailableDevices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def getAvailableDevices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = self.inputdevice.getAvailableDevices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev["name"]))): self._available_devices[dev["name"]] = dev["id"] approved_devs.append(dev) return approved_devs def enableRawReading(self, deviceId): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ self.inputdevice.enableRawReading(deviceId) def disableRawReading(self): """Disable raw reading of input device.""" self.inputdevice.disableRawReading() def readRawValues(self): """ Read raw values from the input device.""" return self.inputdevice.readRawValues() def start_input(self, device_name, config_name): """ Start reading input from the device with name device_name using config config_name """ try: device_id = self._available_devices[device_name] self.inputdevice.start_input( device_id, ConfigManager().get_config(config_name)) self._read_timer.start() except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) def stop_input(self): """Stop reading from the input device.""" self._read_timer.stop() def set_yaw_limit(self, max_yaw_rate): """Set a new max yaw rate value.""" self._max_yaw_rate = max_yaw_rate def set_rp_limit(self, max_rp_angle): """Set a new max roll/pitch value.""" self._max_rp_angle = max_rp_angle def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate) self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit) if (thrust_slew_rate > 0): self._thrust_slew_enabled = True else: self._thrust_slew_enabled = False def set_thrust_limits(self, min_thrust, max_thrust): """Set a new min/max thrust limit.""" self._min_thrust = JoystickReader.p2t(min_thrust) self._max_thrust = JoystickReader.p2t(max_thrust) def set_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" self._trim_roll = trim_roll def set_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" self._trim_pitch = trim_pitch def setMaxAltitude(self, maxAltitude): self._maxAltitude = maxAltitude def setCurrentAltitude(self, altitude): if altitude < self.minAltitude or self.minAltitude == 0: self.minAltitude = altitude self.currentAltitude = altitude def read_input(self): """Read input data from the selected device""" if self.pointerDevice is not None: #DT = 0.001 if self.pointerDevice.poll(): buttons = self.pointerDevice.get_buttons() if buttons & psmove.Btn_MOVE: self.pointerDevice.set_leds(0, 255, 0) self.pointerDevice.update_leds() self.SF = sensorFusion() ''' trigger_value = self.move.get_trigger() self.move.set_leds(trigger_value, 0, 0) self.move.update_leds() ''' ax, ay, az = self.pointerDevice.get_accelerometer_frame(psmove.Frame_SecondHalf) gx, gy, gz = self.pointerDevice.get_gyroscope_frame(psmove.Frame_SecondHalf) gx = gx * 180 / math.pi gy = gy * 180 / math.pi gz = gz * 180 / math.pi #print "A: %5.2f %5.2f %5.2f " % ( ax , ay , az ) #print "G: %8.2f %8.2f %8.2f " % ( gx , gy , gz ) self.SF.sensfusion6UpdateQ(gx, gy, gz, ax, ay, az, 1/100) roll, pitch, yaw = self.SF.sensfusion6GetEulerRPY() self.PointerYaw = -yaw ''' # Read sensor acc_x = self.pointerDevice.ax acc_y = self.pointerDevice.ay acc_z = self.pointerDevice.az gyr_x = self.pointerDevice.gx gyr_y = self.pointerDevice.gy gyr_z = self.pointerDevice.gz #// Calculate pitch and roll based only on acceleration. acc_pitch = math.atan2(acc_x, -acc_z) acc_roll = -math.atan2(acc_y, -acc_z) # Perform filtering self.kalmanPitch.kalman_innovate(acc_pitch, gyr_x, DT) self.kalmanRoll.kalman_innovate(acc_roll, gyr_y, DT) # The angle is stored in state 1 pitch = self.kalmanPitch.x1 roll = self.kalmanRoll.x1 cosRoll = math.cos(roll) sinRoll = math.sin(roll) cosPitch = math.cos(pitch) sinPitch = math.sin(pitch) magX = self.pointerDevice.mx * cosPitch + self.pointerDevice.mz * sinPitch magY = self.pointerDevice.mx * sinRoll * sinPitch + self.pointerDevice.my * cosRoll - self.pointerDevice.mz * sinRoll * cosPitch norm = math.sqrt(magX*magX + magY*magY) magHeadingX = magX/norm magHeadingY = -magY/norm #Absolute Yaw #self.PointerYaw = self.pointerDevice.mz #roll = self.pointerDevice.gy - self.pointerDevice.gy/GYROSCOPE_SENSITIVITY*dt #pitch = self.pointerDevice.gx - self.pointerDevice.gx/GYROSCOPE_SENSITIVITY*dt DT = 0.001 yaw = self.pointerDevice.gz - self.pointerDevice.gz/GYROSCOPE_SENSITIVITY*DT self.PointerYaw -= yaw*DT if self.PointerYaw >= 180: self.PointerYaw = -180 elif self.PointerYaw <= -180: self.PointerYaw = 180 if self.pointerDevice.get_buttons() & psmove.Btn_MOVE: #psmove.Btn_T: self.pointerDevice.set_leds(0, 255, 0) self.pointerDevice.update_leds() self.PointerYaw = 0 ''' if self.PointerYaw >= 0: self.pointerDevice.set_leds(int(255*self.PointerYaw/180), 255, 0) else: self.pointerDevice.set_leds(0, 255, int(255*math.fabs(self.PointerYaw)/180)) self.pointerDevice.update_leds() self.pointer_input_updated.call(self.PointerYaw, False) try: data = self.inputdevice.read_input() roll = data["roll"] * self._max_rp_angle pitch = data["pitch"] * self._max_rp_angle thrust = data["thrust"] yaw = data["yaw"] raw_thrust = data["thrust"] emergency_stop = data["estop"] trim_roll = data["rollcal"] trim_pitch = data["pitchcal"] althold = data["althold"] flipleft = data["flipleft"] flipright = data["flipright"] viscousMode = data["viscousMode"] switchMode = data["switchmode"] if switchMode and self._canSwitch: self._canSwitch = False self.switch_mode_updated.call() elif not switchMode: self._canSwitch = True if (self._old_alt_hold != althold): self.althold_updated.call(althold) self._old_alt_hold = althold if self._emergency_stop != emergency_stop: self._emergency_stop = emergency_stop self.emergency_stop_updated.call(self._emergency_stop) if self.auto: self.auto_input_updated.call(trim_roll, trim_pitch, yaw, thrust) else: # Altitude Hold Mode Toggled if (self._old_alt_hold != althold): self.althold_updated.call(althold) self._old_alt_hold = althold # Disable hover mode if enabled if self._emergency_stop: if self._has_pressure_sensor: if self._old_alt_hold: self.althold_updated.call(False) self._old_alt_hold = False althold = False ''' modalità in cui il quad rimane fermo in altitudine e può salire o scendere in base a come si utilizza il joystick thrust up (>0) => sale (costantemente) thrust down (<0) => scende (costantemente) ''' if viscousMode: viscous_thrust = self.p2t(self.viscousModeThrust) if raw_thrust > 0 and raw_thrust <= 0.5: raw_thrust = 1 elif raw_thrust > 0.5: raw_thrust = 2 elif raw_thrust >= -0.5 and raw_thrust < 0: raw_thrust = -0.5 elif raw_thrust < -0.5: raw_thrust = -1 ''' if (self.currentAltitude - self.minAltitude) == self._maxAltitude: raw_thrust = 0 elif (self.currentAltitude - self.minAltitude) > self._maxAltitude: raw_thrust = -0.2 ''' thrust = int(round(viscous_thrust + raw_thrust*self.p2t(10))) # Thust limiting (slew, minimum and emergency stop) elif (althold and self._has_pressure_sensor) or (flipleft or flipright): thrust = int(round(JoystickReader.deadband(thrust,0.2)*32767 + 32767)) #Convert to uint16 else: if raw_thrust < 0.05 or emergency_stop: thrust = 0 else: thrust = self._min_thrust + thrust * (self._max_thrust - self._min_thrust) if (self._thrust_slew_enabled == True and self._thrust_slew_limit > thrust and not emergency_stop): #if (self._thrust_slew_enabled == True and not emergency_stop): if self._old_thrust > self._thrust_slew_limit: self._old_thrust = self._thrust_slew_limit if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)): thrust = self._old_thrust - self._thrust_slew_rate / 100 if raw_thrust < 0 or thrust < self._min_thrust: thrust = 0 #if trim_pitch > 0: # thrust += self.p2t(1) #if trim_pitch < 0: # thrust -= self.p2t(1) if self._emergency_landing: thrust = self._old_thrust - self.p2t(10)*0.2 if thrust < 0: thrust = 0 self._old_thrust = thrust # Yaw deadband # TODO: Add to input device config? yaw = JoystickReader.deadband(yaw,0.2)*self._max_yaw_rate if trim_roll != 0 or trim_pitch != 0: self._trim_roll += trim_roll self._trim_pitch += trim_pitch self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) if (flipleft or flipright) and self._flip_time_start < 0: self._flip_time_start = time.time(); #ricavo il momento in cui inizia il flip if flipleft: self._old_flip_type = 0; if flipright: self._old_flip_type = 1; #if (flipleft and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 0: if flipleft and self._old_flip_type == 0: thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente roll = 1600 #elif (flipright and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 1: elif flipright and self._old_flip_type == 1: #thrust = self.p2t(70) #roll = 30 #self.input_updated.call(roll, 0, yaw, thrust) #time.sleep(0.04) thrust = self.p2t(50) roll = -1000 self.input_updated.call(roll, 0, yaw, thrust) #time.sleep(FLIP_TIME) ''' ####### ## 1 ## ####### thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente roll = 30 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 2 ## ####### thrust = self.p2t(50) roll = -1600 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 3 ## ####### thrust = 0 roll = 0 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.0004) ####### ## 4 ## ####### thrust = self.p2t(50) roll = -1600 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 5 ## ####### thrust = self.p2t(70) roll = 30 self.input_updated.call(roll, 0, yaw, thrust) time.sleep(0.004) ####### ## 6 ## ####### thrust = self.p2t(53) roll = 0 self.input_updated.call(roll, 0, yaw, thrust) return; ''' trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch if not flipleft and not flipright and not self.flipTimeControl(self._flip_time_start): self._old_flip_type = -1; self._flip_time_start = -float("inf"); #resetto _flip_time_start self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch #yaw = yaw + self._trim_yaw self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self._read_timer.stop() def update_trim_yaw_signal(self, yaw): self._trim_yaw = yaw @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) @staticmethod def deadband(value, threshold): if abs(value) < threshold: value = 0 elif value > 0: value -= threshold elif value < 0: value += threshold return value/(1-threshold) @staticmethod def flipTimeControl(startTime): return (time.time()-startTime >= 0 and time.time()-startTime <= FLIP_TIME)
class JoystickReader: """ Thread that will read input from devices/joysticks and send control-set ponts to the Crazyflie """ inputConfig = [] def __init__(self, do_device_discovery=True, cf=None): # TODO: Should be OS dependant self.inputdevice = AiController(cf)#PyGameReader() self._min_thrust = 0 self._max_thrust = 0 self._thrust_slew_rate = 0 self._thrust_slew_enabled = False self._thrust_slew_limit = 0 self._emergency_stop = False self._has_pressure_sensor = False self._old_thrust = 0 self._old_alt_hold = False self._trim_roll = Config().get("trim_roll") self._trim_pitch = Config().get("trim_pitch") if (Config().get("flightmode") is "Normal"): self._max_yaw_rate = Config().get("normal_max_yaw") self._max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits( Config().get("normal_min_thrust"), Config().get("normal_max_thrust")) self.set_thrust_slew_limiting( Config().get("normal_slew_rate"), Config().get("normal_slew_limit")) else: self._max_yaw_rate = Config().get("max_yaw") self._max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.set_thrust_limits( Config().get("min_thrust"), Config().get("max_thrust")) self.set_thrust_slew_limiting( Config().get("slew_rate"), Config().get("slew_limit")) self._dev_blacklist = None if (len(Config().get("input_device_blacklist")) > 0): self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(0.01, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager(). configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.althold_updated = Caller() def setAltHoldAvailable(self, available): self._has_pressure_sensor = available def setAltHold(self, althold): self._old_alt_hold = althold def _do_device_discovery(self): devs = self.getAvailableDevices() if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def getAvailableDevices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = self.inputdevice.getAvailableDevices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev["name"]))): self._available_devices[dev["name"]] = dev["id"] approved_devs.append(dev) return approved_devs def enableRawReading(self, deviceId): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ self.inputdevice.enableRawReading(deviceId) def disableRawReading(self): """Disable raw reading of input device.""" self.inputdevice.disableRawReading() def readRawValues(self): """ Read raw values from the input device.""" return self.inputdevice.readRawValues() def start_input(self, device_name, config_name): """ Start reading input from the device with name device_name using config config_name """ try: device_id = self._available_devices[device_name] self.inputdevice.start_input( device_id, ConfigManager().get_config(config_name)) self._read_timer.start() except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) def stop_input(self): """Stop reading from the input device.""" self._read_timer.stop() def set_yaw_limit(self, max_yaw_rate): """Set a new max yaw rate value.""" self._max_yaw_rate = max_yaw_rate def set_rp_limit(self, max_rp_angle): """Set a new max roll/pitch value.""" self._max_rp_angle = max_rp_angle def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit): """Set new values for limit where the slewrate control kicks in and for the slewrate.""" self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate) self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit) if (thrust_slew_rate > 0): self._thrust_slew_enabled = True else: self._thrust_slew_enabled = False def set_thrust_limits(self, min_thrust, max_thrust): """Set a new min/max thrust limit.""" self._min_thrust = JoystickReader.p2t(min_thrust) self._max_thrust = JoystickReader.p2t(max_thrust) def set_trim_roll(self, trim_roll): """Set a new value for the roll trim.""" self._trim_roll = trim_roll def set_trim_pitch(self, trim_pitch): """Set a new value for the trim trim.""" self._trim_pitch = trim_pitch def read_input(self): """Read input data from the selected device""" try: data = self.inputdevice.read_input() roll = data["roll"] * self._max_rp_angle pitch = data["pitch"] * self._max_rp_angle thrust = data["thrust"] yaw = data["yaw"] raw_thrust = data["thrust"] emergency_stop = data["estop"] trim_roll = data["rollcal"] trim_pitch = data["pitchcal"] althold = data["althold"] if (self._old_alt_hold != althold): self.althold_updated.call(str(althold)) self._old_alt_hold = althold if self._emergency_stop != emergency_stop: self._emergency_stop = emergency_stop self.emergency_stop_updated.call(self._emergency_stop) # Thust limiting (slew, minimum and emergency stop) if althold and self._has_pressure_sensor: thrust = int(round(JoystickReader.deadband(thrust,0.2)*32767 + 32767)) #Convert to uint16 else: if raw_thrust < 0.05 or emergency_stop: thrust = 0 else: thrust = self._min_thrust + thrust * (self._max_thrust - self._min_thrust) if (self._thrust_slew_enabled == True and self._thrust_slew_limit > thrust and not emergency_stop): if self._old_thrust > self._thrust_slew_limit: self._old_thrust = self._thrust_slew_limit if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)): thrust = self._old_thrust - self._thrust_slew_rate / 100 if raw_thrust < 0 or thrust < self._min_thrust: thrust = 0 self._old_thrust = thrust # Yaw deadband # TODO: Add to input device config? yaw = JoystickReader.deadband(yaw,0.2)*self._max_yaw_rate if trim_roll != 0 or trim_pitch != 0: self._trim_roll += trim_roll self._trim_pitch += trim_pitch self.rp_trim_updated.call(self._trim_roll, self._trim_pitch) trimmed_roll = roll + self._trim_roll trimmed_pitch = pitch + self._trim_pitch self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) @staticmethod def deadband(value, threshold): if abs(value) < threshold: value = 0 elif value > 0: value -= threshold elif value < 0: value += threshold return value/(1-threshold)
class LogConfig(object): """Representation of one log configuration that enables logging from the Crazyflie""" _config_id_counter = 1 def __init__(self, name, period_in_ms): """Initialize the entry""" self.data_received_cb = Caller() self.error_cb = Caller() self.started_cb = Caller() self.added_cb = Caller() self.err_no = 0 self.id = LogConfig._config_id_counter LogConfig._config_id_counter += 1 % 255 self.cf = None self.period = period_in_ms / 10 self.period_in_ms = period_in_ms self._added = False self._started = False self.valid = False self.variables = [] self.default_fetch_as = [] self.name = name def add_variable(self, name, fetch_as=None): """Add a new variable to the configuration. name - Complete name of the variable in the form group.name fetch_as - String representation of the type the variable should be fetched as (i.e uint8_t, float, FP16, etc) If no fetch_as type is supplied, then the stored as type will be used (i.e the type of the fetched variable is the same as it's stored in the Crazyflie).""" if fetch_as: self.variables.append(LogVariable(name, fetch_as)) else: # We cannot determine the default type until we have connected. So # save the name and we will add these once we are connected. self.default_fetch_as.append(name) def add_memory(self, name, fetch_as, stored_as, address): """Add a raw memory position to log. name - Arbitrary name of the variable fetch_as - String representation of the type of the data the memory should be fetch as (i.e uint8_t, float, FP16) stored_as - String representation of the type the data is stored as in the Crazyflie address - The address of the data """ self.variables.append(LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as, address)) def _set_added(self, added): self._added = added self.added_cb.call(added) def _get_added(self): return self._added def _set_started(self, started): self._started = started self.started_cb.call(started) def _get_started(self): return self._started added = property(_get_added, _set_added) started = property(_get_started, _set_started) def start(self): """Start the logging for this entry""" if (self.cf.link is not None): if (self._added is False): logger.debug("First time block is started, add block") pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_CREATE_BLOCK, self.id) for var in self.variables: if (var.is_toc_variable() is False): # Memory location logger.debug("Logging to raw memory %d, 0x%04X", var.get_storage_and_fetch_byte(), var.address) pk.data += struct.pack('<B', var.get_storage_and_fetch_byte()) pk.data += struct.pack('<I', var.address) else: # Item in TOC logger.debug("Adding %s with id=%d and type=0x%02X", var.name, self.cf.log.toc.get_element_id( var.name), var.get_storage_and_fetch_byte()) pk.data += struct.pack('<B', var.get_storage_and_fetch_byte()) pk.data += struct.pack('<B', self.cf.log.toc. get_element_id(var.name)) logger.debug("Adding log block id {}".format(self.id)) self.cf.send_packet(pk, expect_answer=True) else: logger.debug("Block already registered, starting logging" " for %d", self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.id, self.period) self.cf.send_packet(pk, expect_answer=True) def stop(self): """Stop the logging for this entry""" if (self.cf.link is not None): if (self.id is None): logger.warning("Stopping block, but no block registered") else: logger.debug("Sending stop logging for block %d", self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.id) self.cf.send_packet(pk, expect_answer=True) def delete(self): """Delete this entry in the Crazyflie""" if (self.cf.link is not None): if (self.id is None): logger.warning("Delete block, but no block registered") else: logger.debug("LogEntry: Sending delete logging for block %d" % self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.id) self.cf.send_packet(pk, expect_answer=True) def unpack_log_data(self, log_data, timestamp): """Unpack received logging data so it represent real values according to the configuration in the entry""" ret_data = {} data_index = 0 for var in self.variables: size = LogTocElement.get_size_from_id(var.fetch_as) name = var.name unpackstring = LogTocElement.get_unpack_string_from_id( var.fetch_as) value = struct.unpack(unpackstring, log_data[data_index:data_index + size])[0] data_index += size ret_data[name] = value self.data_received_cb.call(timestamp, ret_data, self)
class Localization(): """ Handle localization-related data communication with the Crazyflie """ # Implemented channels POSITION_CH = 0 GENERIC_CH = 1 # Location message types for generig channel RANGE_STREAM_REPORT = 0 RANGE_STREAM_REPORT_FP16 = 1 LPS_SHORT_LPP_PACKET = 2 EMERGENCY_STOP = 3 EMERGENCY_STOP_WATCHDOG = 4 COMM_GNSS_NMEA = 6 COMM_GNSS_PROPRIETARY = 7 EXT_POSE = 8 EXT_POSE_PACKED = 9 LH_ANGLE_STREAM = 10 LH_PERSIST_DATA = 11 def __init__(self, crazyflie=None): """ Initialize the Extpos object. """ self._cf = crazyflie self.receivedLocationPacket = Caller() self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming) def _incoming(self, packet): """ Callback for data received from the copter. """ if len(packet.data) < 1: logger.warning('Localization packet received with incorrect' + 'length (length is {})'.format(len(packet.data))) return pk_type = struct.unpack('<B', packet.data[:1])[0] data = packet.data[1:] # Decoding the known packet types # TODO: more generic decoding scheme? decoded_data = None if pk_type == self.RANGE_STREAM_REPORT: if len(data) % 5 != 0: logger.error('Wrong range stream report data lenght') return decoded_data = {} raw_data = data for i in range(int(len(data) / 5)): anchor_id, distance = struct.unpack('<Bf', raw_data[:5]) decoded_data[anchor_id] = distance raw_data = raw_data[5:] elif pk_type == self.LH_PERSIST_DATA: decoded_data = bool(data[0]) elif pk_type == self.LH_ANGLE_STREAM: decoded_data = self._decode_lh_angle(data) pk = LocalizationPacket(pk_type, data, decoded_data) self.receivedLocationPacket.call(pk) def _decode_lh_angle(self, data): decoded_data = {} raw_data = struct.unpack('<Bfhhhfhhh', data) decoded_data['basestation'] = raw_data[0] decoded_data['x'] = [0, 0, 0, 0] decoded_data['x'][0] = raw_data[1] decoded_data['x'][1] = raw_data[1] - fp16_to_float(raw_data[2]) decoded_data['x'][2] = raw_data[1] - fp16_to_float(raw_data[3]) decoded_data['x'][3] = raw_data[1] - fp16_to_float(raw_data[4]) decoded_data['y'] = [0, 0, 0, 0] decoded_data['y'][0] = raw_data[5] decoded_data['y'][1] = raw_data[5] - fp16_to_float(raw_data[6]) decoded_data['y'][2] = raw_data[5] - fp16_to_float(raw_data[7]) decoded_data['y'][3] = raw_data[5] - fp16_to_float(raw_data[8]) return decoded_data def send_extpos(self, pos): """ Send the current Crazyflie X, Y, Z position. This is going to be forwarded to the Crazyflie's position estimator. """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.POSITION_CH pk.data = struct.pack('<fff', pos[0], pos[1], pos[2]) self._cf.send_packet(pk) def send_extpose(self, pos, quat): """ Send the current Crazyflie pose (position [x, y, z] and attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded to the Crazyflie's position estimator. """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH pk.data = struct.pack('<Bfffffff', self.EXT_POSE, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]) self._cf.send_packet(pk) def send_short_lpp_packet(self, dest_id, data): """ Send ultra-wide-band LPP packet to dest_id """ pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data self._cf.send_packet(pk) def send_lh_persist_data_packet(self, geo_list, calib_list): """ Send geometry and calibration data to persistent memory subsystem """ geo_list.sort() calib_list.sort() max_bs_nr = 15 if len(geo_list) > 0: if geo_list[0] < 0 or geo_list[-1] > max_bs_nr: raise Exception('Geometry BS list is not valid') if len(calib_list) > 0: if calib_list[0] < 0 or calib_list[-1] > max_bs_nr: raise Exception('Calibration BS list is not valid') mask_geo = 0 mask_calib = 0 for bs in geo_list: mask_geo += 1 << bs for bs in calib_list: mask_calib += 1 << bs pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.GENERIC_CH pk.data = struct.pack('<BHH', self.LH_PERSIST_DATA, mask_geo, mask_calib) self._cf.send_packet(pk) return pk.data
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self._initialized = Event() self.values = {} def request_update_of_all_params(self): """Request an update of all the parameters in the TOC""" for group in self.toc.toc: for name in self.toc.toc[group]: complete_name = '%s.%s' % (group, name) self.request_param_update(complete_name) def _check_if_all_updated(self): """Check if all parameters from the TOC has at least been fetched once""" for g in self.toc.toc: if g not in self.values: return False for n in self.toc.toc[g]: if n not in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: s = struct.unpack(element.pytype, pk.data[1:])[0] s = s.__str__() complete_name = '%s.%s' % (element.group, element.name) # Save the value for synchronous access if element.group not in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s logger.debug('Updated parameter [%s]' % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( complete_name, s) if element.group in self.group_update_callbacks: self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) # Once all the parameters are updated call the # callback for "everything updated" (after all the param # updated callbacks) if self._check_if_all_updated() and not self.is_updated: self.is_updated = True self._initialized.set() self.all_updated.call() else: logger.debug('Variable id [%d] not found in TOC', var_id) def remove_update_callback(self, group, name=None, cb=None): """Remove the supplied callback for a group or a group.name""" if not cb: return if not name: if group in self.group_update_callbacks: self.group_update_callbacks[group].remove_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname in self.param_update_callbacks: self.param_update_callbacks[paramname].remove_callback(cb) def add_update_callback(self, group=None, name=None, cb=None): """ Add a callback for a specific parameter name. This callback will be executed when a new value is read from the Crazyflie. """ if not group and not name: self.all_update_callback.add_callback(cb) elif not name: if group not in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False self._initialized.clear() # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value_raw(self, complete_name, type, value): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) # This gives us the type for the struct.pack pytype = ParamTocElement.types[type][1][1] pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f'<B{len_array}sB{pytype}', 0, char_array, type, value) # We will not get an update callback when using raw (MISC_CHANNEL) # so just send. self.cf.send_packet(pk) def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ element = self.toc.get_element_by_complete_name(complete_name) if not element: logger.warning("Cannot set value for [%s], it's not in the TOC!", complete_name) raise KeyError('{} not in param TOC'.format(complete_name)) elif element.access == ParamTocElement.RO_ACCESS: logger.debug('[%s] is read only, no trying to set value', complete_name) raise AttributeError('{} is read-only!'.format(complete_name)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) if self._useV2: pk.data = struct.pack('<H', varid) else: pk.data = struct.pack('<B', varid) try: value_nr = eval(value) except TypeError: value_nr = value pk.data += struct.pack(element.pytype, value_nr) self.param_updater.request_param_setvalue(pk) def get_value(self, complete_name, timeout=60): """ Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') [group, name] = complete_name.split('.') return self.values[group][name]
class Param(): """ Used to read and write parameter values in the Crazyflie. """ toc = Toc() def __init__(self, crazyflie): self.cf = crazyflie self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater(self.cf, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self.param_updater.close) self.all_updated = Caller() self._have_updated = False self.values = {} def request_update_of_all_params(self): """Request an update of all the parameters in the TOC""" for group in self.toc.toc: for name in self.toc.toc[group]: complete_name = "%s.%s" % (group, name) self.request_param_update(complete_name) def _check_if_all_updated(self): """Check if all parameters from the TOC has at least been fetched once""" for g in self.toc.toc: if not g in self.values: return False for n in self.toc.toc[g]: if not n in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" var_id = pk.datal[0] element = self.toc.get_element_by_id(var_id) if element: s = struct.unpack(element.pytype, pk.data[1:])[0] s = s.__str__() complete_name = "%s.%s" % (element.group, element.name) # Save the value for synchronous access if not element.group in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s # This will only be called once if self._check_if_all_updated() and not self._have_updated: self._have_updated = True self.all_updated.call() logger.debug("Updated parameter [%s]" % complete_name) if complete_name in self.param_update_callbacks: self.param_update_callbacks[complete_name].call( complete_name, s) if element.group in self.group_update_callbacks: self.group_update_callbacks[element.group].call( complete_name, s) self.all_update_callback.call(complete_name, s) else: logger.debug("Variable id [%d] not found in TOC", var_id) def remove_update_callback(self, group, name=None, cb=None): """Remove the supplied callback for a group or a group.name""" if not cb: return if not name: if group in self.group_update_callbacks: self.group_update_callbacks[group].remove_callback(cb) else: paramname = "{}.{}".format(group, name) if paramname in self.param_update_callbacks: self.param_update_callbacks[paramname].remove_callback(cb) def add_update_callback(self, group=None, name=None, cb=None): """ Add a callback for a specific parameter name. This callback will be executed when a new value is read from the Crazyflie. """ if not group and not name: self.all_update_callback.add_callback(cb) elif not name: if not group in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = "{}.{}".format(group, name) if not paramname in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self.toc = Toc() toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self._have_updated = False def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value(self, complete_name, value): """ Set the value for the supplied parameter. """ element = self.toc.get_element_by_complete_name(complete_name) if not element: logger.warning("Cannot set value for [%s], it's not in the TOC!", complete_name) raise KeyError("{} not in param TOC".format(complete_name)) elif element.access == ParamTocElement.RO_ACCESS: logger.debug("[%s] is read only, no trying to set value", complete_name) raise AttributeError("{} is read-only!".format(complete_name)) else: varid = element.ident pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL) pk.data = struct.pack('<B', varid) pk.data += struct.pack(element.pytype, eval(value)) self.param_updater.request_param_setvalue(pk)
class Crazyflie(): """The Crazyflie class""" def __init__(self, link=None, ro_cache=None, rw_cache=None): """ Create the objects from this module and register callbacks. ro_cache -- Path to read-only cache (string) rw_cache -- Path to read-write cache (string) """ # Called on disconnect, no matter the reason self.disconnected = Caller() # Called on unintentional disconnect only self.connection_lost = Caller() # Called when the first packet in a new link is received self.link_established = Caller() # Called when the user requests a connection self.connection_requested = Caller() # Called when the link is established and the TOCs (that are not # cached) have been downloaded self.connected = Caller() # Called if establishing of the link fails (i.e times out) self.connection_failed = Caller() # Called for every packet received self.packet_received = Caller() # Called for every packet sent self.packet_sent = Caller() # Called when the link driver updates the link quality measurement self.link_quality_updated = Caller() self.state = State.DISCONNECTED self.link = link self._toc_cache = TocCache(ro_cache=ro_cache, rw_cache=rw_cache) self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) self.incoming.start() self.commander = Commander(self) self.loc = Localization(self) self.extpos = Extpos(self) self.log = Log(self) self.console = Console(self) self.param = Param(self) self.mem = Memory(self) self.platform = PlatformService(self) self.link_uri = '' # Used for retry when no reply was sent back self.packet_received.add_callback(self._check_for_initial_packet_cb) self.packet_received.add_callback(self._check_for_answers) self._answer_patterns = {} self._send_lock = Lock() self.connected_ts = None # Connect callbacks to logger self.disconnected.add_callback( lambda uri: logger.info('Callback->Disconnected from [%s]', uri)) self.disconnected.add_callback(self._disconnected) self.link_established.add_callback( lambda uri: logger.info('Callback->Connected to [%s]', uri)) self.connection_lost.add_callback( lambda uri, errmsg: logger.info( 'Callback->Connection lost to [%s]: %s', uri, errmsg)) self.connection_failed.add_callback( lambda uri, errmsg: logger.info( 'Callback->Connected failed to [%s]: %s', uri, errmsg)) self.connection_requested.add_callback( lambda uri: logger.info( 'Callback->Connection initialized[%s]', uri)) self.connected.add_callback( lambda uri: logger.info( 'Callback->Connection setup finished [%s]', uri)) def _disconnected(self, link_uri): """ Callback when disconnected.""" self.connected_ts = None def _start_connection_setup(self): """Start the connection setup by refreshing the TOCs""" logger.info('We are connected[%s], request connection setup', self.link_uri) self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache) def _param_toc_updated_cb(self): """Called when the param TOC has been fully updated""" logger.info('Param TOC finished updating') self.connected_ts = datetime.datetime.now() self.connected.call(self.link_uri) # Trigger the update for all the parameters self.param.request_update_of_all_params() def _mems_updated_cb(self): """Called when the memories have been identified""" logger.info('Memories finished updating') self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache) def _log_toc_updated_cb(self): """Called when the log TOC has been fully updated""" logger.info('Log TOC finished updating') self.mem.refresh(self._mems_updated_cb) def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED or self.state == State.SETUP_FINISHED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection (download log/parameter TOC). """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver( link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) self._start_connection_setup() except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self._answer_patterns = {} self.disconnected.call(self.link_uri) """Check if the communication link is open or not.""" def is_connected(self): return self.connected_ts is not None def add_port_callback(self, port, cb): """Add a callback to cb on port""" self.incoming.add_port_callback(port, cb) def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) def _no_answer_do_retry(self, pk, pattern): """Resend packets that we have not gotten answers to""" logger.info('Resending for pattern %s', pattern) # Set the timer to None before trying to send again self.send_packet(pk, expected_reply=pattern, resend=True) def _check_for_answers(self, pk): """ Callback called for every packet received to check if we are waiting for an answer on this port. If so, then cancel the retry timer. """ longest_match = () if len(self._answer_patterns) > 0: data = (pk.header,) + tuple(pk.data) for p in list(self._answer_patterns.keys()): logger.debug('Looking for pattern match on %s vs %s', p, data) if len(p) <= len(data): if p == data[0:len(p)]: match = data[0:len(p)] if len(match) >= len(longest_match): logger.debug('Found new longest match %s', match) longest_match = match if len(longest_match) > 0: self._answer_patterns[longest_match].cancel() del self._answer_patterns[longest_match] def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ Send a packet through the link interface. pk -- Packet to send expect_answer -- True if a packet from the Crazyflie is expected to be sent back, otherwise false """ self._send_lock.acquire() if self.link is not None: if len(expected_reply) > 0 and not resend and \ self.link.needs_resending: pattern = (pk.header,) + expected_reply logger.debug( 'Sending packet and expecting the %s pattern back', pattern) new_timer = Timer(timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() elif resend: # Check if we have gotten an answer, if not try again pattern = expected_reply if pattern in self._answer_patterns: logger.debug('We want to resend and the pattern is there') if self._answer_patterns[pattern]: new_timer = Timer(timeout, lambda: self._no_answer_do_retry( pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() else: logger.debug('Resend requested, but no pattern found: %s', self._answer_patterns) self.link.send_packet(pk) self.packet_sent.call(pk) self._send_lock.release()
class JoystickReader(object): """ Thread that will read input from devices/joysticks and send control-set points to the Crazyflie """ inputConfig = [] ASSISTED_CONTROL_ALTHOLD = 0 ASSISTED_CONTROL_POSHOLD = 1 ASSISTED_CONTROL_HEIGHTHOLD = 2 ASSISTED_CONTROL_HOVER = 3 def __init__(self, do_device_discovery=True): self._input_device = None self._mux = [ NoMux(self), TakeOverSelectiveMux(self), TakeOverMux(self) ] # Set NoMux as default self._selected_mux = self._mux[0] self.min_thrust = 0 self.max_thrust = 0 self._thrust_slew_rate = 0 self.thrust_slew_enabled = False self.thrust_slew_limit = 0 self.has_pressure_sensor = False self._hover_max_height = MAX_TARGET_HEIGHT self.max_rp_angle = 0 self.max_yaw_rate = 0 try: self.set_assisted_control(Config().get("assistedControl")) except KeyError: self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD) self._old_thrust = 0 self._old_raw_thrust = 0 self.springy_throttle = True self._target_height = INITAL_TAGET_HEIGHT self.trim_roll = Config().get("trim_roll") self.trim_pitch = Config().get("trim_pitch") self._rp_dead_band = 0.1 self._input_map = None if Config().get("flightmode") == "Normal": self.max_yaw_rate = Config().get("normal_max_yaw") self.max_rp_angle = Config().get("normal_max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("normal_min_thrust") self.max_thrust = Config().get("normal_max_thrust") self.thrust_slew_limit = Config().get("normal_slew_limit") self.thrust_slew_rate = Config().get("normal_slew_rate") else: self.max_yaw_rate = Config().get("max_yaw") self.max_rp_angle = Config().get("max_rp") # Values are stored at %, so use the functions to set the values self.min_thrust = Config().get("min_thrust") self.max_thrust = Config().get("max_thrust") self.thrust_slew_limit = Config().get("slew_limit") self.thrust_slew_rate = Config().get("slew_rate") self._dev_blacklist = None if len(Config().get("input_device_blacklist")) > 0: self._dev_blacklist = re.compile( Config().get("input_device_blacklist")) logger.info("Using device blacklist [{}]".format( Config().get("input_device_blacklist"))) self._available_devices = {} # TODO: The polling interval should be set from config file self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input) if do_device_discovery: self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery) self._discovery_timer.start() # Check if user config exists, otherwise copy files if not os.path.exists(ConfigManager().configs_dir): logger.info("No user config found, copying dist files") os.makedirs(ConfigManager().configs_dir) for f in glob.glob(cfclient.module_path + "/configs/input/[A-Za-z]*.json"): dest = os.path.join(ConfigManager().configs_dir, os.path.basename(f)) if not os.path.isfile(dest): logger.debug("Copying %s", f) shutil.copy2(f, ConfigManager().configs_dir) ConfigManager().get_list_of_configs() self.input_updated = Caller() self.assisted_input_updated = Caller() self.heighthold_input_updated = Caller() self.hover_input_updated = Caller() self.rp_trim_updated = Caller() self.emergency_stop_updated = Caller() self.device_discovery = Caller() self.device_error = Caller() self.assisted_control_updated = Caller() self.alt1_updated = Caller() self.alt2_updated = Caller() # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting) self.limiting_updated = Caller() def _get_device_from_name(self, device_name): """Get the raw device from a name""" for d in readers.devices(): if d.name == device_name: return d return None def set_hover_max_height(self, height): self._hover_max_height = height def set_alt_hold_available(self, available): """Set if altitude hold is available or not (depending on HW)""" self.has_pressure_sensor = available def _do_device_discovery(self): devs = self.available_devices() # This is done so that devs can easily get access # to limits without creating lots of extra code for d in devs: d.input = self if len(devs): self.device_discovery.call(devs) self._discovery_timer.stop() def available_mux(self): return self._mux def set_mux(self, name=None, mux=None): old_mux = self._selected_mux if name: for m in self._mux: if m.name == name: self._selected_mux = m elif mux: self._selected_mux = mux old_mux.close() logger.info("Selected MUX: {}".format(self._selected_mux.name)) def set_assisted_control(self, mode): self._assisted_control = mode def get_assisted_control(self): return self._assisted_control def available_devices(self): """List all available and approved input devices. This function will filter available devices by using the blacklist configuration and only return approved devices.""" devs = readers.devices() devs += interfaces.devices() approved_devs = [] for dev in devs: if ((not self._dev_blacklist) or (self._dev_blacklist and not self._dev_blacklist.match(dev.name))): dev.input = self approved_devs.append(dev) return approved_devs def enableRawReading(self, device_name): """ Enable raw reading of the input device with id deviceId. This is used to get raw values for setting up of input devices. Values are read without using a mapping. """ if self._input_device: self._input_device.close() self._input_device = None for d in readers.devices(): if d.name == device_name: self._input_device = d # Set the mapping to None to get raw values self._input_device.input_map = None self._input_device.open() def get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in list(device_config_mapping.keys()): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def stop_raw_reading(self): """Disable raw reading of input device.""" if self._input_device: self._input_device.close() self._input_device = None def read_raw_values(self): """ Read raw values from the input device.""" [axes, buttons, mapped_values] = self._input_device.read(include_raw=True) dict_axes = {} dict_buttons = {} for i, a in enumerate(axes): dict_axes[i] = a for i, b in enumerate(buttons): dict_buttons[i] = b return [dict_axes, dict_buttons, mapped_values] def set_raw_input_map(self, input_map): """Set an input device map""" # TODO: Will not work! if self._input_device: self._input_device.input_map = input_map def set_input_map(self, device_name, input_map_name): """Load and set an input device map with the given name""" dev = self._get_device_from_name(device_name) settings = ConfigManager().get_settings(input_map_name) if settings: self.springy_throttle = settings["springythrottle"] self._rp_dead_band = settings["rp_dead_band"] self._input_map = ConfigManager().get_config(input_map_name) dev.input_map = self._input_map dev.input_map_name = input_map_name Config().get("device_config_mapping")[device_name] = input_map_name dev.set_dead_band(self._rp_dead_band) def start_input(self, device_name, role="Device", config_name=None): """ Start reading input from the device with name device_name using config config_name. Returns True if device supports mapping, otherwise False """ try: # device_id = self._available_devices[device_name] # Check if we supplied a new map, if not use the preferred one device = self._get_device_from_name(device_name) self._selected_mux.add_device(device, role) # Update the UI with the limiting for this device self.limiting_updated.call(device.limit_rp, device.limit_yaw, device.limit_thrust) self._read_timer.start() return device.supports_mapping except Exception: self.device_error.call( "Error while opening/initializing input device\n\n%s" % (traceback.format_exc())) if not self._input_device: self.device_error.call( "Could not find device {}".format(device_name)) return False def resume_input(self): self._selected_mux.resume() self._read_timer.start() def pause_input(self, device_name=None): """Stop reading from the input device.""" self._read_timer.stop() self._selected_mux.pause() def _set_thrust_slew_rate(self, rate): self._thrust_slew_rate = rate if rate > 0: self.thrust_slew_enabled = True else: self.thrust_slew_enabled = False def _get_thrust_slew_rate(self): return self._thrust_slew_rate def read_input(self): """Read input data from the selected device""" try: data = self._selected_mux.read() if data: if data.toggled.assistedControl: if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_POSHOLD or \ self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HOVER: if data.assistedControl and self._assisted_control != \ JoystickReader.ASSISTED_CONTROL_HOVER: for d in self._selected_mux.devices(): d.limit_thrust = False d.limit_rp = False elif data.assistedControl: for d in self._selected_mux.devices(): d.limit_thrust = True d.limit_rp = False else: for d in self._selected_mux.devices(): d.limit_thrust = True d.limit_rp = True if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_ALTHOLD: self.assisted_control_updated.call( data.assistedControl) if ((self._assisted_control == JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD) or (self._assisted_control == JoystickReader.ASSISTED_CONTROL_HOVER)): try: self.assisted_control_updated.call( data.assistedControl) if not data.assistedControl: # Reset height controller state to initial # target height both in the UI and in the # Crazyflie. # TODO: Implement a proper state update of the # input layer self.heighthold_input_updated.\ call(0, 0, 0, INITAL_TAGET_HEIGHT) self.hover_input_updated.\ call(0, 0, 0, INITAL_TAGET_HEIGHT) except Exception as e: logger.warning( "Exception while doing callback from " "input-device for assited " "control: {}".format(e)) if data.toggled.estop: try: self.emergency_stop_updated.call(data.estop) except Exception as e: logger.warning("Exception while doing callback from" "input-device for estop: {}".format(e)) if data.toggled.alt1: try: self.alt1_updated.call(data.alt1) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt1: {}".format(e)) if data.toggled.alt2: try: self.alt2_updated.call(data.alt2) except Exception as e: logger.warning("Exception while doing callback from" "input-device for alt2: {}".format(e)) # Reset height target when height-hold is not selected if not data.assistedControl or \ (self._assisted_control != JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD and self._assisted_control != JoystickReader.ASSISTED_CONTROL_HOVER): self._target_height = INITAL_TAGET_HEIGHT if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_POSHOLD \ and data.assistedControl: vx = data.roll vy = data.pitch vz = data.thrust yawrate = data.yaw # The odd use of vx and vy is to map forward on the # physical joystick to positiv X-axis self.assisted_input_updated.call(vy, -vx, vz, yawrate) elif self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HOVER \ and data.assistedControl: vx = data.roll vy = data.pitch # Scale thrust to a value between -1.0 to 1.0 vz = (data.thrust - 32767) / 32767.0 # Integrate velosity setpoint self._target_height += vz * INPUT_READ_PERIOD # Cap target height if self._target_height > self._hover_max_height: self._target_height = self._hover_max_height if self._target_height < MIN_HOVER_HEIGHT: self._target_height = MIN_HOVER_HEIGHT yawrate = data.yaw # The odd use of vx and vy is to map forward on the # physical joystick to positiv X-axis self.hover_input_updated.call(vy, -vx, yawrate, self._target_height) else: # Update the user roll/pitch trim from device if data.toggled.pitchNeg and data.pitchNeg: self.trim_pitch -= .2 if data.toggled.pitchPos and data.pitchPos: self.trim_pitch += .2 if data.toggled.rollNeg and data.rollNeg: self.trim_roll -= .2 if data.toggled.rollPos and data.rollPos: self.trim_roll += .2 if data.toggled.pitchNeg or data.toggled.pitchPos or \ data.toggled.rollNeg or data.toggled.rollPos: self.rp_trim_updated.call(self.trim_roll, self.trim_pitch) if self._assisted_control == \ JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD \ and data.assistedControl: roll = data.roll + self.trim_roll pitch = data.pitch + self.trim_pitch yawrate = data.yaw # Scale thrust to a value between -1.0 to 1.0 vz = (data.thrust - 32767) / 32767.0 # Integrate velosity setpoint self._target_height += vz * INPUT_READ_PERIOD # Cap target height if self._target_height > self._hover_max_height: self._target_height = self._hover_max_height if self._target_height < MIN_TARGET_HEIGHT: self._target_height = MIN_TARGET_HEIGHT self.heighthold_input_updated.call( roll, -pitch, yawrate, self._target_height) else: # Using alt hold the data is not in a percentage if not data.assistedControl: data.thrust = JoystickReader.p2t(data.thrust) # Thrust might be <0 here, make sure it's not otherwise # we'll get an error. if data.thrust < 0: data.thrust = 0 if data.thrust > 0xFFFF: data.thrust = 0xFFFF self.input_updated.call(data.roll + self.trim_roll, data.pitch + self.trim_pitch, data.yaw, data.thrust) else: self.input_updated.call(0, 0, 0, 0) except Exception: logger.warning("Exception while reading inputdevice: %s", traceback.format_exc()) self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc()) self.input_updated.call(0, 0, 0, 0) self._read_timer.stop() @staticmethod def p2t(percentage): """Convert a percentage to raw thrust""" return int(MAX_THRUST * (percentage / 100.0)) thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class LogConfig(object): """Representation of one log configuration that enables logging from the Crazyflie""" # Maximum log payload length (4 bytes are used for block id and timestamp) MAX_LEN = 26 def __init__(self, name, period_in_ms): """Initialize the entry""" self.data_received_cb = Caller() self.error_cb = Caller() self.started_cb = Caller() self.added_cb = Caller() self.err_no = 0 # These 3 variables are set by the log subsystem when the bock is added self.id = 0 self.cf = None self.useV2 = False self.period = int(period_in_ms / 10) self.period_in_ms = period_in_ms self._added = False self._started = False self.pending = False self.valid = False self.variables = [] self.default_fetch_as = [] self.name = name def add_variable(self, name, fetch_as=None): """Add a new variable to the configuration. name - Complete name of the variable in the form group.name fetch_as - String representation of the type the variable should be fetched as (i.e uint8_t, float, FP16, etc) If no fetch_as type is supplied, then the stored as type will be used (i.e the type of the fetched variable is the same as it's stored in the Crazyflie).""" if fetch_as: self.variables.append(LogVariable(name, fetch_as)) else: # We cannot determine the default type until we have connected. So # save the name and we will add these once we are connected. self.default_fetch_as.append(name) def add_memory(self, name, fetch_as, stored_as, address): """Add a raw memory position to log. name - Arbitrary name of the variable fetch_as - String representation of the type of the data the memory should be fetch as (i.e uint8_t, float, FP16) stored_as - String representation of the type the data is stored as in the Crazyflie address - The address of the data """ self.variables.append( LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as, address)) def _set_added(self, added): if added != self._added: self.added_cb.call(self, added) self._added = added def _get_added(self): return self._added def _set_started(self, started): if started != self._started: self.started_cb.call(self, started) self._started = started def _get_started(self): return self._started added = property(_get_added, _set_added) started = property(_get_started, _set_started) def _cmd_create_block(self): if self.useV2: return CMD_CREATE_BLOCK_V2 else: return CMD_CREATE_BLOCK def _cmd_append_block(self): if self.useV2: return CMD_APPEND_BLOCK_V2 else: return CMD_APPEND_BLOCK def _setup_log_elements(self, pk, next_to_add): i = next_to_add for i in range(next_to_add, len(self.variables)): var = self.variables[i] if (var.is_toc_variable() is False): # Memory location logger.debug('Logging to raw memory %d, 0x%04X', var.get_storage_and_fetch_byte(), var.address) pk.data.append( struct.pack('<B', var.get_storage_and_fetch_byte())) pk.data.append(struct.pack('<I', var.address)) else: # Item in TOC element_id = self.cf.log.toc.get_element_id(var.name) logger.debug('Adding %s with id=%d and type=0x%02X', var.name, element_id, var.get_storage_and_fetch_byte()) pk.data.append(var.get_storage_and_fetch_byte()) if self.useV2: size_to_add = 2 if pk.available_data_size() >= size_to_add: pk.data.append(element_id & 0x0ff) pk.data.append((element_id >> 8) & 0x0ff) else: # Packet is full return False, i else: pk.data.append(element_id) return True, i def create(self): """Save the log configuration in the Crazyflie""" command = self._cmd_create_block() next_to_add = 0 is_done = False num_variables = 0 pending = 0 for block in self.cf.log.log_blocks: if block.pending or block.added or block.started: pending += 1 num_variables += len(block.variables) if pending < Log.MAX_BLOCKS: # # The Crazyflie firmware can only handle 128 variables before # erroring out with ENOMEM. # if num_variables + len(self.variables) > Log.MAX_VARIABLES: raise AttributeError( ('Adding this configuration would exceed max number ' 'of variables (%d)' % Log.MAX_VARIABLES)) else: raise AttributeError( 'Configuration has max number of blocks (%d)' % Log.MAX_BLOCKS) self.pending += 1 while not is_done: pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (command, self.id) is_done, next_to_add = self._setup_log_elements(pk, next_to_add) logger.debug('Adding/appending log block id {}'.format(self.id)) self.cf.send_packet(pk, expected_reply=(command, self.id)) # Use append if we have to add more variables command = self._cmd_append_block() def start(self): """Start the logging for this entry""" if (self.cf.link is not None): if (self._added is False): self.create() logger.debug('First time block is started, add block') else: logger.debug( 'Block already registered, starting logging' ' for id=%d', self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, self.id, self.period) self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id)) def stop(self): """Stop the logging for this entry""" if (self.cf.link is not None): if (self.id is None): logger.warning('Stopping block, but no block registered') else: logger.debug('Sending stop logging for block id=%d', self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_STOP_LOGGING, self.id) self.cf.send_packet(pk, expected_reply=(CMD_STOP_LOGGING, self.id)) def delete(self): """Delete this entry in the Crazyflie""" if (self.cf.link is not None): if (self.id is None): logger.warning('Delete block, but no block registered') else: logger.debug( 'LogEntry: Sending delete logging for block id=%d' % self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.id) self.cf.send_packet(pk, expected_reply=(CMD_DELETE_BLOCK, self.id)) def unpack_log_data(self, log_data, timestamp): """Unpack received logging data so it represent real values according to the configuration in the entry""" ret_data = {} data_index = 0 for var in self.variables: size = LogTocElement.get_size_from_id(var.fetch_as) name = var.name unpackstring = LogTocElement.get_unpack_string_from_id( var.fetch_as) value = struct.unpack(unpackstring, log_data[data_index:data_index + size])[0] data_index += size ret_data[name] = value self.data_received_cb.call(timestamp, ret_data, self)
class Log(): """Create log configuration""" # These codes can be decoded using os.stderror, but # some of the text messages will look very stange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: "No more memory available", errno.ENOEXEC: "Command not found", errno.ENOENT: "No such block id", errno.E2BIG: "Block too large" } def __init__(self, crazyflie=None): self.log_blocks = [] # Called with newly created blocks self.block_added_cb = Caller() self.cf = crazyflie self.toc = None self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb) self.toc_updated = Caller() self.state = IDLE self.fake_toc_crc = 0xDEADBEEF def add_config(self, logconf): """Add a log configuration to the logging framework. When doing this the contents of the log configuration will be validated and listeners for new log configurations will be notified. When validating the configuration the variables are checked against the TOC to see that they actually exist. If they don't then the configuration cannot be used. Since a valid TOC is required, a Crazyflie has to be connected when calling this method, otherwise it will fail.""" if not self.cf.link: logger.error("Cannot add configs without being connected to a " "Crazyflie!") return # If the log configuration contains variables that we added without # type (i.e we want the stored as type for fetching as well) then # resolve this now and add them to the block again. for name in logconf.default_fetch_as: var = self.toc.get_element_by_complete_name(name) if not var: logger.warning("%s not in TOC, this block cannot be" " used!", name) logconf.valid = False return # Now that we know what type this variable has, add it to the log # config again with the correct type logconf.add_variable(name, var.ctype) # Now check that all the added variables are in the TOC and that # the total size constraint of a data packet with logging data is # not size = 0 for var in logconf.variables: size += LogTocElement.get_size_from_id(var.fetch_as) # Check that we are able to find the variable in the TOC so # we can return error already now and not when the config is sent if var.is_toc_variable(): if (self.toc.get_element_by_complete_name( var.name) is None): logger.warning("Log: %s not in TOC, this block cannot be" " used!", var.name) logconf.valid = False return if (size <= MAX_LOG_DATA_PACKET_SIZE and (logconf.period > 0 and logconf.period < 0xFF)): logconf.valid = True logconf.cf = self.cf self.log_blocks.append(logconf) self.block_added_cb.call(logconf) else: logconf.valid = False def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING, ) self.cf.send_packet(pk, expect_answer=True) self.log_blocks = [] self.toc = Toc() toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _find_block(self, id): for block in self.log_blocks: if block.id == id: return block return None def _new_packet_cb(self, packet): """Callback for newly arrived packets with TOC information""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) if (chan == CHAN_SETTINGS): id = ord(payload[0]) error_status = ord(payload[1]) block = self._find_block(id) if (cmd == CMD_CREATE_BLOCK): if (block is not None): if (error_status == 0): # No error logger.debug("Have successfully added id=%d", id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, id, block.period) self.cf.send_packet(pk, expect_answer=True) block.added = True else: msg = self._err_codes[error_status] logger.warning("Error %d when adding id=%d (%s)" , error_status, id, msg) block.err_no = error_status block.added_cb.call(False) block.error_cb.call(block, msg) else: logger.warning("No LogEntry to assign block to !!!") if (cmd == CMD_START_LOGGING): if (error_status == 0x00): logger.info("Have successfully started logging for block=%d", id) if block: block.started = True else: msg = self._err_codes[error_status] logger.warning("Error %d when starting id=%d (%s)" , error_status, id, msg) if block: block.err_no = error_status block.started_cb.call(False) block.error_cb.call(block, msg) if (cmd == CMD_STOP_LOGGING): if (error_status == 0x00): logger.info("Have successfully stopped logging for block=%d", id) if block: block.started = False if (cmd == CMD_DELETE_BLOCK): if (error_status == 0x00): logger.info("Have successfully deleted block=%d", id) if block: block.started = False block.added = False if (chan == CHAN_LOGDATA): chan = packet.channel id = packet.datal[0] block = self._find_block(id) timestamps = struct.unpack("<BBB", packet.data[1:4]) timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16) logdata = packet.data[4:] if (block is not None): block.unpack_log_data(logdata, timestamp) else: logger.warning("Error no LogEntry to handle id=%d", id)
class Log(): """Create log configuration""" # These codes can be decoded using os.stderror, but # some of the text messages will look very stange # in the UI, so they are redefined here _err_codes = { errno.ENOMEM: "No more memory available", errno.ENOEXEC: "Command not found", errno.ENOENT: "No such block id", errno.E2BIG: "Block too large" } def __init__(self, crazyflie=None): self.log_blocks = [] # Called with newly created blocks self.block_added_cb = Caller() self.cf = crazyflie self.toc = None self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb) self.toc_updated = Caller() self.state = IDLE self.fake_toc_crc = 0xDEADBEEF def create_log_packet(self, logconf): """Create a new log configuration""" size = 0 period = logconf.getPeriod() / 10 for var in logconf.getVariables(): size += LogTocElement.get_size_from_id(var.getFetchAs()) # Check that we are able to find the variable in the TOC so # we can return error already now and not when the config is sent if (var.isTocVariable()): if (self.toc.get_element_by_complete_name( var.getName()) is None): logger.warning("Log: %s not in TOC, this block cannot be" " used!", var.getName()) return None if (size <= MAX_LOG_DATA_PACKET_SIZE and period > 0 and period < 0xFF): block = LogEntry(self.cf, logconf) self.log_blocks.append(block) self.block_added_cb.call(block) return block else: return None def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" pk = CRTPPacket() pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS) pk.data = (CMD_RESET_LOGGING, ) self.cf.send_packet(pk) self.log_blocks = [] self.toc = Toc() toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _find_block(self, block_id): for block in self.log_blocks: if block.block_id == block_id: return block return None def _new_packet_cb(self, packet): """Callback for newly arrived packets with TOC information""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) if (chan == CHAN_SETTINGS): block_id = ord(payload[0]) error_status = ord(payload[1]) block = self._find_block(block_id) if (cmd == CMD_CREATE_BLOCK): if (block is not None): if (error_status == 0): # No error logger.debug("Have successfully added block_id=%d", block_id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_START_LOGGING, block_id, block.period) self.cf.send_packet(pk) block.added = True else: msg = self._err_codes[error_status] logger.warning("Error %d when adding block_id=%d (%s)" , error_status, block_id, msg) block.err_no = error_status block.added_cb.call(False) block.error.call(block.logconf, msg) else: logger.warning("No LogEntry to assign block to !!!") if (cmd == CMD_START_LOGGING): if (error_status == 0x00): logger.info("Have successfully logging for block=%d", block_id) if block: block.started = True else: msg = self._err_codes[error_status] logger.warning("Error %d when starting block_id=%d (%s)" , error_status, new_block_id, msg) if block: block.err_no = error_status block.started_cb.call(False) block.error.call(block.logconf, msg) if (chan == CHAN_LOGDATA): chan = packet.channel block_id = packet.datal[0] block = self._find_block(block_id) timestamps = struct.unpack("<BBB", packet.data[1:4]) timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16) logdata = packet.data[4:] if (block is not None): block.unpack_log_data(logdata, timestamp) else: logger.warning("Error no LogEntry to handle block=%d", block_id)