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(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:
"""
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:
"""
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 = []
def __init__(self, do_device_discovery=True):
self._input_device = None
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self.max_rp_angle = 0
self.max_yaw_rate = 0
self._old_thrust = 0
self._old_raw_thrust = 0
self._old_alt_hold = False
self.springy_throttle = True
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._input_map = None
self._mux = [
NoMux(self),
TakeOverSelectiveMux(self),
TakeOverMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(cfclient.module_path +
"/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def enable_alt_hold(self, althold):
"""Enable or disable altitude hold"""
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons,
mapped_values] = self._input_device.read(include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._input_map = ConfigManager().get_config(input_map_name)
self._get_device_from_name(device_name).input_map = self._input_map
self._get_device_from_name(device_name).input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp, device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.althold:
try:
self.althold_updated.call(str(data.althold))
except Exception as e:
logger.warning(
"Exception while doing callback from input-device "
"for althold: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= 1
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += 1
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= 1
if data.toggled.rollPos and data.rollPos:
self.trim_roll += 1
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll, self.trim_pitch)
# Thrust might be <0 here, make sure it's not otherwise we'll
# get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
# If we are using alt hold the data is not in a percentage
if not data.althold:
data.thrust = JoystickReader.p2t(data.thrust)
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch, data.yaw,
data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
self._input_device = None
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self.max_rp_angle = 0
self.max_yaw_rate = 0
self._old_thrust = 0
self._old_raw_thrust = 0
self._old_alt_hold = False
self.springy_throttle = True
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._input_map = None
self._mux = [NoMux(self), TakeOverSelectiveMux(self),
TakeOverMux(self)]
# Set NoMux as default
self._selected_mux = self._mux[0]
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(
cfclient.module_path + "/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def enable_alt_hold(self, althold):
"""Enable or disable altitude hold"""
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist) or
(self._dev_blacklist and
not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons, mapped_values] = self._input_device.read(
include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._input_map = ConfigManager().get_config(input_map_name)
self._get_device_from_name(device_name).input_map = self._input_map
self._get_device_from_name(device_name).input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp,
device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.althold:
try:
self.althold_updated.call(str(data.althold))
except Exception as e:
logger.warning(
"Exception while doing callback from input-device "
"for althold: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= 1
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += 1
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= 1
if data.toggled.rollPos and data.rollPos:
self.trim_roll += 1
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll, self.trim_pitch)
# Thrust might be <0 here, make sure it's not otherwise we'll
# get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
# If we are using alt hold the data is not in a percentage
if not data.althold:
data.thrust = JoystickReader.p2t(data.thrust)
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch,
data.yaw, data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class 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.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)
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):
# 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()