def __init__(self, tabWidget, helper, *args): super(GpsTab, self).__init__(*args) self.setupUi(self) self.tabName = "GPS" self.menuName = "GPS" self.helper = helper self.tabWidget = tabWidget self.GPS = GpsPoller() self.GPS.start() self.gpsTimer = PeriodicTimer(1.0, self.updateCoords) self.gpsTimer.start() self.cf = helper.cf #Init the tree widget #self.logTree.setHeaderLabels(['Name', 'ID', 'Unpack', 'Storage']) self.cf.connectSetupFinished.add_callback(self.connectedSignal.emit) self.connectedSignal.connect(self.connected) self.updateDest.clicked.connect(self.destCoordsChanged) self.revertChanges.clicked.connect(self.destRevertChanges) # Clear the log TOC list when the Crazyflie is disconnected self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self.disconnected) mapHTML = showmap.generateHTML(29.71984,-95.398087,29.732395,-95.394824) self.webView.setHtml(mapHTML)
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 __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()
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 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)
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()
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(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 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): # 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 GpsTab(Tab, param_tab_class): connectedSignal = pyqtSignal(str) disconnectedSignal = pyqtSignal(str) destLatVal = 0 destLongVal = 0 def __init__(self, tabWidget, helper, *args): super(GpsTab, self).__init__(*args) self.setupUi(self) self.tabName = "GPS" self.menuName = "GPS" self.helper = helper self.tabWidget = tabWidget self.GPS = GpsPoller() self.GPS.start() self.gpsTimer = PeriodicTimer(1.0, self.updateCoords) self.gpsTimer.start() self.cf = helper.cf #Init the tree widget #self.logTree.setHeaderLabels(['Name', 'ID', 'Unpack', 'Storage']) self.cf.connectSetupFinished.add_callback(self.connectedSignal.emit) self.connectedSignal.connect(self.connected) self.updateDest.clicked.connect(self.destCoordsChanged) self.revertChanges.clicked.connect(self.destRevertChanges) # Clear the log TOC list when the Crazyflie is disconnected self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self.disconnected) mapHTML = showmap.generateHTML(29.71984,-95.398087,29.732395,-95.394824) self.webView.setHtml(mapHTML) @pyqtSlot('QString') def disconnected(self, linkname): self.curLat.value = 0 @pyqtSlot(str) def connected(self, linkURI): self.curLat.value = 0 def destCoordsChanged(self): self.destLatVal = self.destLat.text().toFloat()[0] self.destLongVal = self.destLong.text().toFloat()[0] latitude = self.GPS.getGpsd().fix.latitude longitude = self.GPS.getGpsd().fix.longitude url=""" http://maps.googleapis.com/maps/api/directions/json?origin={0},{1}&destination={2},{3}&sensor=false """.format(latitude,longitude,self.destLatVal,self.destLongVal) dirsResult=simplejson.load(urllib.urlopen(url)) dirsResult=dirsResult["routes"][0]["legs"][0]["steps"] for step in dirsResult: loc = step["end_location"] print "adding waypoint ", loc["lat"], ", ", loc["lng"] JoystickReader.controller.getDestinationCoords(loc["lat"], loc["lng"]) #latitude = 29.7198 #longitude = -95.398087 print("%0.5f"%self.destLatVal) print("%0.5f"%self.destLongVal) mapHTML = showmap.generateHTML(latitude,longitude,self.destLatVal,self.destLongVal) self.webView.setHtml(mapHTML) def destRevertChanges(self): self.destLat.setText(("%0.10f" % self.destLatVal)) self.destLong.setText(("%0.10f" % self.destLongVal)) def updateCoords(self): latitude = self.GPS.getGpsd().fix.latitude longitude = self.GPS.getGpsd().fix.longitude self.curLat.setText(("%0.10f" % latitude)) self.curLong.setText(("%0.10f" % longitude)) JoystickReader.controller.getCurrentCoords(latitude, longitude)
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()
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 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 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)
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()
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)
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()
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()