Feature: Add QML-configurable gimbal max speed and deadzones

cpm-package-lock.cmake

@@ -0,0 +1,3 @@
+# CPM Package Lock
+# This file should be committed to version control
+

src/Gimbal/GimbalController.cc

@@ -15,6 +15,7 @@
 #include "QmlObjectListModel.h"
 #include "SettingsManager.h"
 #include "Vehicle.h"
+#include <cmath>
 
 QGC_LOGGING_CATEGORY(GimbalControllerLog, "qgc.gimbal.gimbalcontroller")
 
@@ -590,6 +591,53 @@ void GimbalController::sendRate()
     }
 }
 
+void GimbalController::sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s)
+{
+    if (!_tryGetGimbalControl()) {
+        return;
+    }
+
+    _sendGimbalAttitudeRates(pitch_rate_deg_s, yaw_rate_deg_s);
+
+    if (pitch_rate_deg_s == 0.f && yaw_rate_deg_s == 0.f) {
+        _rateSenderTimer.stop();
+    } else {
+        _rateSenderTimer.start();
+    }
+}
+
+void GimbalController::_sendGimbalAttitudeRates(float pitch_rate_deg_s,
+                                                float yaw_rate_deg_s)
+{
+    auto sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
+    if (!sharedLink) {
+        qCDebug(GimbalControllerLog) << "_sendGimbalAttitudeRates: primary link gone!";
+        return;
+    }
+
+    uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
+
+    const float qnan[4] = {NAN, NAN, NAN, NAN};
+    mavlink_message_t msg;
+
+    mavlink_msg_gimbal_manager_set_attitude_pack_chan(
+        MAVLinkProtocol::instance()->getSystemId(),
+        MAVLinkProtocol::getComponentId(),
+        sharedLink->mavlinkChannel(),
+        &msg,
+        _vehicle->id(),
+        static_cast<uint8_t>(_activeGimbal->managerCompid()->rawValue().toUInt()),
+        flags,
+        static_cast<uint8_t>(_activeGimbal->deviceId()->rawValue().toUInt()),
+        qnan,
+        NAN,
+        qDegreesToRadians(pitch_rate_deg_s),
+        qDegreesToRadians(yaw_rate_deg_s)
+    );
+
+    _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 _sendGimbalAttitudeRates(float pitch_rate_deg_s, float yaw_rate_deg_s);
+
     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/Joystick/JoystickAndroid.cc

@@ -298,4 +298,4 @@ void JoystickAndroid::setNativeMethods()
     }
 
     (void) AndroidInterface::cleanJavaException();
-}
+}

src/Joystick/JoystickAndroid.h

@@ -52,4 +52,4 @@ class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionE
 
     static int ACTION_DOWN, ACTION_UP, AXIS_HAT_X, AXIS_HAT_Y;
     static QMutex _mutex;
-};
+};