From a9be1b6b953f3315625744eb127e8afa54cf245e Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Tue, 16 Sep 2025 13:29:55 +0200 Subject: [PATCH 1/7] gimbal speed control (cherry picked from commit 43d58afc7289a509684acee96810995b90113497) --- src/Gimbal/Gimbal.cc | 1 + src/Gimbal/Gimbal.h | 2 + src/Gimbal/GimbalController.cc | 60 ++++ src/Gimbal/GimbalController.h | 4 + src/Gimbal/GimbalFact.json | 6 + src/Joystick/Joystick.cc | 132 ++++++++- src/Joystick/Joystick.h | 38 ++- src/Joystick/JoystickSDL.cc | 25 +- .../GimbalController.SettingsGroup.json | 11 +- src/Settings/GimbalControllerSettings.cc | 1 + src/Settings/GimbalControllerSettings.h | 1 + .../JoystickConfigCalibration.qml | 265 +++++++++++++++--- .../VehicleSetup/JoystickConfigController.cc | 14 +- 13 files changed, 490 insertions(+), 70 deletions(-) diff --git a/src/Gimbal/Gimbal.cc b/src/Gimbal/Gimbal.cc index 0b305e569ef7..893a03540e02 100644 --- a/src/Gimbal/Gimbal.cc +++ b/src/Gimbal/Gimbal.cc @@ -73,6 +73,7 @@ void Gimbal::_initFacts() _absoluteYawFact.setRawValue(0.0f); _deviceIdFact.setRawValue(0); _managerCompidFact.setRawValue(0); + _gimbalMaxSpeedFact.rawValue(); // dev } void Gimbal::setCapabilityFlags(uint32_t flags) diff --git a/src/Gimbal/Gimbal.h b/src/Gimbal/Gimbal.h index 133909c4db8b..9c0253615ac5 100644 --- a/src/Gimbal/Gimbal.h +++ b/src/Gimbal/Gimbal.h @@ -50,6 +50,7 @@ class Gimbal : public FactGroup Fact *absoluteYaw() { return &_absoluteYawFact; } Fact *deviceId() { return &_deviceIdFact; } Fact *managerCompid() { return &_managerCompidFact; } + Fact *gimbalMaxSpeed() { return &_gimbalMaxSpeedFact; } float pitchRate() const { return _pitchRate; } float yawRate() const { return _yawRate; } @@ -104,6 +105,7 @@ class Gimbal : public FactGroup Fact _absoluteYawFact = Fact(0, QStringLiteral("gimbalAzimuth"), FactMetaData::valueTypeFloat); Fact _deviceIdFact = Fact(0, QStringLiteral("deviceId"), FactMetaData::valueTypeUint8); ///< Component ID of gimbal device (or 1-6 for non-MAVLink gimbal) Fact _managerCompidFact = Fact(0, QStringLiteral("managerCompid"), FactMetaData::valueTypeUint8); + Fact _gimbalMaxSpeedFact = Fact(0, QStringLiteral("gimbalMaxSpeed"), FactMetaData::valueTypeUint8); // dev float _pitchRate = 0.f; float _yawRate = 0.f; diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 1405828c5f4f..0c9e31bb0a45 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -15,6 +15,8 @@ #include "QmlObjectListModel.h" #include "SettingsManager.h" #include "Vehicle.h" +#include // dev +#include "Gimbal.h" // dev QGC_LOGGING_CATEGORY(GimbalControllerLog, "Gimbal.GimbalController") @@ -591,6 +593,64 @@ void GimbalController::sendRate() } } +// dev +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 | + GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; // use vehicle/body frame + + // Preserve current yaw-lock state instead of changing it: + if (_activeGimbal->yawLock()) { + flags |= GIMBAL_MANAGER_FLAGS_YAW_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(_activeGimbal->managerCompid()->rawValue().toUInt()), + flags, + static_cast(_activeGimbal->deviceId()->rawValue().toUInt()), + qnan, + NAN, + qDegreesToRadians(pitch_rate_deg_s), + qDegreesToRadians(yaw_rate_deg_s) + ); + + _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); +} +// dev end + void GimbalController::_rateSenderTimeout() { // Send rate again to avoid timeout on autopilot side. diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index e570271e7651..a766b1b27f00 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -50,6 +50,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); // dev + signals: void activeGimbalChanged(); void showAcquireGimbalControlPopup(); // This triggers a popup in QML asking the user for aproval to take control @@ -105,6 +107,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; diff --git a/src/Gimbal/GimbalFact.json b/src/Gimbal/GimbalFact.json index 868b242b842a..f6025aa52935 100644 --- a/src/Gimbal/GimbalFact.json +++ b/src/Gimbal/GimbalFact.json @@ -35,6 +35,12 @@ "name": "deviceId", "shortDesc": "gimbal device Id", "type": "uint8" +}, +{ + "name": "gimbalMaxSpeed", + "shortDesc": "gimbal max speed", + "default": 80, + "type": "uint8" } ] } diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 76efe1f47d60..250902ed8d14 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -587,6 +587,62 @@ void Joystick::_handleButtons() } } +// dev +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) + ); + } + } + } +} +// dev end void Joystick::_handleAxis() { const int axisDelay = static_cast(1000.0f / _axisFrequencyHz); @@ -609,6 +665,7 @@ void Joystick::_handleAxis() int axis = _rgFunctionAxis[rollFunction]; float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + qDebug()<<"-------------AXIS: "<<_rgAxisValues[axis]; // dev axis = _rgFunctionAxis[pitchFunction]; float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); @@ -619,18 +676,80 @@ void Joystick::_handleAxis() axis = _rgFunctionAxis[throttleFunction]; float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband); - float gimbalPitch = NAN; - if (_enableManualControlExtensions && _axisCount > 4) { + + float gimbalPitch = 0.0f; + float gimbalYaw = 0.0f; + if (_axisCount > 4) { axis = _rgFunctionAxis[gimbalPitchFunction]; gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); } - float gimbalYaw = NAN; - if (_enableManualControlExtensions && _axisCount > 5) { + if (_axisCount > 5) { axis = _rgFunctionAxis[gimbalYawFunction]; gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); } +// dev +if (_axisCount > 5 && _gimbalAxisEnabled) { + // 1) use mapped indices, not hard-coded 4/5 + const int pitchAxisIndex = _rgFunctionAxis[gimbalYawFunction]; + const int yawAxisIndex = _rgFunctionAxis[gimbalPitchFunction]; + + // 2) normalize to 0..1 WITHOUT deadband here (we'll handle deadzone ourselves) + // NOTE: _adjustRange's 3rd param must be "useDeadband" (bool). If it's not, adapt accordingly. + const float pitchNorm01 = _adjustRange(_rgAxisValues[pitchAxisIndex], + _rgCalibration[pitchAxisIndex], + /*useDeadband=*/false); + const float yawNorm01 = _adjustRange(_rgAxisValues[yawAxisIndex], + _rgCalibration[yawAxisIndex], + /*useDeadband=*/false); + + qDebug()<<"-------------AXIS: "<<_rgAxisValues[pitchAxisIndex]; + + // 3) recenter to −1..+1 symmetrically + float pitchNorm = (pitchNorm01 - 0.5f) * 2.0f; + float yawNorm = (yawNorm01 - 0.5f) * 2.0f; + + // 4) apply UI deadzone (percent 0..100) in normalized space + auto applyDeadzonePercentNorm = [](float n, float dzPercent){ + float dz = std::clamp(dzPercent * 0.01f, 0.0f, 0.95f); + float a = std::fabs(n); + if (a <= dz) return 0.0f; + float out = (a - dz) / (1.0f - dz); // preserve full-scale reach + return (n < 0.f) ? -out : out; + }; + + pitchNorm = applyDeadzonePercentNorm(pitchNorm, float(_rgCalibration[pitchAxisIndex].deadband)); + yawNorm = applyDeadzonePercentNorm(yawNorm, float(_rgCalibration[yawAxisIndex].deadband)); + + // 5) scale to deg/s (no magic offsets) + const float pitchDegPerSec = pitchNorm * float(_gimbalMaxSpeed); + const float yawDegPerSec = yawNorm * float(_gimbalMaxSpeed); + + // Optional: simple debounce like you had before + if (std::abs(pitchDegPerSec) == 0.f) ++zeroPitchCount; else zeroPitchCount = 0; + if (std::abs(yawDegPerSec) == 0.f) ++zeroYawCount; else zeroYawCount = 0; + + if (!(zeroPitchCount >= 3 && zeroYawCount >= 3)) { + if (_activeVehicle) { + if (auto* gc = _activeVehicle->gimbalController()) { + qDebug()<<"Pitch"<gimbalControllerSettings()->CameraVFov()->rawValue().toFloat(); + // qDebug()<<"---------------------"<activeVehicle()); } else if (_pollingStartedForCalibration) { + if (_axisCount > 5) { + _rgFunctionAxis[gimbalYawFunction] = 4; + _rgFunctionAxis[gimbalPitchFunction] = 5; + } + _saveSettings(); stopPolling(); } } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 3c359cee3f2a..547c6f24f104 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -82,7 +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(bool enableManualControlExtensions READ enableManualControlExtensions WRITE setEnableManualControlExtensions NOTIFY enableManualControlExtensionsChanged) + Q_PROPERTY(int gimbalPitchDeadzone READ gimbalPitchDeadzone WRITE setGimbalPitchDeadzone NOTIFY gimbalPitchDeadzoneChanged) // dev + Q_PROPERTY(int gimbalYawDeadzone READ gimbalYawDeadzone WRITE setGimbalYawDeadzone NOTIFY gimbalYawDeadzoneChanged) // dev + Q_PROPERTY(bool gimbalAxisEnabled READ gimbalAxisEnabled WRITE setGimbalAxisEnabled NOTIFY gimbalAxisEnabledChanged) // dev + Q_PROPERTY(int gimbalMaxSpeed READ gimbalMaxSpeed WRITE setGimbalMaxSpeed NOTIFY gimbalMaxSpeedChanged) enum ButtonEvent_t { BUTTON_UP, @@ -185,8 +188,23 @@ class Joystick : public QThread /// Set joystick button repeat rate (in Hz) void setButtonFrequency(float val); - bool enableManualControlExtensions() const { return _enableManualControlExtensions; } - void setEnableManualControlExtensions(bool enable); +// dev + 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); + + // Joystick.h + Q_INVOKABLE int axisRawMin(int index) const { return _rgCalibration[index].min; } + Q_INVOKABLE int axisRawMax(int index) const { return _rgCalibration[index].max; } + Q_INVOKABLE int axisRawCenter(int index)const { return _rgCalibration[index].center; } +// dev end signals: // The raw signals are only meant for use by calibration @@ -230,6 +248,13 @@ class Joystick : public QThread void motorInterlock(bool enable); void unknownAction(const QString &action); +// dev + void gimbalPitchDeadzoneChanged(); + void gimbalYawDeadzoneChanged(); + void gimbalMaxSpeedChanged(); + void gimbalAxisEnabledChanged(bool enabled); +// dev end + protected: void _setDefaultCalibration(); @@ -376,4 +401,11 @@ 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"); + +// dev + bool _gimbalAxisEnabled = true; + int _gimbalMaxSpeed = 0; // Default max speed + int zeroPitchCount = 0; + int zeroYawCount = 0; +// dev end }; diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 7b742f411012..e05700699068 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -207,20 +207,31 @@ bool JoystickSDL::_update() return true; } +// dev bool JoystickSDL::_getButton(int idx) const { - // First try the standardized gamepad set if idx is inside that set - if (_sdlGamepad && (idx >= 0) && (idx < SDL_GAMEPAD_BUTTON_COUNT)) { - return SDL_GetGamepadButton(_sdlGamepad, static_cast(idx)); + if (!_sdlJoystick || idx < 0) { + return false; + } + + bool pressed = false; + + // 1) Standardized controller buttons (only up to SDL_CONTROLLER_BUTTON_MAX-1) + if (_isGameController && _sdlController && idx < SDL_CONTROLLER_BUTTON_MAX) { + pressed |= (SDL_GameControllerGetButton( + _sdlController, + static_cast(idx)) == 1); } - // Fall back to raw joystick buttons (covers unmapped/extras) - if (_sdlJoystick && (idx >= 0) && (idx < SDL_GetNumJoystickButtons(_sdlJoystick))) { - return SDL_GetJoystickButton(_sdlJoystick, idx); + // 2) Always fall back to raw joystick buttons (covers unmapped/extras) + const int rawCount = SDL_JoystickNumButtons(_sdlJoystick); + if (idx < rawCount) { + pressed |= (SDL_JoystickGetButton(_sdlJoystick, idx) == 1); } - return false; + return pressed; } +// dev end int JoystickSDL::_getAxis(int idx) const { diff --git a/src/Settings/GimbalController.SettingsGroup.json b/src/Settings/GimbalController.SettingsGroup.json index efc667b2150f..0ae01f0afe66 100644 --- a/src/Settings/GimbalController.SettingsGroup.json +++ b/src/Settings/GimbalController.SettingsGroup.json @@ -21,14 +21,14 @@ "name": "CameraVFov", "shortDesc": "Vertical camera field of view", "type": "uint32", - "default": 70, + "default": 80, "units": "deg" }, { "name": "CameraHFov", "shortDesc": "Horizontal camera field of view", "type": "uint32", - "default": 70, + "default": 80, "units": "deg" }, { @@ -63,6 +63,13 @@ "type": "uint32", "default": 20, "units": "deg/s" +}, +{ + "name": "gimbalSpeed", + "shortDesc": "gimbal max speed", + "type": "uint32", + "default": 80, + "units": "deg" } ] } \ No newline at end of file diff --git a/src/Settings/GimbalControllerSettings.cc b/src/Settings/GimbalControllerSettings.cc index c3dd13eba3db..84164cd4707c 100644 --- a/src/Settings/GimbalControllerSettings.cc +++ b/src/Settings/GimbalControllerSettings.cc @@ -22,3 +22,4 @@ DECLARE_SETTINGSFACT(GimbalControllerSettings, showAzimuthIndicatorOnMap) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAzimuth) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAcquireReleaseControl) DECLARE_SETTINGSFACT(GimbalControllerSettings, joystickButtonsSpeed) +DECLARE_SETTINGSFACT(GimbalControllerSettings, gimbalSpeed) diff --git a/src/Settings/GimbalControllerSettings.h b/src/Settings/GimbalControllerSettings.h index 7b8e375109e5..c0b1e6605ff6 100644 --- a/src/Settings/GimbalControllerSettings.h +++ b/src/Settings/GimbalControllerSettings.h @@ -30,4 +30,5 @@ class GimbalControllerSettings : public SettingsGroup DEFINE_SETTINGFACT(toolbarIndicatorShowAzimuth) DEFINE_SETTINGFACT(toolbarIndicatorShowAcquireReleaseControl) DEFINE_SETTINGFACT(joystickButtonsSpeed) + DEFINE_SETTINGFACT(gimbalSpeed) }; diff --git a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml index aa88f0d05989..4eae8885d1d6 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml +++ b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml @@ -110,55 +110,119 @@ Item { } } } + Repeater { - id: axisMonitorRepeater - model: _activeJoystick ? _activeJoystick.axisCount : 0 - width: parent.width - Row { - spacing: 5 - anchors.horizontalCenter: parent.horizontalCenter - // Need this to get to loader from Connections above - property Item axis: theAxis - QGCLabel { - id: axisLabel - text: modelData - } - AxisMonitor { - id: theAxis - anchors.verticalCenter: axisLabel.verticalCenter - height: ScreenTools.defaultFontPixelHeight - width: 200 - narrowIndicator: true - mapped: true - reversed: false - MouseArea { - id: deadbandMouseArea - anchors.fill: parent.item - enabled: controller.deadbandToggle - preventStealing: true - property real startX - property real direction - onPressed: (mouse) => { - startX = mouseX - direction = startX > width/2 ? 1 : -1 - parent.item.deadbandColor = "#3C6315" - } - onPositionChanged: { - var mouseToDeadband = 32768/(width/2) // Factor to have deadband follow the mouse movement - var newValue = parent.item.deadbandValue + direction*(mouseX - startX)*mouseToDeadband - if ((newValue > 0) && (newValue <32768)){parent.item.deadbandValue=newValue;} - startX = mouseX - } - onReleased: { - controller.setDeadbandValue(modelData,parent.item.deadbandValue) - parent.item.deadbandColor = "#8c161a" - } - } - } - } + id: axisMonitorRepeater + model: _activeJoystick ? _activeJoystick.axisCount : 0 + width: parent.width + + Row { + spacing: 5 + anchors.horizontalCenter: parent.horizontalCenter + + // 👇 restore this so Connections can find ".axis" + property Item axis: theAxis + property int axisIndex: modelData + + QGCLabel { + id: axisLabel + text: modelData + } + + AxisMonitor { + id: theAxis + anchors.verticalCenter: axisLabel.verticalCenter + height: ScreenTools.defaultFontPixelHeight + width: 200 + narrowIndicator: true + mapped: true + reversed: false + + // (optional) raw range accessors + property int rawMin: _activeJoystick ? _activeJoystick.axisRawMin(axisIndex) : 0 + property int rawMax: _activeJoystick ? _activeJoystick.axisRawMax(axisIndex) : 32767 + property int rawCenter: _activeJoystick ? _activeJoystick.axisRawCenter(axisIndex) : Math.round((rawMin + rawMax)/2) + property int halfRange: Math.max(rawCenter - rawMin, rawMax - rawCenter) + + MouseArea { + id: deadbandMouseArea + anchors.fill: parent.item + enabled: controller.deadbandToggle + preventStealing: true + property real startX + property real direction + + onPressed: (mouse) => { + startX = mouse.x + direction = startX > width/2 ? 1 : -1 + parent.item.deadbandColor = "#3C6315" + } + onPositionChanged: (mouse) => { + var unitsPerPixel = theAxis.halfRange / (theAxis.width / 2) + var deltaUnits = direction * (mouse.x - startX) * unitsPerPixel + var newValue = parent.item.deadbandValue + deltaUnits + newValue = Math.max(0, Math.min(theAxis.halfRange, newValue)) + parent.item.deadbandValue = newValue + startX = mouse.x + } + onReleased: { + controller.setDeadbandValue(axisIndex, parent.item.deadbandValue) + parent.item.deadbandColor = "#8c161a" } } } + } +} + + // Repeater { + // id: axisMonitorRepeater + // model: _activeJoystick ? _activeJoystick.axisCount : 0 + // width: parent.width + // Row { + // spacing: 5 + // anchors.horizontalCenter: parent.horizontalCenter + // // Need this to get to loader from Connections above + // property Item axis: theAxis + // QGCLabel { + // id: axisLabel + // text: modelData + // } + // AxisMonitor { + // id: theAxis + // anchors.verticalCenter: axisLabel.verticalCenter + // height: ScreenTools.defaultFontPixelHeight + // width: 200 + // narrowIndicator: true + // mapped: true + // reversed: false + // MouseArea { + // id: deadbandMouseArea + // anchors.fill: parent.item + // enabled: controller.deadbandToggle + // preventStealing: true + // property real startX + // property real direction + // onPressed: (mouse) => { + // startX = mouseX + // direction = startX > width/2 ? 1 : -1 + // parent.item.deadbandColor = "#3C6315" + // } + // onPositionChanged: { + // var mouseToDeadband = 32768/(width/2) // Factor to have deadband follow the mouse movement + // var newValue = parent.item.deadbandValue + direction*(mouseX - startX)*mouseToDeadband + // if ((newValue > 0) && (newValue <32768)){parent.item.deadbandValue=newValue;} + // startX = mouseX + // } + // onReleased: { + // controller.setDeadbandValue(modelData,parent.item.deadbandValue) + // parent.item.deadbandColor = "#8c161a" + // } + // } + // } + // } + // } + } + } // Command Buttons Row { spacing: ScreenTools.defaultFontPixelWidth * 2 @@ -197,6 +261,117 @@ 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: 100 + 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: 100 + 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 + } + + QGCTextField { + id: speedField + width: 150 + enabled: _activeJoystick !== null + placeholderText: qsTr("0–100") + text: _activeJoystick ? String(_activeJoystick.gimbalMaxSpeed) : "0" + inputMethodHints: Qt.ImhDigitsOnly + validator: IntValidator { bottom: 0; top: 100 } + + // Commit on Enter or when the field loses focus + onAccepted: { + if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = parseInt(text) + } + onEditingFinished: { + if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = parseInt(text) + } + } + + QGCLabel { + text: qsTr("deg/s") + } +} + + + 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") + } + } + + } + } } } diff --git a/src/Vehicle/VehicleSetup/JoystickConfigController.cc b/src/Vehicle/VehicleSetup/JoystickConfigController.cc index 129b634ec8cf..adcfd0d65f8a 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigController.cc +++ b/src/Vehicle/VehicleSetup/JoystickConfigController.cc @@ -61,10 +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* msgGimbalPitchUp = "Move the Gimbal Pitch control all the way up and hold it there..."; - static constexpr const char* msgGimbalPitchDown = "Move the Gimbal Pitch control all the way down and hold it there..."; - static constexpr const char* msgGimbalYawUp = "Move the Gimbal Yaw control all the way up and hold it there..."; - static constexpr const char* msgGimbalYawDown = "Move the Gimbal Yaw control all the way down and hold it there..."; + 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[] = { @@ -79,11 +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::gimbalPitchFunction, msgGimbalPitchUp, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 }, - { Joystick::gimbalPitchFunction, msgGimbalPitchDown, _sticksCentered, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 4 }, - { Joystick::gimbalYawFunction, msgGimbalYawUp, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 }, - { Joystick::gimbalYawFunction, msgGimbalYawDown, _sticksCentered, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 5 }, - { Joystick::maxAxisFunction, 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((sizeof(rgStateMachine) / sizeof(rgStateMachine[0])))); From 4914bd6a2e4badf73a51243ff163e04b71da41bc Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Tue, 28 Oct 2025 15:42:56 +0100 Subject: [PATCH 2/7] auto FOV --- src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository | 1 + 1 file changed, 1 insertion(+) create mode 160000 src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository diff --git a/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository b/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository new file mode 160000 index 000000000000..a458e8e86a8f --- /dev/null +++ b/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository @@ -0,0 +1 @@ +Subproject commit a458e8e86a8ffa3b7f52f4601adcdaaff0db5f42 From 881656a6db7aad9e7c320884cfaf054b005a3858 Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Wed, 29 Oct 2025 08:56:39 +0100 Subject: [PATCH 3/7] fix --- src/Joystick/Joystick.h | 4 +++ src/Joystick/JoystickSDL.cc | 26 +++++++++++-------- .../VehicleSetup/JoystickConfigController.cc | 2 +- 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 547c6f24f104..e0c8287c94d8 100644 --- a/src/Joystick/Joystick.h +++ b/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) Q_PROPERTY(int gimbalPitchDeadzone READ gimbalPitchDeadzone WRITE setGimbalPitchDeadzone NOTIFY gimbalPitchDeadzoneChanged) // dev Q_PROPERTY(int gimbalYawDeadzone READ gimbalYawDeadzone WRITE setGimbalYawDeadzone NOTIFY gimbalYawDeadzoneChanged) // dev Q_PROPERTY(bool gimbalAxisEnabled READ gimbalAxisEnabled WRITE setGimbalAxisEnabled NOTIFY gimbalAxisEnabledChanged) // dev @@ -188,6 +189,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); + // dev int gimbalPitchDeadzone() const { return _rgCalibration[5].deadband; } int gimbalYawDeadzone() const { return _rgCalibration[4].deadband; } diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index e05700699068..7febf0a17425 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -210,26 +210,30 @@ bool JoystickSDL::_update() // dev bool JoystickSDL::_getButton(int idx) const { - if (!_sdlJoystick || idx < 0) { + if (idx < 0) { return false; } - bool pressed = false; + // 1) Standardized gamepad buttons (SDL3) + if (_sdlGamepad) { + if (idx < SDL_GAMEPAD_BUTTON_COUNT) { + return SDL_GetGamepadButton( + _sdlGamepad, + static_cast(idx) + ) != 0; + } + } - // 1) Standardized controller buttons (only up to SDL_CONTROLLER_BUTTON_MAX-1) - if (_isGameController && _sdlController && idx < SDL_CONTROLLER_BUTTON_MAX) { - pressed |= (SDL_GameControllerGetButton( - _sdlController, - static_cast(idx)) == 1); + if (!_sdlJoystick) { + return false; } - // 2) Always fall back to raw joystick buttons (covers unmapped/extras) - const int rawCount = SDL_JoystickNumButtons(_sdlJoystick); + const int rawCount = SDL_GetNumJoystickButtons(_sdlJoystick); if (idx < rawCount) { - pressed |= (SDL_JoystickGetButton(_sdlJoystick, idx) == 1); + return SDL_GetJoystickButton(_sdlJoystick, idx) != 0; } - return pressed; + return false; } // dev end diff --git a/src/Vehicle/VehicleSetup/JoystickConfigController.cc b/src/Vehicle/VehicleSetup/JoystickConfigController.cc index adcfd0d65f8a..c85bf3866e6c 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigController.cc +++ b/src/Vehicle/VehicleSetup/JoystickConfigController.cc @@ -79,7 +79,7 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge { Joystick::pitchFunction, msgPitchCenter, _sticksCentered, &JoystickConfigController::_inputCenterWait, nullptr, nullptr, 3 }, { 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 }, + { Joystick::maxAxisFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 }, }; Q_ASSERT(step >= 0 && step < static_cast((sizeof(rgStateMachine) / sizeof(rgStateMachine[0])))); From 22f759ae84f61fb63ddcf275f325a60ba86cf70e Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Wed, 29 Oct 2025 10:39:28 +0100 Subject: [PATCH 4/7] auto FOV --- src/Camera/QGCCameraManager.cc | 104 ++++++- src/Camera/QGCCameraManager.h | 10 + src/Gimbal/Gimbal.cc | 1 - src/Gimbal/Gimbal.h | 4 +- src/Gimbal/GimbalController.cc | 1 + src/Gimbal/GimbalFact.json | 6 - src/Joystick/Joystick.cc | 132 +-------- src/Joystick/Joystick.h | 38 +-- src/Joystick/JoystickSDL.cc | 29 +- .../GimbalController.SettingsGroup.json | 15 +- src/Settings/GimbalControllerSettings.cc | 3 +- src/Settings/GimbalControllerSettings.h | 3 +- src/UI/toolbar/GimbalIndicator.qml | 26 ++ .../JoystickConfigCalibration.qml | 266 +++--------------- .../VehicleSetup/JoystickConfigController.cc | 16 +- 15 files changed, 222 insertions(+), 432 deletions(-) diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 97ff2bbd9884..64d19b1dd00f 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -18,6 +18,13 @@ #include "QGCVideoStreamInfo.h" #include "SimulatedCameraControl.h" +#include +#include "GimbalControllerSettings.h" +#include "SettingsManager.h" +#include + +constexpr double kPi = std::numbers::pi_v; + QGC_LOGGING_CATEGORY(CameraManagerLog, "Camera.QGCCameraManager") namespace { @@ -28,6 +35,36 @@ namespace { QVariantList QGCCameraManager::_cameraList; +//dev +static void _requestFovOnZoom_Handler( + void* user, + MAV_RESULT result, + Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, + const mavlink_message_t& message) +{ + auto* mgr = static_cast(user); + + if (result != MAV_RESULT_ACCEPTED) { + qCDebug(CameraManagerLog) << "CAMERA_FOV_STATUS request failed, result:" + << result << "failure:" << failureCode; + return; + } + + if (message.msgid != MAVLINK_MSG_ID_CAMERA_FOV_STATUS) { + qCDebug(CameraManagerLog) << "Unexpected msg id:" << message.msgid; + return; + } + + mavlink_camera_fov_status_t fov{}; + + mavlink_msg_camera_fov_status_decode(&message, &fov); + + if (!mgr) return; + if (auto* cam = mgr->currentCameraInstance()) { + } +} +//dev end + /*===========================================================================*/ QGCCameraManager::CameraStruct::CameraStruct(QGCCameraManager *manager_, uint8_t compID_, Vehicle *vehicle_) @@ -148,6 +185,9 @@ void QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t &message) case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS: _handleTrackingImageStatus(message); break; + case MAVLINK_MSG_ID_CAMERA_FOV_STATUS: + _handleCameraFovStatus(message); + break; default: break; } @@ -289,6 +329,25 @@ void QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message) _cameraInfoRequest[sCompID]->backoffTimer.stop(); qCDebug(CameraManagerLog) << "Success for compId" << QGCMAVLink::compIdToString(message.compid) << "- reset retry counter"; } + + double aspect = std::numeric_limits::quiet_NaN(); + + if (info.resolution_h > 0 && info.resolution_v > 0) { + aspect = double(info.resolution_v) / double(info.resolution_h); + } else if (info.sensor_size_h > 0.f && info.sensor_size_v > 0.f) { + aspect = double(info.sensor_size_v / info.sensor_size_h); + } + + _aspectByCompId.insert(message.compid, aspect); + + const double sensorH = static_cast(info.sensor_size_h); + const double sensorV = static_cast(info.sensor_size_v); + const double focal = static_cast(info.focal_length); + + if (sensorH > 0.0 && sensorV > 0.0 && focal > 0.0) { + const double hfovDeg = 2.0 * std::atan(sensorH / (2.0 * focal)) * 180.0 / M_PI; + const double vfovDeg = 2.0 * std::atan(sensorV / (2.0 * focal)) * 180.0 / M_PI; + } } void QGCCameraManager::_checkForLostCameras() @@ -364,13 +423,16 @@ void QGCCameraManager::_handleStorageInfo(const mavlink_message_t &message) } } -void QGCCameraManager::_handleCameraSettings(const mavlink_message_t &message) +void QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message) { - MavlinkCameraControl *pCamera = _findCamera(message.compid); + auto pCamera = _findCamera(message.compid); if (pCamera) { mavlink_camera_settings_t settings{}; mavlink_msg_camera_settings_decode(&message, &settings); pCamera->handleSettings(settings); + + zoomValueCurrent = static_cast(settings.zoomLevel); + requestCameraFovForComp(message.compid); } } @@ -680,3 +742,41 @@ const QVariantList &QGCCameraManager::cameraList() const } return _cameraList; } + +void QGCCameraManager::requestCameraFovForComp(int compId) { + _vehicle->requestMessage(_requestFovOnZoom_Handler, /*user*/this, + compId, MAVLINK_MSG_ID_CAMERA_FOV_STATUS); +} + +//----------------------------------------------------------------------------- +double QGCCameraManager::aspectForComp(int compId) const { + auto it = _aspectByCompId.constFind(compId); + return (it == _aspectByCompId.cend()) + ? std::numeric_limits::quiet_NaN() + : it.value(); +} + +double QGCCameraManager::currentCameraAspect() const { + if (auto* cam = const_cast(this)->currentCameraInstance()) { + return aspectForComp(cam->compID()); + } + return std::numeric_limits::quiet_NaN(); +} +void QGCCameraManager::_handleCameraFovStatus(const mavlink_message_t& message) +{ + mavlink_camera_fov_status_t fov{}; + mavlink_msg_camera_fov_status_decode(&message, &fov); + if (!std::isfinite(fov.hfov) || fov.hfov <= 0.0 || fov.hfov >= 180.0) return; + double aspect = aspectForComp(message.compid); + + if (!std::isfinite(aspect) || aspect <= 0.0) aspect = 9.0/16.0; + const double hfovRad = fov.hfov * kPi / 180.0; + const double vfovRad = 2.0 * std::atan(std::tan(hfovRad * 0.5) * aspect); + const double vfovDeg = vfovRad * 180.0 / kPi; + SettingsManager::instance()->gimbalControllerSettings()->CameraHFov()->setRawValue(QVariant::fromValue(fov.hfov)); + SettingsManager::instance()->gimbalControllerSettings()->CameraVFov()->setRawValue(QVariant::fromValue(vfovDeg)); +} + +int QGCCameraManager::currentZoomLevel() const { + return zoomValueCurrent; +} diff --git a/src/Camera/QGCCameraManager.h b/src/Camera/QGCCameraManager.h index 4aa7aa1f23b1..c82a1469832f 100644 --- a/src/Camera/QGCCameraManager.h +++ b/src/Camera/QGCCameraManager.h @@ -82,6 +82,15 @@ class QGCCameraManager : public QObject CameraStruct* findCameraStruct(uint8_t compId) const { return _cameraInfoRequest.value(QString::number(compId), nullptr); } + int zoomValueCurrent = 0; + int currentZoomLevel() const; + double aspectForComp(int compId) const; + double currentCameraAspect() const; + Q_INVOKABLE void requestCameraFovForComp(int compId); + +private: + QHash _aspectByCompId; + signals: void camerasChanged(); void cameraLabelsChanged(); @@ -118,6 +127,7 @@ protected slots: void _handleBatteryStatus(const mavlink_message_t& message); void _handleTrackingImageStatus(const mavlink_message_t& message); void _addCameraControlToLists(MavlinkCameraControl* cameraControl); + void _handleCameraFovStatus(const mavlink_message_t& message); QPointer _vehicle; QPointer _simulatedCameraControl; diff --git a/src/Gimbal/Gimbal.cc b/src/Gimbal/Gimbal.cc index 893a03540e02..0b305e569ef7 100644 --- a/src/Gimbal/Gimbal.cc +++ b/src/Gimbal/Gimbal.cc @@ -73,7 +73,6 @@ void Gimbal::_initFacts() _absoluteYawFact.setRawValue(0.0f); _deviceIdFact.setRawValue(0); _managerCompidFact.setRawValue(0); - _gimbalMaxSpeedFact.rawValue(); // dev } void Gimbal::setCapabilityFlags(uint32_t flags) diff --git a/src/Gimbal/Gimbal.h b/src/Gimbal/Gimbal.h index 9c0253615ac5..c5d4ccd96ad0 100644 --- a/src/Gimbal/Gimbal.h +++ b/src/Gimbal/Gimbal.h @@ -50,7 +50,6 @@ class Gimbal : public FactGroup Fact *absoluteYaw() { return &_absoluteYawFact; } Fact *deviceId() { return &_deviceIdFact; } Fact *managerCompid() { return &_managerCompidFact; } - Fact *gimbalMaxSpeed() { return &_gimbalMaxSpeedFact; } float pitchRate() const { return _pitchRate; } float yawRate() const { return _yawRate; } @@ -105,7 +104,6 @@ class Gimbal : public FactGroup Fact _absoluteYawFact = Fact(0, QStringLiteral("gimbalAzimuth"), FactMetaData::valueTypeFloat); Fact _deviceIdFact = Fact(0, QStringLiteral("deviceId"), FactMetaData::valueTypeUint8); ///< Component ID of gimbal device (or 1-6 for non-MAVLink gimbal) Fact _managerCompidFact = Fact(0, QStringLiteral("managerCompid"), FactMetaData::valueTypeUint8); - Fact _gimbalMaxSpeedFact = Fact(0, QStringLiteral("gimbalMaxSpeed"), FactMetaData::valueTypeUint8); // dev float _pitchRate = 0.f; float _yawRate = 0.f; @@ -113,4 +111,4 @@ class Gimbal : public FactGroup bool _retracted = false; bool _haveControl = false; bool _othersHaveControl = false; -}; +}; \ No newline at end of file diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index 0c9e31bb0a45..a419f4232fc8 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -17,6 +17,7 @@ #include "Vehicle.h" #include // dev #include "Gimbal.h" // dev +#include "QGCCameraManager.h" // dev QGC_LOGGING_CATEGORY(GimbalControllerLog, "Gimbal.GimbalController") diff --git a/src/Gimbal/GimbalFact.json b/src/Gimbal/GimbalFact.json index f6025aa52935..868b242b842a 100644 --- a/src/Gimbal/GimbalFact.json +++ b/src/Gimbal/GimbalFact.json @@ -35,12 +35,6 @@ "name": "deviceId", "shortDesc": "gimbal device Id", "type": "uint8" -}, -{ - "name": "gimbalMaxSpeed", - "shortDesc": "gimbal max speed", - "default": 80, - "type": "uint8" } ] } diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 250902ed8d14..76efe1f47d60 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -587,62 +587,6 @@ void Joystick::_handleButtons() } } -// dev -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) - ); - } - } - } -} -// dev end void Joystick::_handleAxis() { const int axisDelay = static_cast(1000.0f / _axisFrequencyHz); @@ -665,7 +609,6 @@ void Joystick::_handleAxis() int axis = _rgFunctionAxis[rollFunction]; float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); - qDebug()<<"-------------AXIS: "<<_rgAxisValues[axis]; // dev axis = _rgFunctionAxis[pitchFunction]; float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); @@ -676,80 +619,18 @@ void Joystick::_handleAxis() axis = _rgFunctionAxis[throttleFunction]; float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband); - - float gimbalPitch = 0.0f; - float gimbalYaw = 0.0f; - if (_axisCount > 4) { + float gimbalPitch = NAN; + if (_enableManualControlExtensions && _axisCount > 4) { axis = _rgFunctionAxis[gimbalPitchFunction]; gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); } - if (_axisCount > 5) { + float gimbalYaw = NAN; + if (_enableManualControlExtensions && _axisCount > 5) { axis = _rgFunctionAxis[gimbalYawFunction]; gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); } -// dev -if (_axisCount > 5 && _gimbalAxisEnabled) { - // 1) use mapped indices, not hard-coded 4/5 - const int pitchAxisIndex = _rgFunctionAxis[gimbalYawFunction]; - const int yawAxisIndex = _rgFunctionAxis[gimbalPitchFunction]; - - // 2) normalize to 0..1 WITHOUT deadband here (we'll handle deadzone ourselves) - // NOTE: _adjustRange's 3rd param must be "useDeadband" (bool). If it's not, adapt accordingly. - const float pitchNorm01 = _adjustRange(_rgAxisValues[pitchAxisIndex], - _rgCalibration[pitchAxisIndex], - /*useDeadband=*/false); - const float yawNorm01 = _adjustRange(_rgAxisValues[yawAxisIndex], - _rgCalibration[yawAxisIndex], - /*useDeadband=*/false); - - qDebug()<<"-------------AXIS: "<<_rgAxisValues[pitchAxisIndex]; - - // 3) recenter to −1..+1 symmetrically - float pitchNorm = (pitchNorm01 - 0.5f) * 2.0f; - float yawNorm = (yawNorm01 - 0.5f) * 2.0f; - - // 4) apply UI deadzone (percent 0..100) in normalized space - auto applyDeadzonePercentNorm = [](float n, float dzPercent){ - float dz = std::clamp(dzPercent * 0.01f, 0.0f, 0.95f); - float a = std::fabs(n); - if (a <= dz) return 0.0f; - float out = (a - dz) / (1.0f - dz); // preserve full-scale reach - return (n < 0.f) ? -out : out; - }; - - pitchNorm = applyDeadzonePercentNorm(pitchNorm, float(_rgCalibration[pitchAxisIndex].deadband)); - yawNorm = applyDeadzonePercentNorm(yawNorm, float(_rgCalibration[yawAxisIndex].deadband)); - - // 5) scale to deg/s (no magic offsets) - const float pitchDegPerSec = pitchNorm * float(_gimbalMaxSpeed); - const float yawDegPerSec = yawNorm * float(_gimbalMaxSpeed); - - // Optional: simple debounce like you had before - if (std::abs(pitchDegPerSec) == 0.f) ++zeroPitchCount; else zeroPitchCount = 0; - if (std::abs(yawDegPerSec) == 0.f) ++zeroYawCount; else zeroYawCount = 0; - - if (!(zeroPitchCount >= 3 && zeroYawCount >= 3)) { - if (_activeVehicle) { - if (auto* gc = _activeVehicle->gimbalController()) { - qDebug()<<"Pitch"<gimbalControllerSettings()->CameraVFov()->rawValue().toFloat(); - // qDebug()<<"---------------------"<activeVehicle()); } else if (_pollingStartedForCalibration) { - if (_axisCount > 5) { - _rgFunctionAxis[gimbalYawFunction] = 4; - _rgFunctionAxis[gimbalPitchFunction] = 5; - } - _saveSettings(); stopPolling(); } } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index e0c8287c94d8..96e8e9f0e480 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -83,10 +83,6 @@ class Joystick : public QThread 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) - Q_PROPERTY(int gimbalPitchDeadzone READ gimbalPitchDeadzone WRITE setGimbalPitchDeadzone NOTIFY gimbalPitchDeadzoneChanged) // dev - Q_PROPERTY(int gimbalYawDeadzone READ gimbalYawDeadzone WRITE setGimbalYawDeadzone NOTIFY gimbalYawDeadzoneChanged) // dev - Q_PROPERTY(bool gimbalAxisEnabled READ gimbalAxisEnabled WRITE setGimbalAxisEnabled NOTIFY gimbalAxisEnabledChanged) // dev - Q_PROPERTY(int gimbalMaxSpeed READ gimbalMaxSpeed WRITE setGimbalMaxSpeed NOTIFY gimbalMaxSpeedChanged) enum ButtonEvent_t { BUTTON_UP, @@ -192,24 +188,6 @@ class Joystick : public QThread bool enableManualControlExtensions() const { return _enableManualControlExtensions; } void setEnableManualControlExtensions(bool enable); -// dev - 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); - - // Joystick.h - Q_INVOKABLE int axisRawMin(int index) const { return _rgCalibration[index].min; } - Q_INVOKABLE int axisRawMax(int index) const { return _rgCalibration[index].max; } - Q_INVOKABLE int axisRawCenter(int index)const { return _rgCalibration[index].center; } -// dev end - signals: // The raw signals are only meant for use by calibration void rawAxisValueChanged(int index, int value); @@ -252,13 +230,6 @@ class Joystick : public QThread void motorInterlock(bool enable); void unknownAction(const QString &action); -// dev - void gimbalPitchDeadzoneChanged(); - void gimbalYawDeadzoneChanged(); - void gimbalMaxSpeedChanged(); - void gimbalAxisEnabledChanged(bool enabled); -// dev end - protected: void _setDefaultCalibration(); @@ -405,11 +376,4 @@ 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"); - -// dev - bool _gimbalAxisEnabled = true; - int _gimbalMaxSpeed = 0; // Default max speed - int zeroPitchCount = 0; - int zeroYawCount = 0; -// dev end -}; +}; \ No newline at end of file diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 7febf0a17425..599c6c17edda 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -207,35 +207,20 @@ bool JoystickSDL::_update() return true; } -// dev bool JoystickSDL::_getButton(int idx) const { - if (idx < 0) { - return false; + // First try the standardized gamepad set if idx is inside that set + if (_sdlGamepad && (idx >= 0) && (idx < SDL_GAMEPAD_BUTTON_COUNT)) { + return SDL_GetGamepadButton(_sdlGamepad, static_cast(idx)); } - // 1) Standardized gamepad buttons (SDL3) - if (_sdlGamepad) { - if (idx < SDL_GAMEPAD_BUTTON_COUNT) { - return SDL_GetGamepadButton( - _sdlGamepad, - static_cast(idx) - ) != 0; - } - } - - if (!_sdlJoystick) { - return false; - } - - const int rawCount = SDL_GetNumJoystickButtons(_sdlJoystick); - if (idx < rawCount) { - return SDL_GetJoystickButton(_sdlJoystick, idx) != 0; + // Fall back to raw joystick buttons (covers unmapped/extras) + if (_sdlJoystick && (idx >= 0) && (idx < SDL_GetNumJoystickButtons(_sdlJoystick))) { + return SDL_GetJoystickButton(_sdlJoystick, idx); } return false; } -// dev end int JoystickSDL::_getAxis(int idx) const { @@ -294,4 +279,4 @@ void JoystickSDL::_loadGamepadMappings() } } #endif -} +} \ No newline at end of file diff --git a/src/Settings/GimbalController.SettingsGroup.json b/src/Settings/GimbalController.SettingsGroup.json index 0ae01f0afe66..562dcfc24e3d 100644 --- a/src/Settings/GimbalController.SettingsGroup.json +++ b/src/Settings/GimbalController.SettingsGroup.json @@ -65,11 +65,18 @@ "units": "deg/s" }, { - "name": "gimbalSpeed", - "shortDesc": "gimbal max speed", + "name": "zoomMinSpeed", + "shortDesc": "Maximum gimbal speed for min zoom (deg/sec)", "type": "uint32", - "default": 80, - "units": "deg" + "default": 5, + "units": "deg/s" +}, +{ + "name": "zoomMaxSpeed", + "shortDesc": "Minimum gimbal speed for max zoom (deg/sec)", + "type": "uint32", + "default": 60, + "units": "deg/s" } ] } \ No newline at end of file diff --git a/src/Settings/GimbalControllerSettings.cc b/src/Settings/GimbalControllerSettings.cc index 84164cd4707c..40c3ab5ec973 100644 --- a/src/Settings/GimbalControllerSettings.cc +++ b/src/Settings/GimbalControllerSettings.cc @@ -22,4 +22,5 @@ DECLARE_SETTINGSFACT(GimbalControllerSettings, showAzimuthIndicatorOnMap) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAzimuth) DECLARE_SETTINGSFACT(GimbalControllerSettings, toolbarIndicatorShowAcquireReleaseControl) DECLARE_SETTINGSFACT(GimbalControllerSettings, joystickButtonsSpeed) -DECLARE_SETTINGSFACT(GimbalControllerSettings, gimbalSpeed) +DECLARE_SETTINGSFACT(GimbalControllerSettings, zoomMaxSpeed) +DECLARE_SETTINGSFACT(GimbalControllerSettings, zoomMinSpeed) diff --git a/src/Settings/GimbalControllerSettings.h b/src/Settings/GimbalControllerSettings.h index c0b1e6605ff6..80577c11b297 100644 --- a/src/Settings/GimbalControllerSettings.h +++ b/src/Settings/GimbalControllerSettings.h @@ -30,5 +30,6 @@ class GimbalControllerSettings : public SettingsGroup DEFINE_SETTINGFACT(toolbarIndicatorShowAzimuth) DEFINE_SETTINGFACT(toolbarIndicatorShowAcquireReleaseControl) DEFINE_SETTINGFACT(joystickButtonsSpeed) - DEFINE_SETTINGFACT(gimbalSpeed) + DEFINE_SETTINGFACT(zoomMaxSpeed) + DEFINE_SETTINGFACT(zoomMinSpeed) }; diff --git a/src/UI/toolbar/GimbalIndicator.qml b/src/UI/toolbar/GimbalIndicator.qml index 22e5dea1b6a0..8d160d303888 100644 --- a/src/UI/toolbar/GimbalIndicator.qml +++ b/src/UI/toolbar/GimbalIndicator.qml @@ -273,6 +273,32 @@ Item { } } + SettingsGroupLayout { + heading: qsTr("Zoom speed") + showDividers: false + + QGCLabel { + text: qsTr("Max speed (min zoom):") + visible: true + } + + FactTextField { + fact: QGroundControl.settingsManager.gimbalControllerSettings.zoomMaxSpeed + visible: true + } + + QGCLabel { + text: qsTr("Min speed (max zoom):") + visible: true + } + + FactTextField { + fact: QGroundControl.settingsManager.gimbalControllerSettings.zoomMinSpeed + visible: true + } + + } + SettingsGroupLayout { LabelledFactTextField { label: qsTr("Joystick buttons speed:") diff --git a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml index 4eae8885d1d6..3cab0f5ac379 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml +++ b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml @@ -110,119 +110,55 @@ Item { } } } - Repeater { - id: axisMonitorRepeater - model: _activeJoystick ? _activeJoystick.axisCount : 0 - width: parent.width - - Row { - spacing: 5 - anchors.horizontalCenter: parent.horizontalCenter - - // 👇 restore this so Connections can find ".axis" - property Item axis: theAxis - property int axisIndex: modelData - - QGCLabel { - id: axisLabel - text: modelData - } - - AxisMonitor { - id: theAxis - anchors.verticalCenter: axisLabel.verticalCenter - height: ScreenTools.defaultFontPixelHeight - width: 200 - narrowIndicator: true - mapped: true - reversed: false - - // (optional) raw range accessors - property int rawMin: _activeJoystick ? _activeJoystick.axisRawMin(axisIndex) : 0 - property int rawMax: _activeJoystick ? _activeJoystick.axisRawMax(axisIndex) : 32767 - property int rawCenter: _activeJoystick ? _activeJoystick.axisRawCenter(axisIndex) : Math.round((rawMin + rawMax)/2) - property int halfRange: Math.max(rawCenter - rawMin, rawMax - rawCenter) - - MouseArea { - id: deadbandMouseArea - anchors.fill: parent.item - enabled: controller.deadbandToggle - preventStealing: true - property real startX - property real direction - - onPressed: (mouse) => { - startX = mouse.x - direction = startX > width/2 ? 1 : -1 - parent.item.deadbandColor = "#3C6315" - } - onPositionChanged: (mouse) => { - var unitsPerPixel = theAxis.halfRange / (theAxis.width / 2) - var deltaUnits = direction * (mouse.x - startX) * unitsPerPixel - var newValue = parent.item.deadbandValue + deltaUnits - newValue = Math.max(0, Math.min(theAxis.halfRange, newValue)) - parent.item.deadbandValue = newValue - startX = mouse.x - } - onReleased: { - controller.setDeadbandValue(axisIndex, parent.item.deadbandValue) - parent.item.deadbandColor = "#8c161a" + id: axisMonitorRepeater + model: _activeJoystick ? _activeJoystick.axisCount : 0 + width: parent.width + Row { + spacing: 5 + anchors.horizontalCenter: parent.horizontalCenter + // Need this to get to loader from Connections above + property Item axis: theAxis + QGCLabel { + id: axisLabel + text: modelData + } + AxisMonitor { + id: theAxis + anchors.verticalCenter: axisLabel.verticalCenter + height: ScreenTools.defaultFontPixelHeight + width: 200 + narrowIndicator: true + mapped: true + reversed: false + MouseArea { + id: deadbandMouseArea + anchors.fill: parent.item + enabled: controller.deadbandToggle + preventStealing: true + property real startX + property real direction + onPressed: (mouse) => { + startX = mouseX + direction = startX > width/2 ? 1 : -1 + parent.item.deadbandColor = "#3C6315" + } + onPositionChanged: { + var mouseToDeadband = 32768/(width/2) // Factor to have deadband follow the mouse movement + var newValue = parent.item.deadbandValue + direction*(mouseX - startX)*mouseToDeadband + if ((newValue > 0) && (newValue <32768)){parent.item.deadbandValue=newValue;} + startX = mouseX + } + onReleased: { + controller.setDeadbandValue(modelData,parent.item.deadbandValue) + parent.item.deadbandColor = "#8c161a" + } + } + } + } } } } - } -} - - // Repeater { - // id: axisMonitorRepeater - // model: _activeJoystick ? _activeJoystick.axisCount : 0 - // width: parent.width - // Row { - // spacing: 5 - // anchors.horizontalCenter: parent.horizontalCenter - // // Need this to get to loader from Connections above - // property Item axis: theAxis - // QGCLabel { - // id: axisLabel - // text: modelData - // } - // AxisMonitor { - // id: theAxis - // anchors.verticalCenter: axisLabel.verticalCenter - // height: ScreenTools.defaultFontPixelHeight - // width: 200 - // narrowIndicator: true - // mapped: true - // reversed: false - // MouseArea { - // id: deadbandMouseArea - // anchors.fill: parent.item - // enabled: controller.deadbandToggle - // preventStealing: true - // property real startX - // property real direction - // onPressed: (mouse) => { - // startX = mouseX - // direction = startX > width/2 ? 1 : -1 - // parent.item.deadbandColor = "#3C6315" - // } - // onPositionChanged: { - // var mouseToDeadband = 32768/(width/2) // Factor to have deadband follow the mouse movement - // var newValue = parent.item.deadbandValue + direction*(mouseX - startX)*mouseToDeadband - // if ((newValue > 0) && (newValue <32768)){parent.item.deadbandValue=newValue;} - // startX = mouseX - // } - // onReleased: { - // controller.setDeadbandValue(modelData,parent.item.deadbandValue) - // parent.item.deadbandColor = "#8c161a" - // } - // } - // } - // } - // } - } - } // Command Buttons Row { spacing: ScreenTools.defaultFontPixelWidth * 2 @@ -261,118 +197,6 @@ 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: 100 - 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: 100 - 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 - } - - QGCTextField { - id: speedField - width: 150 - enabled: _activeJoystick !== null - placeholderText: qsTr("0–100") - text: _activeJoystick ? String(_activeJoystick.gimbalMaxSpeed) : "0" - inputMethodHints: Qt.ImhDigitsOnly - validator: IntValidator { bottom: 0; top: 100 } - - // Commit on Enter or when the field loses focus - onAccepted: { - if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = parseInt(text) - } - onEditingFinished: { - if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = parseInt(text) - } - } - - QGCLabel { - text: qsTr("deg/s") } } - - 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") - } - } - - } - } - } -} - - diff --git a/src/Vehicle/VehicleSetup/JoystickConfigController.cc b/src/Vehicle/VehicleSetup/JoystickConfigController.cc index c85bf3866e6c..eb90499028b2 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigController.cc +++ b/src/Vehicle/VehicleSetup/JoystickConfigController.cc @@ -61,8 +61,10 @@ 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* msgGimbalPitchUp = "Move the Gimbal Pitch control all the way up and hold it there..."; + static constexpr const char* msgGimbalPitchDown = "Move the Gimbal Pitch control all the way down and hold it there..."; + static constexpr const char* msgGimbalYawUp = "Move the Gimbal Yaw control all the way up and hold it there..."; + static constexpr const char* msgGimbalYawDown = "Move the Gimbal Yaw control all the way down and hold it there..."; static constexpr const char* msgComplete = "All settings have been captured.\nClick Next to enable the joystick."; static const stateMachineEntry rgStateMachine[] = { @@ -77,9 +79,11 @@ 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::gimbalPitchFunction, msgGimbalPitch, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 }, - { Joystick::gimbalYawFunction, msgGimbalYaw, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 }, - { Joystick::maxAxisFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 }, + { Joystick::gimbalPitchFunction, msgGimbalPitchUp, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 }, + { Joystick::gimbalPitchFunction, msgGimbalPitchDown, _sticksCentered, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 4 }, + { Joystick::gimbalYawFunction, msgGimbalYawUp, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 }, + { Joystick::gimbalYawFunction, msgGimbalYawDown, _sticksCentered, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 5 }, + { Joystick::maxAxisFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 }, }; Q_ASSERT(step >= 0 && step < static_cast((sizeof(rgStateMachine) / sizeof(rgStateMachine[0])))); @@ -707,4 +711,4 @@ void JoystickConfigController::_setStatusText(const QString& text) { _statusText = text; emit statusTextChanged(); -} +} \ No newline at end of file From 227de6f2149370a311dc2bc5f8b735f5fb33db52 Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Wed, 29 Oct 2025 11:01:03 +0100 Subject: [PATCH 5/7] Remove accidental submodule src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository --- src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository diff --git a/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository b/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository deleted file mode 160000 index a458e8e86a8f..000000000000 --- a/src/FirmwarePlugin/APM/ArduPilot-Parameter-Repository +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a458e8e86a8ffa3b7f52f4601adcdaaff0db5f42 From 1c4f7219a89c6104e398340559b1a326959b7f38 Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Wed, 29 Oct 2025 11:44:22 +0100 Subject: [PATCH 6/7] clean --- src/Joystick/Joystick.h | 2 +- src/Joystick/JoystickSDL.cc | 2 +- src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml | 1 - src/Vehicle/VehicleSetup/JoystickConfigController.cc | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 96e8e9f0e480..3c359cee3f2a 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -376,4 +376,4 @@ 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"); -}; \ No newline at end of file +}; diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 599c6c17edda..7b742f411012 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -279,4 +279,4 @@ void JoystickSDL::_loadGamepadMappings() } } #endif -} \ No newline at end of file +} diff --git a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml index 3cab0f5ac379..a8a5aaf2906a 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml +++ b/src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml @@ -199,4 +199,3 @@ Item { } } } - diff --git a/src/Vehicle/VehicleSetup/JoystickConfigController.cc b/src/Vehicle/VehicleSetup/JoystickConfigController.cc index eb90499028b2..129b634ec8cf 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigController.cc +++ b/src/Vehicle/VehicleSetup/JoystickConfigController.cc @@ -711,4 +711,4 @@ void JoystickConfigController::_setStatusText(const QString& text) { _statusText = text; emit statusTextChanged(); -} \ No newline at end of file +} From 76ad02d6b76b6f99b8ed5fa8bc198726b4ab8b89 Mon Sep 17 00:00:00 2001 From: Niki-dev12 Date: Mon, 3 Nov 2025 09:18:18 +0100 Subject: [PATCH 7/7] dev clean up --- src/Camera/QGCCameraManager.cc | 2 -- src/Gimbal/GimbalController.cc | 8 +++----- src/Gimbal/GimbalController.h | 2 +- 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/Camera/QGCCameraManager.cc b/src/Camera/QGCCameraManager.cc index 64d19b1dd00f..926911f50a7f 100644 --- a/src/Camera/QGCCameraManager.cc +++ b/src/Camera/QGCCameraManager.cc @@ -35,7 +35,6 @@ namespace { QVariantList QGCCameraManager::_cameraList; -//dev static void _requestFovOnZoom_Handler( void* user, MAV_RESULT result, @@ -63,7 +62,6 @@ static void _requestFovOnZoom_Handler( if (auto* cam = mgr->currentCameraInstance()) { } } -//dev end /*===========================================================================*/ diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index a419f4232fc8..24434e363d34 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -15,9 +15,9 @@ #include "QmlObjectListModel.h" #include "SettingsManager.h" #include "Vehicle.h" -#include // dev -#include "Gimbal.h" // dev -#include "QGCCameraManager.h" // dev +#include +#include "Gimbal.h" +#include "QGCCameraManager.h" QGC_LOGGING_CATEGORY(GimbalControllerLog, "Gimbal.GimbalController") @@ -594,7 +594,6 @@ void GimbalController::sendRate() } } -// dev void GimbalController::sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s) { if (!_tryGetGimbalControl()) { @@ -650,7 +649,6 @@ void GimbalController::_sendGimbalAttitudeRates(float pitch_rate_deg_s, _vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg); } -// dev end void GimbalController::_rateSenderTimeout() { diff --git a/src/Gimbal/GimbalController.h b/src/Gimbal/GimbalController.h index a766b1b27f00..7ad9ea706f75 100644 --- a/src/Gimbal/GimbalController.h +++ b/src/Gimbal/GimbalController.h @@ -50,7 +50,7 @@ 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); // dev + Q_INVOKABLE void sendGimbalRate(float pitch_rate_deg_s, float yaw_rate_deg_s); signals: void activeGimbalChanged();