Skip to content

Commit 62f622d

Browse files
Niki-dev12Niki-dev12
committed
Feature: Add QML-configurable gimbal max speed and deadzones
1 parent f8e310c commit 62f622d

File tree

4 files changed

+223
-6
lines changed

4 files changed

+223
-6
lines changed

‎src/Joystick/Joystick.cc‎

Lines changed: 93 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -576,6 +576,50 @@ void Joystick::_handleButtons()
576576
}
577577
}
578578

579+
float applyDeadzone(float value, float deadzone) {
580+
if (std::abs(value) < deadzone) {
581+
return 0.0f;
582+
}else if(value<0){
583+
value=value+deadzone;
584+
}else if(value>0){
585+
value=value-deadzone;
586+
}
587+
return value;
588+
}
589+
590+
void Joystick::setGimbalYawDeadzone(int deadzone) {
591+
if (_rgCalibration[4].deadband != deadzone) {
592+
_rgCalibration[4].deadband = deadzone;
593+
_saveSettings();
594+
emit gimbalYawDeadzoneChanged();
595+
}
596+
}
597+
void Joystick::setGimbalPitchDeadzone(int deadzone) {
598+
if (_rgCalibration[5].deadband != deadzone) {
599+
_rgCalibration[5].deadband = deadzone;
600+
_saveSettings();
601+
emit gimbalPitchDeadzoneChanged();
602+
}
603+
}
604+
void Joystick::setGimbalMaxSpeed(int speed)
605+
{
606+
if (_gimbalMaxSpeed != speed) {
607+
_gimbalMaxSpeed = speed;
608+
emit gimbalMaxSpeedChanged();
609+
_saveSettings();
610+
}
611+
}
612+
613+
void Joystick::setGimbalAxisEnabled(bool enabled)
614+
{
615+
if (_gimbalAxisEnabled != enabled) {
616+
_gimbalAxisEnabled = enabled;
617+
emit gimbalAxisEnabledChanged(enabled);
618+
_saveSettings();
619+
_activeVehicle->sendGimbalAbsolutePosition(0, 0);
620+
}
621+
}
622+
579623
void Joystick::_handleAxis()
580624
{
581625
const int axisDelay = static_cast<int>(1000.0f / _axisFrequencyHz);
@@ -609,17 +653,60 @@ void Joystick::_handleAxis()
609653
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband);
610654

611655
float gimbalPitch = 0.0f;
656+
float gimbalYaw = 0.0f;
657+
612658
if (_axisCount > 4) {
613659
axis = _rgFunctionAxis[gimbalPitchFunction];
614660
gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
615661
}
616662

617-
float gimbalYaw = 0.0f;
618663
if (_axisCount > 5) {
619664
axis = _rgFunctionAxis[gimbalYawFunction];
620665
gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
621666
}
622667

668+
if (_axisCount > 5 && _gimbalAxisEnabled) {
669+
int pitchAxisIndex = 5;
670+
float gimbalPitchNorm = _adjustRange(_rgAxisValues[pitchAxisIndex], _rgCalibration[pitchAxisIndex], _deadband);
671+
// float gimbalPitchDeg = gimbalPitchNorm * 160.0f - 80.0f;
672+
// float gimbalPitchDeg = gimbalPitchNorm * (2.0f * gimbalPitchMax) - gimbalPitchMax;
673+
float gimbalPitchDeg = gimbalPitchNorm * (2.0f * static_cast<float>(_gimbalMaxSpeed)) - static_cast<float>(_gimbalMaxSpeed);
674+
gimbalPitchDeg=gimbalPitchDeg-5;
675+
676+
float gimbalPitchDeadzone = static_cast<float>(_rgCalibration[5].deadband);
677+
float gimbalPitchOut = applyDeadzone(gimbalPitchDeg, gimbalPitchDeadzone);
678+
679+
int yawAxisIndex = 4;
680+
float gimbalYawNorm = _adjustRange(_rgAxisValues[yawAxisIndex], _rgCalibration[yawAxisIndex], _deadband);
681+
// float gimbalYawDeg = gimbalYawNorm * 160.0f - 80.0f;
682+
float gimbalYawDeg = gimbalYawNorm * (2.0f * static_cast<float>(_gimbalMaxSpeed)) - static_cast<float>(_gimbalMaxSpeed);
683+
gimbalYawDeg=gimbalYawDeg-4;
684+
685+
float gimbalYawDeadzone = static_cast<float>(_rgCalibration[4].deadband);
686+
float gimbalYawOut = applyDeadzone(gimbalYawDeg, gimbalYawDeadzone);
687+
688+
static int zeroPitchCount = 0;
689+
static int zeroYawCount = 0;
690+
691+
if (std::abs(gimbalPitchOut) == 0) {
692+
zeroPitchCount++;
693+
} else {
694+
zeroPitchCount = 0;
695+
}
696+
697+
if (std::abs(gimbalYawOut) == 0) {
698+
zeroYawCount++;
699+
} else {
700+
zeroYawCount = 0;
701+
}
702+
703+
if (zeroPitchCount >= 3 && zeroYawCount >= 3) {
704+
705+
}else{
706+
_activeVehicle->sendGimbalAbsolutePosition(gimbalPitchOut, gimbalYawOut);
707+
// qDebug() << gimbalYawOut << gimbalPitchOut;
708+
}
709+
}
623710
if (_accumulator) {
624711
static float throttle_accu = 0.f;
625712
throttle_accu += (throttle * (40 / 1000.f)); // for throttle to change from min to max it will take 1000ms (40ms is a loop time)
@@ -985,6 +1072,11 @@ void Joystick::setCalibrationMode(bool calibrating)
9851072
_pollingStartedForCalibration = true;
9861073
startPolling(MultiVehicleManager::instance()->activeVehicle());
9871074
} else if (_pollingStartedForCalibration) {
1075+
if (_axisCount > 5) {
1076+
_rgFunctionAxis[gimbalYawFunction] = 4;
1077+
_rgFunctionAxis[gimbalPitchFunction] = 5;
1078+
}
1079+
_saveSettings();
9881080
stopPolling();
9891081
}
9901082
}

‎src/Joystick/Joystick.h‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,10 @@ class Joystick : public QThread
8282
Q_PROPERTY(QString name READ name CONSTANT)
8383
Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged)
8484
Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged)
85+
Q_PROPERTY(int gimbalPitchDeadzone READ gimbalPitchDeadzone WRITE setGimbalPitchDeadzone NOTIFY gimbalPitchDeadzoneChanged)
86+
Q_PROPERTY(int gimbalYawDeadzone READ gimbalYawDeadzone WRITE setGimbalYawDeadzone NOTIFY gimbalYawDeadzoneChanged)
87+
Q_PROPERTY(bool gimbalAxisEnabled READ gimbalAxisEnabled WRITE setGimbalAxisEnabled NOTIFY gimbalAxisEnabledChanged)
88+
Q_PROPERTY(int gimbalMaxSpeed READ gimbalMaxSpeed WRITE setGimbalMaxSpeed NOTIFY gimbalMaxSpeedChanged)
8589

8690
enum ButtonEvent_t {
8791
BUTTON_UP,
@@ -183,6 +187,17 @@ class Joystick : public QThread
183187
/// Set joystick button repeat rate (in Hz)
184188
void setButtonFrequency(float val);
185189

190+
int gimbalPitchDeadzone() const { return _rgCalibration[5].deadband; }
191+
int gimbalYawDeadzone() const { return _rgCalibration[4].deadband; }
192+
int gimbalMaxSpeed() const { return _gimbalMaxSpeed; }
193+
194+
Q_INVOKABLE void setGimbalPitchDeadzone(int deadzone);
195+
Q_INVOKABLE void setGimbalYawDeadzone(int deadzone);
196+
Q_INVOKABLE void setGimbalMaxSpeed(int speed);
197+
198+
bool gimbalAxisEnabled() const { return _gimbalAxisEnabled; }
199+
Q_INVOKABLE void setGimbalAxisEnabled(bool enabled);
200+
186201
signals:
187202
// The raw signals are only meant for use by calibration
188203
void rawAxisValueChanged(int index, int value);
@@ -224,6 +239,11 @@ class Joystick : public QThread
224239
void motorInterlock(bool enable);
225240
void unknownAction(const QString &action);
226241

242+
void gimbalPitchDeadzoneChanged();
243+
void gimbalYawDeadzoneChanged();
244+
void gimbalMaxSpeedChanged();
245+
void gimbalAxisEnabledChanged(bool enabled);
246+
227247
protected:
228248
void _setDefaultCalibration();
229249

@@ -366,4 +386,7 @@ private slots:
366386
static constexpr const char *_buttonActionLandingGearRetract= QT_TR_NOOP("Landing gear retract");
367387
static constexpr const char *_buttonActionMotorInterlockEnable= QT_TR_NOOP("Motor Interlock enable");
368388
static constexpr const char *_buttonActionMotorInterlockDisable= QT_TR_NOOP("Motor Interlock disable");
389+
390+
bool _gimbalAxisEnabled = true;
391+
int _gimbalMaxSpeed = 80; // Default max speed
369392
};

‎src/Vehicle/VehicleSetup/JoystickConfigCalibration.qml‎

Lines changed: 101 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,11 +13,11 @@ import QtQuick.Dialogs
1313
import QtQuick.Layouts
1414

1515
import QGroundControl
16-
16+
import QGroundControl.Palette
1717
import QGroundControl.Controls
1818
import QGroundControl.ScreenTools
19-
20-
19+
import QGroundControl.Controllers
20+
import QGroundControl.FactSystem
2121
import QGroundControl.FactControls
2222

2323
Item {
@@ -197,6 +197,104 @@ Item {
197197
horizontalAlignment: Text.AlignHCenter
198198
anchors.horizontalCenter: parent.horizontalCenter
199199
}
200+
201+
Rectangle {
202+
width: parent.width * 0.9
203+
height: 100
204+
color: "transparent"
205+
radius: 8
206+
anchors.horizontalCenter: parent.horizontalCenter
207+
anchors.topMargin: ScreenTools.defaultFontPixelHeight
208+
209+
Column {
210+
anchors.margins: ScreenTools.defaultFontPixelHeight
211+
spacing: ScreenTools.defaultFontPixelHeight * 0.5
212+
213+
QGCLabel {
214+
text: qsTr("Gimbal Control Info")
215+
font.bold: true
216+
}
217+
QGCLabel {
218+
text: qsTr("Axis 5 = Gimbal Pitch | Axis 4 = Gimbal Yaw")
219+
color: qgcPal.text
220+
}
221+
222+
// Gimbal Deadzone Axis 4 (Yaw)
223+
Row {
224+
spacing: ScreenTools.defaultFontPixelWidth * 2
225+
QGCLabel {
226+
text: qsTr("Gimbal Yaw Deadzone (Axis 4):")
227+
width: 220
228+
}
229+
Slider {
230+
id: yawDeadzoneSlider
231+
width: 150
232+
from: 0
233+
to: 20
234+
stepSize: 1
235+
value: _activeJoystick ? _activeJoystick.gimbalYawDeadzone : 0
236+
onValueChanged: if (_activeJoystick) _activeJoystick.gimbalYawDeadzone = value
237+
}
238+
QGCLabel { text: yawDeadzoneSlider.value.toFixed(0) }
239+
}
240+
241+
// Gimbal Deadzone Axis 5 Pitch
242+
Row {
243+
spacing: ScreenTools.defaultFontPixelWidth * 2
244+
QGCLabel {
245+
text: qsTr("Gimbal Pitch Deadzone (Axis 5):")
246+
width: 220
247+
}
248+
Slider {
249+
id: pitchDeadzoneSlider
250+
width: 150
251+
from: 0
252+
to: 20
253+
stepSize: 1
254+
value: _activeJoystick ? _activeJoystick.gimbalPitchDeadzone : 0
255+
onValueChanged: if (_activeJoystick) _activeJoystick.gimbalPitchDeadzone = value
256+
}
257+
QGCLabel { text: pitchDeadzoneSlider.value.toFixed(0) }
258+
}
259+
260+
// Gimbal Speed
261+
Row {
262+
spacing: ScreenTools.defaultFontPixelWidth * 2
263+
QGCLabel {
264+
text: qsTr("Gimbal Max Speed:")
265+
width: 220
266+
}
267+
Slider {
268+
id: speedSlider
269+
width: 150
270+
from: 0
271+
to: 100
272+
stepSize: 1
273+
value: _activeJoystick ? _activeJoystick.gimbalMaxSpeed : 0
274+
onValueChanged: if (_activeJoystick) _activeJoystick.gimbalMaxSpeed = value
275+
}
276+
QGCLabel { text: speedSlider.value.toFixed(0) }
277+
}
278+
279+
Row {
280+
spacing: ScreenTools.defaultFontPixelWidth * 2
281+
QGCLabel {
282+
text: qsTr("Gimbal Axis Control:")
283+
width: 220
284+
}
285+
Switch {
286+
id: gimbalAxisSwitch
287+
checked: _activeJoystick ? _activeJoystick.gimbalAxisEnabled : true
288+
onToggled: if (_activeJoystick) _activeJoystick.gimbalAxisEnabled = checked
289+
}
290+
QGCLabel {
291+
text: gimbalAxisSwitch.checked ? qsTr("Enabled") : qsTr("Disabled")
292+
}
293+
}
294+
295+
}
296+
}
297+
200298
}
201299
}
202300

‎src/Vehicle/VehicleSetup/JoystickConfigController.cc‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,8 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge
6161
static constexpr const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there...";
6262
static constexpr const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there...";
6363
static constexpr const char* msgPitchCenter = "Allow the Pitch stick to move back to center...";
64+
static constexpr const char* msgGimbalPitch = "Move the gimbal pitch axis/knob...";
65+
static constexpr const char* msgGimbalYaw = "Move the gimbal yaw axis/knob...";
6466
static constexpr const char* msgComplete = "All settings have been captured.\nClick Next to enable the joystick.";
6567

6668
static const stateMachineEntry rgStateMachine[] = {
@@ -75,7 +77,9 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge
7577
{ Joystick::pitchFunction, msgPitchUp, _sticksPitchUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 3 },
7678
{ Joystick::pitchFunction, msgPitchDown, _sticksPitchDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 3 },
7779
{ Joystick::pitchFunction, msgPitchCenter, _sticksCentered, &JoystickConfigController::_inputCenterWait, nullptr, nullptr, 3 },
78-
{ Joystick::maxFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 },
80+
{ Joystick::gimbalPitchFunction, msgGimbalPitch, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 },
81+
{ Joystick::gimbalYawFunction, msgGimbalYaw, _sticksCentered, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 },
82+
{ Joystick::maxFunction, msgComplete, _sticksCentered, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 },
7983
};
8084

8185
Q_ASSERT(step >= 0 && step < static_cast<int>((sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))));
@@ -263,7 +267,7 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi
263267
if (_rgAxisInfo[axis].function != Joystick::maxFunction) {
264268
return;
265269
}
266-
270+
267271
if (_stickDetectAxis == _axisNoAxis) {
268272
// We have not detected enough movement on a axis yet
269273
if (abs(_axisValueSave[axis] - value) > _calMoveDelta) {

0 commit comments

Comments
 (0)