Example #1
0
    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)
Example #2
0
    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)
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()
    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)
Example #5
0
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()
Example #6
0
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()