From 91ac79552494a06dc5c92bb396530188eaa6e7a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 5 Mar 2018 23:33:35 -0300 Subject: [PATCH] Joystick: Add circle correction option MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Some digital joysticks have already this correction Add habilit to add or removecircle correction Signed-off-by: Patrick José Pereira --- src/Joystick/Joystick.cc | 44 +++++++++++++++++++++-------- src/Joystick/Joystick.h | 12 ++++++-- src/VehicleSetup/JoystickConfig.qml | 16 +++++++++++ 3 files changed, 58 insertions(+), 14 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 04d51ccf3..9b8c3a181 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"; @@ -59,6 +60,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) @@ -115,6 +117,7 @@ void Joystick::_setDefaultCalibration(void) { _exponential = 0; _accumulator = false; _deadband = false; + _circleCorrection = false; _throttleMode = ThrottleModeCenterZero; _calibrated = true; @@ -179,6 +182,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; @@ -255,9 +259,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"); @@ -453,17 +458,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 @@ -757,6 +764,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 f90b8169b..9df0e9dc7 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 df9a8d22c..d5112b14d 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 -- 2.22.0