diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 1fea772..863866b 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -42,7 +42,6 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _rgAxisValues(NULL) , _rgCalibration(NULL) , _rgButtonValues(NULL) - , _rgButtonActions(NULL) , _lastButtonBits(0) , _throttleMode(ThrottleModeCenterZero) , _activeVehicle(NULL) @@ -53,7 +52,6 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC _rgAxisValues = new int[_axisCount]; _rgCalibration = new Calibration_t[_axisCount]; _rgButtonValues = new bool[_totalButtonCount]; - _rgButtonActions = new QString[_totalButtonCount]; for (int i=0; i<_axisCount; i++) { _rgAxisValues[i] = 0; @@ -61,7 +59,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC for (int i=0; i<_totalButtonCount; i++) { _rgButtonValues[i] = false; } - + _loadSettings(); } @@ -70,66 +68,65 @@ Joystick::~Joystick() delete _rgAxisValues; delete _rgCalibration; delete _rgButtonValues; - delete _rgButtonActions; } void Joystick::_loadSettings(void) { QSettings settings; - + settings.beginGroup(_settingsGroup); settings.beginGroup(_name); - + bool badSettings = false; bool convertOk; - + qCDebug(JoystickLog) << "_loadSettings " << _name; - + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); - + _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk); badSettings |= !convertOk; - + qCDebug(JoystickLog) << "_loadSettings calibrated:throttlemode:badsettings" << _calibrated << _throttleMode << badSettings; - + QString minTpl ("Axis%1Min"); QString maxTpl ("Axis%1Max"); QString trimTpl ("Axis%1Trim"); QString revTpl ("Axis%1Rev"); - + for (int axis=0; axis<_axisCount; axis++) { Calibration_t* calibration = &_rgCalibration[axis]; - + calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk); badSettings |= !convertOk; - + calibration->min = settings.value(minTpl.arg(axis), -32768).toInt(&convertOk); badSettings |= !convertOk; - + calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk); badSettings |= !convertOk; - + calibration->reversed = settings.value(revTpl.arg(axis), false).toBool(); - + qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << badSettings; } - + for (int function=0; functioncenter); settings.setValue(minTpl.arg(axis), calibration->min); settings.setValue(maxTpl.arg(axis), calibration->max); settings.setValue(revTpl.arg(axis), calibration->reversed); - + qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed" << _name << axis @@ -169,12 +166,12 @@ void Joystick::_saveSettings(void) << calibration->center << calibration->reversed; } - + for (int function=0; function calibration.center) { axisBasis = 1.0f; valueNormalized = value - calibration.center; @@ -197,15 +194,15 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) valueNormalized = calibration.center - value; axisLength = calibration.center - calibration.min; } - + float axisPercent = valueNormalized / axisLength; - + float correctedValue = axisBasis * axisPercent; - + if (calibration.reversed) { correctedValue *= -1.0f; } - + #if 0 qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:basis:normalized:length" << correctedValue @@ -226,7 +223,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration) void Joystick::run(void) { _open(); - + while (!_exitThread) { _update(); @@ -237,7 +234,7 @@ void Joystick::run(void) _rgAxisValues[axisIndex] = newAxisValue; emit rawAxisValueChanged(axisIndex, newAxisValue); } - + // Update buttons for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) { bool newButtonValue = _getButton(buttonIndex); @@ -261,14 +258,14 @@ void Joystick::run(void) } } } - + if (_calibrationMode != CalibrationModeCalibrating) { int axis = _rgFunctionAxis[rollFunction]; float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]); - + axis = _rgFunctionAxis[pitchFunction]; float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]); - + axis = _rgFunctionAxis[yawFunction]; float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]); @@ -285,28 +282,28 @@ void Joystick::run(void) pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); - + // Adjust throttle to 0:1 range if (_throttleMode == ThrottleModeCenterZero) { throttle = std::max(0.0f, throttle); - } else { + } else { throttle = (throttle + 1.0f) / 2.0f; } - + // Set up button pressed information - + // We only send the buttons the firmwware has reserved int reservedButtonCount = _activeVehicle->manualControlReservedButtonCount(); if (reservedButtonCount == -1) { reservedButtonCount = _totalButtonCount; } - + quint16 newButtonBits = 0; // New set of button which are down quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal - + for (int buttonIndex=0; buttonIndex<_totalButtonCount; buttonIndex++) { quint16 buttonBit = 1 << buttonIndex; - + if (!_rgButtonValues[buttonIndex]) { // Button up, just record it newButtonBits |= buttonBit; @@ -314,7 +311,7 @@ void Joystick::run(void) if (_lastButtonBits & buttonBit) { // Button was up last time through, but is now down which indicates a button press qCDebug(JoystickLog) << "button triggered" << buttonIndex; - + if (buttonIndex >= reservedButtonCount) { // Button is above firmware reserved set QString buttonAction =_rgButtonActions[buttonIndex]; @@ -328,18 +325,18 @@ void Joystick::run(void) buttonPressedBits |= buttonBit; } } - + _lastButtonBits = newButtonBits; - + qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle; emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); } - + // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms) QGC::SLEEP::msleep(40); } - + _close(); } @@ -385,7 +382,7 @@ void Joystick::stopPolling(void) } // FIXME: **** //disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); - + _exitThread = true; } } @@ -396,7 +393,7 @@ void Joystick::setCalibration(int axis, Calibration_t& calibration) qCWarning(JoystickLog) << "Invalid axis index" << axis; return; } - + _calibrated = true; _rgCalibration[axis] = calibration; _saveSettings(); @@ -408,7 +405,7 @@ Joystick::Calibration_t Joystick::getCalibration(int axis) if (!_validAxis(axis)) { qCWarning(JoystickLog) << "Invalid axis index" << axis; } - + return _rgCalibration[axis]; } @@ -443,7 +440,7 @@ QStringList Joystick::actions(void) if (_activeVehicle) { list << _activeVehicle->flightModes(); } - + return list; } @@ -453,9 +450,9 @@ void Joystick::setButtonAction(int button, const QString& action) qCWarning(JoystickLog) << "Invalid button index" << button; return; } - + qDebug() << "setButtonAction" << action; - + _rgButtonActions[button] = action; _saveSettings(); emit buttonActionsChanged(buttonActions()); @@ -466,18 +463,18 @@ QString Joystick::getButtonAction(int button) if (!_validButton(button)) { qCWarning(JoystickLog) << "Invalid button index" << button; } - + return _rgButtonActions[button]; } QVariantList Joystick::buttonActions(void) { QVariantList list; - + for (int button=0; button<_buttonCount; button++) { list += QVariant::fromValue(_rgButtonActions[button]); } - + return list; } @@ -492,7 +489,7 @@ void Joystick::setThrottleMode(int mode) qCWarning(JoystickLog) << "Invalid throttle mode" << mode; return; } - + _throttleMode = (ThrottleMode_t)mode; _saveSettings(); emit throttleModeChanged(_throttleMode); @@ -504,9 +501,9 @@ void Joystick::startCalibrationMode(CalibrationMode_t mode) qWarning() << "Incorrect mode CalibrationModeOff"; return; } - + _calibrationMode = mode; - + if (!isRunning()) { _pollingStartedForCalibration = true; startPolling(_multiVehicleManager->activeVehicle()); @@ -519,7 +516,7 @@ void Joystick::stopCalibrationMode(CalibrationMode_t mode) qWarning() << "Incorrect mode: CalibrationModeOff"; return; } - + if (mode == CalibrationModeCalibrating) { _calibrationMode = CalibrationModeMonitor; } else { diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 64ec433..ecc57c5 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -24,7 +24,7 @@ Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog) class Joystick : public QThread { Q_OBJECT - + public: Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager); @@ -36,7 +36,7 @@ public: int center; bool reversed; } Calibration_t; - + typedef enum { rollFunction, pitchFunction, @@ -44,76 +44,76 @@ public: throttleFunction, maxFunction } AxisFunction_t; - + typedef enum { ThrottleModeCenterZero, ThrottleModeDownZero, ThrottleModeMax } ThrottleMode_t; - + Q_PROPERTY(QString name READ name CONSTANT) - + Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) - + Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) Q_PROPERTY(int axisCount READ axisCount CONSTANT) - + Q_PROPERTY(QStringList actions READ actions CONSTANT) - + Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged) Q_INVOKABLE void setButtonAction(int button, const QString& action); Q_INVOKABLE QString getButtonAction(int button); - + Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) // Property accessors int axisCount(void) { return _axisCount; } int totalButtonCount(void) { return _totalButtonCount; } - + /// Start the polling thread which will in turn emit joystick signals void startPolling(Vehicle* vehicle); void stopPolling(void); - + void setCalibration(int axis, Calibration_t& calibration); Calibration_t getCalibration(int axis); - + void setFunctionAxis(AxisFunction_t function, int axis); int getFunctionAxis(AxisFunction_t function); - + QStringList actions(void); QVariantList buttonActions(void); - + QString name(void) { return _name; } - + int throttleMode(void); void setThrottleMode(int mode); - + typedef enum { CalibrationModeOff, // Not calibrating CalibrationModeMonitor, // Monitors are active, continue to send to vehicle if already polling CalibrationModeCalibrating, // Calibrating, stop sending joystick to vehicle } CalibrationMode_t; - + /// Set the current calibration mode void startCalibrationMode(CalibrationMode_t mode); - + /// Clear the current calibration mode void stopCalibrationMode(CalibrationMode_t mode); - + signals: void calibratedChanged(bool calibrated); - + // The raw signals are only meant for use by calibration void rawAxisValueChanged(int index, int value); void rawButtonPressedChanged(int index, int pressed); - + void buttonActionsChanged(QVariantList actions); - + void throttleModeChanged(int mode); - + void enabledChanged(bool enabled); - + /// Signal containing new joystick information /// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up @@ -121,9 +121,9 @@ signals: /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle /// @param mode See Vehicle::JoystickMode_t enum void manualControl(float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode); - + void buttonActionTriggered(int action); - + protected: void _saveSettings(void); void _loadSettings(void); @@ -145,9 +145,9 @@ private: virtual void run(void); protected: - + bool _exitThread; ///< true: signal thread to exit - + QString _name; bool _calibrated; int _axisCount; @@ -155,24 +155,24 @@ protected: int _hatCount; int _hatButtonCount; int _totalButtonCount; - + CalibrationMode_t _calibrationMode; - + int* _rgAxisValues; Calibration_t* _rgCalibration; int _rgFunctionAxis[maxFunction]; - + bool* _rgButtonValues; - QString* _rgButtonActions; + QStringList _rgButtonActions; quint16 _lastButtonBits; - + ThrottleMode_t _throttleMode; - + Vehicle* _activeVehicle; bool _pollingStartedForCalibration; MultiVehicleManager* _multiVehicleManager; - + private: static const char* _rgFunctionSettingsKey[maxFunction]; @@ -181,5 +181,5 @@ private: static const char* _buttonActionSettingsKey; static const char* _throttleModeSettingsKey; }; - + #endif