class JoystickReader(QThread):
"""
Thread that will read input from devices/joysticks and send control-set
ponts 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):
QThread.__init__(self)
# self.moveToThread(self)
# TODO: Should be OS dependant
print "setting input.py inputdevice"
self.inputdevice = PyGameReader()
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()
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)
update_trim_yaw_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)
inputDeviceErrorSignal = pyqtSignal('QString')
sendControlSetpointSignal = pyqtSignal(float, float, float, int)
discovery_signal = pyqtSignal(object)
inputConfig = []
def __init__(self):
QThread.__init__(self)
#self.moveToThread(self)
# TODO: Should be OS dependant
self.inputdevice = PyGameReader()
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.update_trim_yaw_signal.connect(self._update_trim_yaw)
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.oldThrust = 0
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
self._trim_yaw = 0.0
# 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(10);
self.connect(self._discovery_timer, SIGNAL("timeout()"), self._do_device_discovery)
self._discovery_timer.start()
self.listOfConfigs = []
self._available_devices = {}
# Check if user config exists, otherwise copy files
if (not os.path.isdir(sys.path[1] + "/input")):
logger.info("No user config found, copying dist files")
os.makedirs(sys.path[1] + "/input")
for f in glob.glob(sys.path[0] + "/cfclient/configs/input/[A-Za-z]*.json"):
shutil.copy2(f, sys.path[1] + "/input")
try:
configsfound = [ os.path.basename(f) for f in glob.glob(sys.path[1] + "/input/[A-Za-z]*.json")]
self.inputConfig = []
for conf in configsfound:
logger.info("Parsing [%s]", conf)
json_data = open (sys.path[1] + "/input/%s"%conf)
self.data = json.load(json_data)
newInputDevice = {}
for a in self.data["inputconfig"]["inputdevice"]["axis"]:
axis = {}
axis["scale"] = a["scale"]
axis["type"] = a["type"]
axis["key"] = a["key"]
axis["name"] = a["name"]
try:
ids = a["ids"]
except:
ids = [a["id"]]
for id in ids:
locaxis = copy.deepcopy(axis)
if "ids" in a:
if id == a["ids"][0]:
locaxis["scale"] = locaxis["scale"] * -1
locaxis["id"] = id
index = "%s-%d" % (a["type"], id) # 'type'-'id' defines unique index for axis
newInputDevice[index] = locaxis
self.inputConfig.append(newInputDevice)
json_data.close()
self.listOfConfigs.append(conf[:-5])
except Exception as e:
logger.warning("Exception while parsing inputconfig file: %s ", e)
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 getConfig(self, configName):
"""Get the configuratio for an input device."""
try:
idx = self.listOfConfigs.index(configName)
return self.inputConfig[idx]
except:
return None
def getListOfConfigs(self):
"""Get a list of all the input devices."""
return self.listOfConfigs
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, configName):
"""Start reading inpyt from the device with id deviceId using config configName"""
try:
idx = self.listOfConfigs.index(configName)
device_id = self._available_devices[device_name]
self.inputdevice.startInput(device_id, self.inputConfig[idx])
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(float)
def _update_trim_yaw(self, trim_yaw):
"""Set a new value for the roll trim."""
self._trim_yaw = trim_yaw
@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"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
# Thust limiting (slew, minimum and emergency stop)
if (raw_thrust<0.05 or data["estop"] == True):
thrust=0
else:
thrust = self.minThrust + thrust * (self.maxThrust - self.minThrust)
if (self.thrustSlewEnabled == True and self.slewEnableLimit > thrust and data["estop"] == False):
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
yaw = yaw + self._trim_yaw
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
#print 'send control', trimmed_rol, trimmed_pitch,yaw, thrust
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()
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):
QThread.__init__(self)
#self.moveToThread(self)
# TODO: Should be OS dependant
self.inputdevice = PyGameReader()
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()
print "---> devs:", devs
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()