Skip to content

Conversation

Niki-dev12
Copy link
Contributor

Title:

Add QML-configurable gimbal max speed and yaw/pitch deadzone settings

Description

This PR adds new joystick configuration features to allow runtime adjustment of:
Gimbal pitch and yaw max speed, using a single gimbalMaxSpeed parameter exposed to QML
Deadzone control for pitch and yaw gimbal axes via gimbalPitchDeadzone and gimbalYawDeadzone
Enable/disable axis-based gimbal control (gimbalAxisEnabled)

This allows users to fine-tune gimbal behavior without recompiling, which is especially useful when mapping joystick axes for dynamic camera control.

Changes include:

Q_PROPERTY bindings for the three settings
Persistent saving and loading through _saveSettings() and _loadSettings()
Updated _handleAxis() to apply speed scaling and deadzone filtering
Optional offset for gimbal pitch and yaw to avoid zero drift

Test Steps

Launch QGC with a joystick connected
Use a QML debug view or a settings panel to:
    Set gimbalMaxSpeed (e.g., 30–160)
    Adjust gimbalPitchDeadzone or gimbalYawDeadzone
    Toggle gimbalAxisEnabled
Observe how gimbal responsiveness changes accordingly
Confirm values persist between sessions

Tested in:

Real drone running ardupilot with gimbal
Ubuntu 22 and Zorro 

Checklist:

Related Issue

By submitting this pull request, I confirm that you can use, modify, copy, and redistribute this contribution, under the terms of your choice.

@Niki-dev12 Niki-dev12 force-pushed the feature/gimbal-max-speed branch from 9546ecf to 9b14afc Compare August 11, 2025 08:10
@HTRamsey
Copy link
Collaborator

Could you look at the GimbalController and see if you can use that instead of changing Vehicle?

Comment on lines 688 to 708
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;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a bit weird. Can you add a comment how it works. And also remove avoid the empty if clause.

And don't use static there but instead it should be a class member variable.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I gonna fix the static. I use this code to stop overflowing MAVLink messages to the gimbal.
Without it QGroundControl cannot use the onscreen gimbal control.
When the slider is in the neutral / 0 position, the code stops sending gimbal commands.
The slider goes to neutral when you release it, because a spring centers it.
This prevents spamming the gimbal with useless "zero movement" commands when the stick is centered.

Comment on lines 671 to 672
// float gimbalPitchDeg = gimbalPitchNorm * 160.0f - 80.0f;
// float gimbalPitchDeg = gimbalPitchNorm * (2.0f * gimbalPitchMax) - gimbalPitchMax;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What's this commented out code for?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That was a hard-coded max speed for gimbal movement. I forgot to remove it. It should not be there.


uint32_t flags = GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK;
if (_activeGimbal && _activeGimbal->yawLock()) {
flags |= GIMBAL_MANAGER_FLAGS_YAW_LOCK;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This means the yaw mode is relative to North and not relative to forward. Is that expected?

}

mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm assuming we're sending this continuously? If so, a command is the wrong way, and we need to use the message GIMBAL_MANAGER_SET_ATTITUDE instead.

{
auto sharedLink = _vehicle->vehicleLinkManager()->primaryLink().lock();
if (!sharedLink) {
qCDebug(GimbalControllerLog) << "_sendGimbalRateCommandLong: primary link gone!";
return;
}

uint32_t flags = 0;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should set GIMBAL_MANAGER_FLAGS_ROLL_LOCK | GIMBAL_MANAGER_FLAGS_PITCH_LOCK.

If you want to lock to some angle relative to North, you would also set GIMBAL_MANAGER_FLAGS_YAW_LOCK, otherwise it's relative to vehicle forward and will turn with the vehicle.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for the note. My intent was vehicle-relative yaw; I have kept YAW_LOCK unset and now explicitly set ROLL_LOCK | PITCH_LOCK to make that clear.

void GimbalController::_sendGimbalRateCommandLong(float pitch_rate_deg_s,
float yaw_rate_deg_s,
uint32_t flags)
float yaw_rate_deg_s)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The method name is now misleading.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did change it

@@ -17,6 +17,8 @@
#include "Vehicle.h"
#include <cmath> // for NAN

#define M_DEG_TO_RAD_F 0.0174532925f
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Define is unused.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for catching that

Copy link
Contributor

@julianoes julianoes left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is starting to look good.

Have you thought about the case where we don't have control over the gimbal yet and need to acquire it?

@Niki-dev12
Copy link
Contributor Author

This is starting to look good.

Have you thought about the case where we don't have control over the gimbal yet and need to acquire it?

I suggest handling this in a follow up PR, since it feels a bit out of scope for the changes in this one.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants