Merge pull request #6177 from bluerobotics/patrickelectric/digital_joystick

Joystick: Add circle correction option

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<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
-            float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
-            float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
-            float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(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<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
+                float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
+                float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
+                float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(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;

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;

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