Fix gamepad joystick calibration

Added setting on advanced page for allowing gimbal values in manual control

Author: DonLakeFlyer

src/Joystick/Joystick.cc

@@ -212,6 +212,7 @@ void Joystick::_loadSettings()
     _circleCorrection  = settings.value(_circleCorrectionSettingsKey, false).toBool();
     _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool();
     _throttleMode = static_cast<ThrottleMode_t>(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk));
+    _enableManualControlExtensions = settings.value(_manualControlExtensionsEnabledKey, false).toBool();
 
     badSettings |= !convertOk;
 
@@ -329,6 +330,7 @@ void Joystick::_saveSettings()
     settings.setValue(_throttleModeSettingsKey, _throttleMode);
     settings.setValue(_negativeThrustSettingsKey, _negativeThrust);
     settings.setValue(_circleCorrectionSettingsKey, _circleCorrection);
+    settings.setValue(_manualControlExtensionsEnabledKey, _enableManualControlExtensions);
 
     qCDebug(JoystickLog) << _name;
     qCDebug(JoystickLog) << "  calibrated:exponential:accumulator:circlecorrection:negativeThrust:throttlemode:deadband:axisfrequency:buttonfrequency:txmode";
@@ -618,13 +620,13 @@ void Joystick::_handleAxis()
     float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband);
 
     float gimbalPitch = NAN;
-    if (_axisCount > 4) {
+    if (_enableManualControlExtensions && _axisCount > 4) {
         axis = _rgFunctionAxis[gimbalPitchFunction];
         gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
     }
 
     float gimbalYaw = NAN;
-    if (_axisCount > 5) {
+    if (_enableManualControlExtensions && _axisCount > 5) {
         axis = _rgFunctionAxis[gimbalYawFunction];
         gimbalYaw = _adjustRange(_rgAxisValues[axis],   _rgCalibration[axis],_deadband);
     }
@@ -1263,4 +1265,13 @@ QString Joystick::axisFunctionToString(AxisFunction_t function)
     default:
         return QStringLiteral("Unknown");
     }
+}
+
+void Joystick::setEnableManualControlExtensions(bool enable)
+{
+    if (_enableManualControlExtensions != enable) {
+        _enableManualControlExtensions = enable;
+        _saveSettings();
+        emit enableManualControlExtensionsChanged();
+    }
 }

src/Joystick/Joystick.h

@@ -82,6 +82,7 @@ class Joystick : public QThread
     Q_PROPERTY(QString                  name                    READ    name                                                CONSTANT)
     Q_PROPERTY(QStringList              assignableActionTitles  READ    assignableActionTitles                              NOTIFY assignableActionsChanged)
     Q_PROPERTY(QStringList              buttonActions           READ    buttonActions                                       NOTIFY buttonActionsChanged)
+    Q_PROPERTY(bool                     enableManualControlExtensions READ enableManualControlExtensions WRITE setEnableManualControlExtensions NOTIFY enableManualControlExtensionsChanged)
 
     enum ButtonEvent_t {
         BUTTON_UP,
@@ -184,6 +185,9 @@ class Joystick : public QThread
     /// Set joystick button repeat rate (in Hz)
     void setButtonFrequency(float val);
 
+    bool enableManualControlExtensions() const { return _enableManualControlExtensions; }
+    void setEnableManualControlExtensions(bool enable);
+
 signals:
     // The raw signals are only meant for use by calibration
     void rawAxisValueChanged(int index, int value);
@@ -197,6 +201,7 @@ class Joystick : public QThread
     void accumulatorChanged(bool accumulator);
     void enabledChanged(bool enabled);
     void circleCorrectionChanged(bool circleCorrection);
+    void enableManualControlExtensionsChanged();
     void axisValues(float roll, float pitch, float yaw, float throttle);
     void axisFrequencyHzChanged();
     void buttonFrequencyHzChanged();
@@ -287,6 +292,7 @@ private slots:
     bool _deadband = false;
     bool _negativeThrust = false;
     bool _pollingStartedForCalibration = false;
+    bool _enableManualControlExtensions = false;
     float _axisFrequencyHz = _defaultAxisFrequencyHz;
     float _buttonFrequencyHz = _defaultButtonFrequencyHz;
     float _exponential = 0;
@@ -335,6 +341,7 @@ private slots:
     static constexpr const char *_roverTXModeSettingsKey =         "TXMode_Rover";
     static constexpr const char *_vtolTXModeSettingsKey =          "TXMode_VTOL";
     static constexpr const char *_submarineTXModeSettingsKey =     "TXMode_Submarine";
+    static constexpr const char *_manualControlExtensionsEnabledKey = "ManualControlExtensionsEnabled";
 
     static constexpr const char *_buttonActionNone =               QT_TR_NOOP("No Action");
     static constexpr const char *_buttonActionArm =                QT_TR_NOOP("Arm");

src/Vehicle/VehicleSetup/JoystickConfigAdvanced.qml

@@ -87,6 +87,14 @@ Item {
                 text:   expoSlider.value.toFixed(2)
             }
         }
+
+        QGCCheckBox {
+            Layout.columnSpan:  2
+            text: qsTr("Allow additional axes to be used for manual control extensions")
+            checked: _activeJoystick ? _activeJoystick.enableManualControlExtensions : false
+            onClicked: _activeJoystick.enableManualControlExtensions = checked
+        }
+
         //-----------------------------------------------------------------
         //-- Enable Advanced Mode
         QGCLabel {

src/Vehicle/VehicleSetup/JoystickConfigController.cc

@@ -94,9 +94,12 @@ void JoystickConfigController::_advanceState()
 {
     const stateMachineEntry* state = _getStateMachineEntry(++_currentStep);
 
+    Joystick* joystick = JoystickManager::instance()->activeJoystick();
+    int axisCount = joystick->enableManualControlExtensions() ? _axisCount : 4;
+
     // The state machine includes additional states for optional axes. If those
     // axes aren't detected, those states should be skipped.
-    while (state->function >= _axisCount && state->function < Joystick::maxAxisFunction) {
+    while (state->function >= axisCount && state->function < Joystick::maxAxisFunction) {
         state = _getStateMachineEntry(++_currentStep);
     }
 

src/Vehicle/VehicleSetup/JoystickConfigController.h

@@ -241,7 +241,7 @@ private slots:
     static constexpr int _calValidMaxValue =     32767;      ///< Smallest valid maximum axis value
     static constexpr int _calDefaultMinValue =   -32768;     ///< Default value for Min if not set
     static constexpr int _calDefaultMaxValue =   32767;      ///< Default value for Max if not set
-    static constexpr int _calRoughCenterDelta =  500;        ///< Delta around center point which is considered to be roughly centered
+    static constexpr int _calRoughCenterDelta =  700;        ///< Delta around center point which is considered to be roughly centered (Note: PS5 controller has very noisy center, hence 700)
     static constexpr int _calMoveDelta =         32768/2;    ///< Amount of delta past center which is considered stick movement
     static constexpr int _calSettleDelta =       600;        ///< Amount of delta which is considered no stick movement
     static constexpr int _calMinDelta =          1000;       ///< Amount of delta allowed around min value to consider channel at min