def __init__(self,cf=None):
QThread.__init__(self)
#self.moveToThread(self)
# TODO: Should be OS dependant
##### self.inputdevice = PyGameReader()
self.inputdevice = AiController(cf)
self.startInputSignal.connect(self.startInput)
self.stopInputSignal.connect(self.stopInput)
self.updateMinMaxThrustSignal.connect(self.updateMinMaxThrust)
self.update_trim_roll_signal.connect(self._update_trim_roll)
self.update_trim_pitch_signal.connect(self._update_trim_pitch)
self.updateMaxRPAngleSignal.connect(self.updateMaxRPAngle)
self.updateThrustLoweringSlewrateSignal.connect(self.updateThrustLoweringSlewrate)
self.updateMaxYawRateSignal.connect(self.updateMaxYawRate)
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 = QTimer()
self.readTimer.setInterval(10);
self.connect(self.readTimer, SIGNAL("timeout()"), self.readInput)
self._discovery_timer = QTimer()
self._discovery_timer.setInterval(1000);
self.connect(self._discovery_timer, SIGNAL("timeout()"), 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)
def __init__(self, do_device_discovery=True, cf=None):
# TODO: Should be OS dependant
self.inputdevice = AiController(cf)#PyGameReader()
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_alt_hold = False
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(
Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("min_thrust"), Config().get("max_thrust"))
self.set_thrust_slew_limiting(
Config().get("slew_rate"), Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
class JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True, 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)
def __init__(self, do_device_discovery=True, cf=None):
# TODO: Should be OS dependant
self.inputdevice = AiController(cf)
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_alt_hold = False
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("min_thrust"),
Config().get("max_thrust"))
self.set_thrust_slew_limiting(Config().get("slew_rate"),
Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
class JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True, cf=None):
# TODO: Should be OS dependant
self.inputdevice = AiController(cf)
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_alt_hold = False
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("min_thrust"),
Config().get("max_thrust"))
self.set_thrust_slew_limiting(Config().get("slew_rate"),
Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
def setAltHoldAvailable(self, available):
self._has_pressure_sensor = available
def setAltHold(self, althold):
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = self.inputdevice.getAvailableDevices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev["name"]))):
self._available_devices[dev["name"]] = dev["id"]
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self._read_timer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self._read_timer.stop()
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
self._max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
self._max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if (thrust_slew_rate > 0):
self._thrust_slew_enabled = True
else:
self._thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
self._min_thrust = JoystickReader.p2t(min_thrust)
self._max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def read_input(self):
"""Read input data from the selected device"""
try:
data = self.inputdevice.read_input()
roll = data["roll"] #* self._max_rp_angle
pitch = data["pitch"] #* self._max_rp_angle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
althold = data["althold"]
#Deadband the roll and pitch
roll = JoystickReader.deadband(roll, 0.2) * self._max_rp_angle
pitch = JoystickReader.deadband(pitch, 0.2) * self._max_rp_angle
if (self._old_alt_hold != althold):
self.althold_updated.call(str(althold))
self._old_alt_hold = althold
if self._emergency_stop != emergency_stop:
self._emergency_stop = emergency_stop
self.emergency_stop_updated.call(self._emergency_stop)
# Thust limiting (slew, minimum and emergency stop)
if althold and self._has_pressure_sensor:
thrust = int(
round(
JoystickReader.deadband(thrust, 0.2) * 32767 +
32767)) #Convert to uint16
else:
if raw_thrust < 0.15 or emergency_stop:
thrust = 0
else:
thrust = self._min_thrust + thrust * (self._max_thrust -
self._min_thrust)
if (self._thrust_slew_enabled == True
and self._thrust_slew_limit > thrust
and not emergency_stop):
if self._old_thrust > self._thrust_slew_limit:
self._old_thrust = self._thrust_slew_limit
if thrust < (self._old_thrust -
(self._thrust_slew_rate / 100)):
thrust = self._old_thrust - self._thrust_slew_rate / 100
if raw_thrust < 0 or thrust < self._min_thrust:
thrust = 0
self._old_thrust = thrust
# Yaw deadband
# TODO: Add to input device config?
yaw = JoystickReader.deadband(yaw, 0.2) * self._max_yaw_rate
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value / (1 - threshold)
class JoystickReader(QThread):
"""Thread that will read input from devices/joysticks and send control-setponts to
the Crazyflie"""
PITCH_AXIS_ID = 0
ROLL_AXIS_ID = 1
YAW_AXIS_ID = 2
THRUST_AXIS_ID = 3
# Incomming signal to start input capture
startInputSignal = pyqtSignal(str, str)
# Incomming signal to stop input capture
stopInputSignal = pyqtSignal()
# Incomming signal to set min/max thrust
updateMinMaxThrustSignal = pyqtSignal(int, int)
# Incomming signal to set roll/pitch calibration
update_trim_roll_signal = pyqtSignal(float)
update_trim_pitch_signal = pyqtSignal(float)
# Incomming signal to set max roll/pitch angle
updateMaxRPAngleSignal = pyqtSignal(int)
# Incomming signal to set max yaw rate
updateMaxYawRateSignal = pyqtSignal(int)
# Incomming signal to set thrust lowering slew rate limiting
updateThrustLoweringSlewrateSignal = pyqtSignal(int, int)
# Configure the aixs: Axis id, joystick axis id and inverse or not
updateAxisConfigSignal = pyqtSignal(int, int, float)
# Start detection of variation for joystick axis
detectAxisVarSignal = pyqtSignal()
# Outgoing signal for device found
inputUpdateSignal = pyqtSignal(float, float, float, float)
# Outgoing signal for when pitch/roll calibration has been updated
calUpdateSignal = pyqtSignal(float, float)
# Outgoing signal for emergency stop
emergencyStopSignal = pyqtSignal(bool)
inputDeviceErrorSignal = pyqtSignal('QString')
sendControlSetpointSignal = pyqtSignal(float, float, float, int)
discovery_signal = pyqtSignal(object)
inputConfig = []
def __init__(self,cf=None):
QThread.__init__(self)
#self.moveToThread(self)
# TODO: Should be OS dependant
##### self.inputdevice = PyGameReader()
self.inputdevice = AiController(cf)
self.startInputSignal.connect(self.startInput)
self.stopInputSignal.connect(self.stopInput)
self.updateMinMaxThrustSignal.connect(self.updateMinMaxThrust)
self.update_trim_roll_signal.connect(self._update_trim_roll)
self.update_trim_pitch_signal.connect(self._update_trim_pitch)
self.updateMaxRPAngleSignal.connect(self.updateMaxRPAngle)
self.updateThrustLoweringSlewrateSignal.connect(self.updateThrustLoweringSlewrate)
self.updateMaxYawRateSignal.connect(self.updateMaxYawRate)
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 = QTimer()
self.readTimer.setInterval(10);
self.connect(self.readTimer, SIGNAL("timeout()"), self.readInput)
self._discovery_timer = QTimer()
self._discovery_timer.setInterval(1000);
self.connect(self._discovery_timer, SIGNAL("timeout()"), 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)
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.discovery_signal.emit(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()
# Fix for Ubuntu... doing self.moveToThread will not work without this
# since it seems that the default implementation of run will not call exec_ to process
# events.
def run(self):
self.exec_()
@pyqtSlot(str, str)
def startInput(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.startInput(device_id, ConfigManager().get_config(config_name))
self.readTimer.start()
except Exception:
self.inputDeviceErrorSignal.emit("Error while opening/initializing input device\n\n%s" % (traceback.format_exc()))
@pyqtSlot()
def stopInput(self):
"""Stop reading from the input device."""
self.readTimer.stop()
@pyqtSlot(int)
def updateMaxYawRate(self, maxRate):
"""Set a new max yaw rate value."""
self.maxYawRate = maxRate
@pyqtSlot(int)
def updateMaxRPAngle(self, maxAngle):
"""Set a new max roll/pitch value."""
self.maxRPAngle = maxAngle
@pyqtSlot(int, int)
def updateThrustLoweringSlewrate(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 setCrazyflie(self, cf):
"""Set the referance for the Crazyflie"""
self.cf = cf
@pyqtSlot(int, int)
def updateMinMaxThrust(self, minThrust, maxThrust):
"""Set a new min/max thrust limit."""
self.minThrust = minThrust
self.maxThrust = maxThrust
@pyqtSlot(float)
def _update_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
@pyqtSlot(float)
def _update_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
@pyqtSlot()
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.emergencyStopSignal.emit(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.calUpdateSignal.emit(self._trim_roll, self._trim_pitch)
self.inputUpdateSignal.emit(roll, pitch, yaw, thrust)
trimmed_rol = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
self.sendControlSetpointSignal.emit(trimmed_rol, trimmed_pitch,
yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s", traceback.format_exc())
self.inputDeviceErrorSignal.emit("Error reading from input device\n\n%s"%traceback.format_exc())
self.readTimer.stop()
