Feature: Add QML-configurable gimbal max speed and deadzones

src/Joystick/Joystick.cc

@@ -576,6 +576,50 @@ 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();
+        _activeVehicle->sendGimbalAbsolutePosition(0, 0);
+    }
+}
+
 void Joystick::_handleAxis()
 {
     const int axisDelay = static_cast<int>(1000.0f / _axisFrequencyHz);
@@ -609,17 +653,60 @@ 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 * 160.0f - 80.0f;
+        // float gimbalPitchDeg = gimbalPitchNorm * (2.0f * gimbalPitchMax) - gimbalPitchMax;
+        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 * 160.0f - 80.0f;
+        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);
+
+        static int zeroPitchCount = 0;
+        static int zeroYawCount = 0;
+
+        if (std::abs(gimbalPitchOut) == 0) {
+            zeroPitchCount++;
+        } else {
+            zeroPitchCount = 0; 
+        }
+
+        if (std::abs(gimbalYawOut) == 0) {
+            zeroYawCount++;
+        } else {
+            zeroYawCount = 0; 
+        }
+
+        if (zeroPitchCount >= 3 && zeroYawCount >= 3) {
+
+        }else{
+            _activeVehicle->sendGimbalAbsolutePosition(gimbalPitchOut, gimbalYawOut);
+            // qDebug() << gimbalYawOut << gimbalPitchOut;
+        }
+    }
     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 +1073,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,7 @@ 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
 };

src/Vehicle/Vehicle.cc

@@ -406,6 +406,36 @@ void Vehicle::deleteGimbalController()
     }
 }
 
+void Vehicle::sendGimbalAbsolutePosition(float pitch_rate_deg_s, float yaw_rate_deg_s)
+{
+    auto sharedLink = vehicleLinkManager()->primaryLink().lock();
+    if (!sharedLink) {
+        qCDebug(VehicleLog) << "sendGimbalAbsolutePosition: primary link gone!";
+        return;
+    }
+
+    mavlink_message_t msg;
+    mavlink_msg_command_long_pack_chan(
+        MAVLinkProtocol::instance()->getSystemId(),        
+        MAVLinkProtocol::getComponentId(),                 
+        sharedLink->mavlinkChannel(),                      
+        &msg,
+        id(),                                              
+        MAV_COMP_ID_AUTOPILOT1,                            
+        MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW,                
+        0,                                                 
+        NAN,                                               
+        NAN,                                               
+        pitch_rate_deg_s,                                  
+        yaw_rate_deg_s,                                    
+        0,                                                 
+        NAN,                                               
+        0                                                  
+    );
+
+    sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
+}
+
 void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant varFirmwareType)
 {
     _firmwareType = static_cast<MAV_AUTOPILOT>(varFirmwareType.toInt());

src/Vehicle/Vehicle.h

@@ -527,6 +527,8 @@ class Vehicle : public VehicleFactGroup
     ///     @param sendMultiple Send multiple time to guarantee Vehicle reception
     void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple = true);
 
+    void sendGimbalAbsolutePosition(float pitch_rate_deg_s, float yaw_rate_deg_s);
+
     // The follow method are used to turn on/off the tracking of settings updates for firmware/vehicle type on offline vehicles.
     void trackFirmwareVehicleTypeChanges(void);
     void stopTrackingFirmwareVehicleTypeChanges(void);

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")
+                    }
+                }
+
+            }
+        }    
+
     }
 }
 

src/Vehicle/VehicleSetup/JoystickConfigController.cc

@@ -61,6 +61,8 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge
     static constexpr const char* msgPitchDown =           "Move the Pitch stick all the way down and hold it there...";
     static constexpr const char* msgPitchUp =             "Move the Pitch stick all the way up and hold it there...";
     static constexpr const char* msgPitchCenter =         "Allow the Pitch stick to move back to center...";
+    static constexpr const char* msgGimbalPitch =         "Move the gimbal pitch axis/knob...";
+    static constexpr const char* msgGimbalYaw =           "Move the gimbal yaw axis/knob...";
     static constexpr const char* msgComplete =            "All settings have been captured.\nClick Next to enable the joystick.";
 
     static const stateMachineEntry rgStateMachine[] = {
@@ -75,7 +77,9 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge
         { Joystick::pitchFunction,          msgPitchUp,         _sticksPitchUp,         &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 3 },
         { Joystick::pitchFunction,          msgPitchDown,       _sticksPitchDown,       &JoystickConfigController::_inputStickMin,          nullptr,                                         nullptr, 3 },
         { Joystick::pitchFunction,          msgPitchCenter,     _sticksCentered,        &JoystickConfigController::_inputCenterWait,        nullptr,                                         nullptr, 3 },
-        { Joystick::maxFunction,            msgComplete,        _sticksCentered,        nullptr,                                            &JoystickConfigController::_writeCalibration,    nullptr, -1 },
+        { Joystick::gimbalPitchFunction,    msgGimbalPitch,     _sticksCentered,        &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 4 },
+        { Joystick::gimbalYawFunction,      msgGimbalYaw,       _sticksCentered,        &JoystickConfigController::_inputStickDetect,       nullptr,                                         nullptr, 5 },
+        { Joystick::maxFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 },
     };
 
     Q_ASSERT(step >= 0 && step < static_cast<int>((sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))));
@@ -263,7 +267,7 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
     if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
         return;
     }
-    
+
     if (_stickDetectAxis == _axisNoAxis) {
         // We have not detected enough movement on a axis yet
         if (abs(_axisValueSave[axis] - value) > _calMoveDelta) {