Feature: Add QML-configurable gimbal max speed and deadzones

src/Gimbal/GimbalController.cc

@@ -15,6 +15,7 @@
 #include "QmlObjectListModel.h"
 #include "SettingsManager.h"
 #include "Vehicle.h"
+#include <cmath> // for NAN
 
 QGC_LOGGING_CATEGORY(GimbalControllerLog, "qgc.gimbal.gimbalcontroller")
 
@@ -590,6 +591,60 @@ void GimbalController::sendRate()
     }
 }
 
+void GimbalController::sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s)
+{
+    if (!_tryGetGimbalControl()) {
+        return;
+    }
+
+    uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
+    if (_activeGimbal && _activeGimbal->yawLock()) {
+        flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK;
+    }
+
+    _sendGimbalRateCommandLong(pitch_rate_deg_s, yaw_rate_deg_s, flags);
+
+    if (pitch_rate_deg_s == 0.f && yaw_rate_deg_s == 0.f) {
+        _rateSenderTimer.stop();
+    } else {
+        _rateSenderTimer.start();
+    }
+}
+
+/// Build and send MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW with rate fields,
+/// using Vehicle::sendMessageOnLinkThreadSafe
+void GimbalController::_sendGimbalRateCommandLong(float pitch_rate_deg_s,
+                                                  float yaw_rate_deg_s,
+                                                  uint32_t flags)
+{
+    auto sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
+    if (!sharedLink) {
+        qCDebug(GimbalControllerLog) << "_sendGimbalRateCommandLong: primary link gone!";
+        return;
+    }
+
+    mavlink_message_t msg;
+    mavlink_msg_command_long_pack_chan(
+        MAVLinkProtocol::instance()->getSystemId(),
+        MAVLinkProtocol::getComponentId(),
+        sharedLink->mavlinkChannel(),
+        &msg,
+        _vehicle->id(),
+        _activeGimbal->managerCompid()->rawValue().toUInt(),
+        MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,
+        0,
+        NAN,
+        NAN,
+        pitch_rate_deg_s,
+        yaw_rate_deg_s,
+        flags,
+        0,
+        _activeGimbal->deviceId()->rawValue().toUInt()
+    );
+
+    _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+}
+
 void GimbalController::_rateSenderTimeout()
 {
     // Send rate again to avoid timeout on autopilot side.

src/Gimbal/GimbalController.h

@@ -49,6 +49,8 @@ class GimbalController : public QObject
     Q_INVOKABLE void releaseGimbalControl();
     Q_INVOKABLE void sendRate();
 
+    Q_INVOKABLE void sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s);
+
 signals:
     void activeGimbalChanged();
     void showAcquireGimbalControlPopup(); // This triggers a popup in QML asking the user for aproval to take control
@@ -104,6 +106,8 @@ private slots:
     bool _tryGetGimbalControl();
     bool _yawInVehicleFrame(uint32_t flags);
 
+    void _sendGimbalRateCommandLong(float pitch_rate_deg_s, float yaw_rate_deg_s, uint32_t flags);
+
     QTimer _rateSenderTimer;
     Vehicle *_vehicle = nullptr;
     Gimbal *_activeGimbal = nullptr;

src/Joystick/Joystick.cc

@@ -576,6 +576,61 @@ void Joystick::_handleButtons()
     }
 }
 
+float applyDeadzone(float value, float deadzone) {
+        if (std::abs(value) < deadzone) {
+            return 0.0f;
+        }else if(value<0){
+            value=value+deadzone;
+        }else if(value>0){
+            value=value-deadzone;
+        }
+        return value;
+    }
+
+void Joystick::setGimbalYawDeadzone(int deadzone) {
+    if (_rgCalibration[4].deadband != deadzone) {
+        _rgCalibration[4].deadband = deadzone;
+        _saveSettings();
+        emit gimbalYawDeadzoneChanged();
+    }
+}
+void Joystick::setGimbalPitchDeadzone(int deadzone) {
+    if (_rgCalibration[5].deadband != deadzone) {
+        _rgCalibration[5].deadband = deadzone;
+        _saveSettings();
+        emit gimbalPitchDeadzoneChanged();
+    }
+}
+void Joystick::setGimbalMaxSpeed(int speed)
+{
+    if (_gimbalMaxSpeed != speed) {
+        _gimbalMaxSpeed = speed;
+        emit gimbalMaxSpeedChanged();
+        _saveSettings();
+    }
+}
+
+void Joystick::setGimbalAxisEnabled(bool enabled)
+{
+    if (_gimbalAxisEnabled != enabled) {
+        _gimbalAxisEnabled = enabled;
+        emit gimbalAxisEnabledChanged(enabled);
+        _saveSettings();
+        // Send a zero-rate command once when the state flips
+        if (_activeVehicle) {
+            if (auto* gc = _activeVehicle->gimbalController()) {
+                QMetaObject::invokeMethod(
+                    gc,
+                    "sendGimbalRate",
+                    Qt::QueuedConnection,
+                    Q_ARG(float, 0.0f),
+                    Q_ARG(float, 0.0f)
+                );
+            }
+        }
+    }
+}
+
 void Joystick::_handleAxis()
 {
     const int axisDelay = static_cast<int>(1000.0f / _axisFrequencyHz);
@@ -609,17 +664,62 @@ void Joystick::_handleAxis()
     float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband);
 
     float gimbalPitch = 0.0f;
+    float gimbalYaw = 0.0f;
+
     if (_axisCount > 4) {
         axis = _rgFunctionAxis[gimbalPitchFunction];
         gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
     }
 
-    float gimbalYaw = 0.0f;
     if (_axisCount > 5) {
         axis = _rgFunctionAxis[gimbalYawFunction];
         gimbalYaw = _adjustRange(_rgAxisValues[axis],   _rgCalibration[axis],_deadband);
     }
 
+    if (_axisCount > 5 && _gimbalAxisEnabled) {
+        int pitchAxisIndex = 5;
+        float gimbalPitchNorm = _adjustRange(_rgAxisValues[pitchAxisIndex], _rgCalibration[pitchAxisIndex], _deadband);
+        float gimbalPitchDeg = gimbalPitchNorm * (2.0f * static_cast<float>(_gimbalMaxSpeed)) - static_cast<float>(_gimbalMaxSpeed);
+        gimbalPitchDeg=gimbalPitchDeg-5;
+
+        float gimbalPitchDeadzone = static_cast<float>(_rgCalibration[5].deadband);
+        float gimbalPitchOut = applyDeadzone(gimbalPitchDeg, gimbalPitchDeadzone);
+
+        int yawAxisIndex = 4;
+        float gimbalYawNorm = _adjustRange(_rgAxisValues[yawAxisIndex], _rgCalibration[yawAxisIndex], _deadband);
+        float gimbalYawDeg = gimbalYawNorm * (2.0f * static_cast<float>(_gimbalMaxSpeed)) - static_cast<float>(_gimbalMaxSpeed);
+        gimbalYawDeg=gimbalYawDeg-4;
+
+        float gimbalYawDeadzone = static_cast<float>(_rgCalibration[4].deadband);
+        float gimbalYawOut = applyDeadzone(gimbalYawDeg, gimbalYawDeadzone);
+
+        if (std::abs(gimbalPitchOut) == 0) {
+            zeroPitchCount++;
+        } else {
+            zeroPitchCount = 0; 
+        }
+
+        if (std::abs(gimbalYawOut) == 0) {
+            zeroYawCount++;
+        } else {
+            zeroYawCount = 0; 
+        }
+
+        if (!(zeroPitchCount >= 3 && zeroYawCount >= 3)) {
+            if (_activeVehicle) {
+                if (auto* gc = _activeVehicle->gimbalController()) {
+                    QMetaObject::invokeMethod(
+                        gc,
+                        "sendGimbalRate",
+                        Qt::QueuedConnection,
+                        Q_ARG(float, gimbalPitchOut),
+                        Q_ARG(float, gimbalYawOut)
+                    );
+                }
+            }
+        }
+    }
+
     if (_accumulator) {
         static float throttle_accu = 0.f;
         throttle_accu += (throttle * (40 / 1000.f)); // for throttle to change from min to max it will take 1000ms (40ms is a loop time)
@@ -986,6 +1086,11 @@ void Joystick::setCalibrationMode(bool calibrating)
         _pollingStartedForCalibration = true;
         startPolling(MultiVehicleManager::instance()->activeVehicle());
     } else if (_pollingStartedForCalibration) {
+        if (_axisCount > 5) {
+            _rgFunctionAxis[gimbalYawFunction] = 4;
+            _rgFunctionAxis[gimbalPitchFunction] = 5;
+        }
+        _saveSettings();
         stopPolling();
     }
 }

src/Joystick/Joystick.h

@@ -82,6 +82,10 @@ 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(int gimbalPitchDeadzone READ gimbalPitchDeadzone WRITE setGimbalPitchDeadzone NOTIFY gimbalPitchDeadzoneChanged)
+    Q_PROPERTY(int gimbalYawDeadzone READ gimbalYawDeadzone WRITE setGimbalYawDeadzone NOTIFY gimbalYawDeadzoneChanged)
+    Q_PROPERTY(bool gimbalAxisEnabled READ gimbalAxisEnabled WRITE setGimbalAxisEnabled NOTIFY gimbalAxisEnabledChanged)
+    Q_PROPERTY(int gimbalMaxSpeed READ gimbalMaxSpeed WRITE setGimbalMaxSpeed NOTIFY gimbalMaxSpeedChanged)
 
     enum ButtonEvent_t {
         BUTTON_UP,
@@ -183,6 +187,17 @@ class Joystick : public QThread
     /// Set joystick button repeat rate (in Hz)
     void setButtonFrequency(float val);
 
+    int gimbalPitchDeadzone() const { return _rgCalibration[5].deadband; }
+    int gimbalYawDeadzone() const { return _rgCalibration[4].deadband; }
+    int gimbalMaxSpeed() const { return _gimbalMaxSpeed; }
+
+    Q_INVOKABLE void setGimbalPitchDeadzone(int deadzone);
+    Q_INVOKABLE void setGimbalYawDeadzone(int deadzone);
+    Q_INVOKABLE void setGimbalMaxSpeed(int speed);
+
+    bool gimbalAxisEnabled() const { return _gimbalAxisEnabled; }
+    Q_INVOKABLE void setGimbalAxisEnabled(bool enabled);
+
 signals:
     // The raw signals are only meant for use by calibration
     void rawAxisValueChanged(int index, int value);
@@ -224,6 +239,11 @@ class Joystick : public QThread
     void motorInterlock(bool enable);
     void unknownAction(const QString &action);
 
+    void gimbalPitchDeadzoneChanged();
+    void gimbalYawDeadzoneChanged();
+    void gimbalMaxSpeedChanged();
+    void gimbalAxisEnabledChanged(bool enabled);
+
 protected:
     void _setDefaultCalibration();
 
@@ -366,4 +386,9 @@ private slots:
     static constexpr const char *_buttonActionLandingGearRetract=  QT_TR_NOOP("Landing gear retract");
     static constexpr const char *_buttonActionMotorInterlockEnable=   QT_TR_NOOP("Motor Interlock enable");
     static constexpr const char *_buttonActionMotorInterlockDisable=  QT_TR_NOOP("Motor Interlock disable");
+
+    bool _gimbalAxisEnabled = true;
+    int _gimbalMaxSpeed = 80;  // Default max speed
+    int  zeroPitchCount = 0;
+    int  zeroYawCount   = 0;
 };

src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml

@@ -13,11 +13,7 @@ import QtQuick.Dialogs
 import QtQuick.Layouts
 
 import QGroundControl
-
 import QGroundControl.Controls
-
-
-
 import QGroundControl.FactControls
 
 Item {
@@ -197,6 +193,104 @@ Item {
             horizontalAlignment: Text.AlignHCenter
             anchors.horizontalCenter: parent.horizontalCenter
         }
+
+        Rectangle {
+            width: parent.width * 0.9
+            height: 100
+            color: "transparent"
+            radius: 8
+            anchors.horizontalCenter: parent.horizontalCenter
+            anchors.topMargin: ScreenTools.defaultFontPixelHeight
+
+            Column {
+                anchors.margins: ScreenTools.defaultFontPixelHeight
+                spacing: ScreenTools.defaultFontPixelHeight * 0.5
+
+                QGCLabel {
+                    text: qsTr("Gimbal Control Info")
+                    font.bold: true
+                }
+                QGCLabel {
+                    text: qsTr("Axis 5 = Gimbal Pitch   |   Axis 4 = Gimbal Yaw")
+                    color: qgcPal.text
+                }
+
+                // Gimbal Deadzone Axis 4 (Yaw)
+                Row {
+                    spacing: ScreenTools.defaultFontPixelWidth * 2
+                    QGCLabel {
+                        text: qsTr("Gimbal Yaw Deadzone (Axis 4):")
+                        width: 220
+                    }
+                    Slider {
+                        id: yawDeadzoneSlider
+                        width: 150
+                        from: 0
+                        to: 20
+                        stepSize: 1
+                        value: _activeJoystick ? _activeJoystick.gimbalYawDeadzone : 0
+                        onValueChanged: if (_activeJoystick) _activeJoystick.gimbalYawDeadzone = value
+                    }
+                    QGCLabel { text: yawDeadzoneSlider.value.toFixed(0) }
+                }
+
+                // Gimbal Deadzone Axis 5 Pitch
+                Row {
+                    spacing: ScreenTools.defaultFontPixelWidth * 2
+                    QGCLabel {
+                        text: qsTr("Gimbal Pitch Deadzone (Axis 5):")
+                        width: 220
+                    }
+                    Slider {
+                        id: pitchDeadzoneSlider
+                        width: 150
+                        from: 0
+                        to: 20
+                        stepSize: 1
+                        value: _activeJoystick ? _activeJoystick.gimbalPitchDeadzone : 0
+                        onValueChanged: if (_activeJoystick) _activeJoystick.gimbalPitchDeadzone = value
+                    }
+                    QGCLabel { text: pitchDeadzoneSlider.value.toFixed(0) }
+                }
+
+                // Gimbal Speed
+                Row {
+                    spacing: ScreenTools.defaultFontPixelWidth * 2
+                    QGCLabel {
+                        text: qsTr("Gimbal Max Speed:")
+                        width: 220
+                    }
+                    Slider {
+                        id: speedSlider
+                        width: 150
+                        from: 0
+                        to: 100
+                        stepSize: 1
+                        value: _activeJoystick ? _activeJoystick.gimbalMaxSpeed : 0
+                        onValueChanged: if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = value
+                    }
+                    QGCLabel { text: speedSlider.value.toFixed(0) }
+                }
+
+                Row {
+                    spacing: ScreenTools.defaultFontPixelWidth * 2
+                    QGCLabel {
+                        text: qsTr("Gimbal Axis Control:")
+                        width: 220
+                    }
+                    Switch {
+                        id: gimbalAxisSwitch
+                        checked: _activeJoystick ? _activeJoystick.gimbalAxisEnabled : true
+                        onToggled: if (_activeJoystick) _activeJoystick.gimbalAxisEnabled = checked
+                    }
+                    QGCLabel {
+                        text: gimbalAxisSwitch.checked ? qsTr("Enabled") : qsTr("Disabled")
+                    }
+                }
+
+            }
+        }    
+
     }
 }