diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 3f0ba6aeb22ea2bead37025410237e5f6b698a4f..c21a843d076214130d4cca4f93ee45b216c11bf2 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -25,6 +25,7 @@ const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_exponentialSettingsKey = "Exponential"; const char* Joystick::_accumulatorSettingsKey = "Accumulator"; const char* Joystick::_deadbandSettingsKey = "Deadband"; +const char* Joystick::_circleCorrectionSettingsKey = "Circle_Correction"; const char* Joystick::_txModeSettingsKey = NULL; const char* Joystick::_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; @@ -64,6 +65,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC , _exponential(0) , _accumulator(false) , _deadband(false) + , _circleCorrection(false) , _activeVehicle(NULL) , _pollingStartedForCalibration(false) , _multiVehicleManager(multiVehicleManager) @@ -124,6 +126,7 @@ void Joystick::_setDefaultCalibration(void) { _exponential = 0; _accumulator = false; _deadband = false; + _circleCorrection = false; _throttleMode = ThrottleModeCenterZero; _calibrated = true; @@ -188,6 +191,7 @@ void Joystick::_loadSettings(void) _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); _deadband = settings.value(_deadbandSettingsKey, false).toBool(); + _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk); badSettings |= !convertOk; @@ -264,9 +268,10 @@ void Joystick::_saveSettings(void) settings.setValue(_exponentialSettingsKey, _exponential); settings.setValue(_accumulatorSettingsKey, _accumulator); settings.setValue(_deadbandSettingsKey, _deadband); + settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); settings.setValue(_throttleModeSettingsKey, _throttleMode); - qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _transmitterMode; + qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; QString minTpl ("Axis%1Min"); QString maxTpl ("Axis%1Max"); @@ -462,17 +467,19 @@ void Joystick::run(void) throttle = throttle_accu; } - float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); - float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); - float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); - float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); - - // Map from unit circle to linear range and limit - roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); - 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)); - + if ( _circleCorrection ) { + float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + + // Map from unit circle to linear range and limit + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + 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)); + } + if ( _exponential != 0 ) { // Exponential (0% to -50% range like most RC radios) //_exponential is set by a slider in joystickConfig.qml @@ -767,6 +774,19 @@ void Joystick::setDeadband(bool deadband) _saveSettings(); } +bool Joystick::circleCorrection(void) +{ + return _circleCorrection; +} + +void Joystick::setCircleCorrection(bool circleCorrection) +{ + _circleCorrection = circleCorrection; + + _saveSettings(); + emit circleCorrectionChanged(_circleCorrection); +} + void Joystick::setCalibrationMode(bool calibrating) { _calibrationMode = calibrating; diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 8cd6accf66306b3b2e89ec6d0512bdc06f3e6234..6394848add59fc7dace717e748465da419c0d2e7 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -76,8 +76,9 @@ public: Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged) Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) - Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) - + Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) + Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) + // Property accessors int axisCount(void) { return _axisCount; } @@ -120,6 +121,9 @@ public: bool deadband(void); void setDeadband(bool accu); + bool circleCorrection(void); + void setCircleCorrection(bool circleCorrection); + void setTXMode(int mode); int getTXMode(void) { return _transmitterMode; } @@ -147,6 +151,8 @@ signals: void enabledChanged(bool enabled); + void circleCorrectionChanged(bool circleCorrection); + /// 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 @@ -213,6 +219,7 @@ protected: float _exponential; bool _accumulator; bool _deadband; + bool _circleCorrection; Vehicle* _activeVehicle; bool _pollingStartedForCalibration; @@ -229,6 +236,7 @@ private: static const char* _exponentialSettingsKey; static const char* _accumulatorSettingsKey; static const char* _deadbandSettingsKey; + static const char* _circleCorrectionSettingsKey; static const char* _txModeSettingsKey; static const char* _fixedWingTXModeSettingsKey; static const char* _multiRotorTXModeSettingsKey; diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index df9a8d22c6264ea186f9275efda8f3c41b012f2e..d5112b14d2e4d964c6e369adbc455472eb4f49e3 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -530,6 +530,22 @@ SetupPage { } } + Row { + width: parent.width + spacing: ScreenTools.defaultFontPixelWidth + visible: advancedSettings.checked + QGCCheckBox { + id: joystickCircleCorrection + checked: _activeVehicle.joystickMode != 0 + text: qsTr("Enable circle correction") + + Component.onCompleted: checked = _activeJoystick.circleCorrection + onClicked: { + _activeJoystick.circleCorrection = checked + } + } + } + Row { width: parent.width spacing: ScreenTools.defaultFontPixelWidth