QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
Joystick.cc
Go to the documentation of this file.
1#include "Joystick.h"
2#include "MavlinkAction.h"
3#include "MavlinkActionManager.h"
4#include "MavlinkActionsSettings.h"
5#include "FirmwarePlugin.h"
6#include "GimbalController.h"
7#include "QGCCorePlugin.h"
8#include "QGCLoggingCategory.h"
9#include "QmlObjectListModel.h"
10#include "SettingsManager.h"
11#include "Vehicle.h"
12#include "VehicleSupports.h"
13#include "JoystickManager.h"
14#include "MultiVehicleManager.h"
15
16#include <QtCore/QCoreApplication>
17#include <algorithm>
18#include <QtCore/QSettings>
19#include <QtCore/QThread>
20
21QGC_LOGGING_CATEGORY(JoystickLog, "Joystick.Joystick")
22QGC_LOGGING_CATEGORY(JoystickVerboseLog, "Joystick.Joystick:verbose")
23
24namespace
25{
26static constexpr const char* kButtonActionArrayGroup = "JoystickButtonActionSettingsArray";
27static constexpr const char* kButtonActionNameKey = "actionName";
28static constexpr const char* kButtonRepeatKey = "repeat";
29static constexpr const char* kAxisSettingsArrayGroup = "JoystickAxisSettingsArray";
30static constexpr const char* kAxisFunctionKey = "function";
31static constexpr const char* kAxisMinKey = "min";
32static constexpr const char* kAxisMaxKey = "max";
33static constexpr const char* kAxisCenterKey = "center";
34static constexpr const char* kAxisDeadbandKey = "deadband";
35static constexpr const char* kAxisReversedKey = "reversed";
36}
37
38/*===========================================================================*/
39
40AssignedButtonAction::AssignedButtonAction(const QString &actionName_, bool repeat_)
41 : actionName(actionName_)
42 , repeat(repeat_)
43{
44 qCDebug(JoystickLog) << this;
45}
46
47AvailableButtonAction::AvailableButtonAction(const QString &actionName_, bool canRepeat_, QObject *parent)
48 : QObject(parent)
49 , _actionName(actionName_)
50 , _repeat(canRepeat_)
51{
52 qCDebug(JoystickLog) << this;
53}
54
55/*===========================================================================*/
56
57Joystick::Joystick(const QString &name, int axisCount, int buttonCount, int hatCount, QObject *parent)
58 : QThread(parent)
59 , _name(name)
60 , _axisCount(axisCount)
61 , _buttonCount(buttonCount)
62 , _hatCount(hatCount)
63 , _hatButtonCount(4 * hatCount)
64 , _totalButtonCount(_buttonCount + _hatButtonCount)
65 , _rgCalibration(_axisCount)
66 , _buttonEventStates(_totalButtonCount)
67 , _assignedButtonActions(_totalButtonCount, nullptr)
68 , _mavlinkActionManager(new MavlinkActionManager(SettingsManager::instance()->mavlinkActionsSettings()->joystickActionsFile(), this))
69 , _availableButtonActions(new QmlObjectListModel(this))
70 , _joystickManager(JoystickManager::instance())
71 , _joystickSettings(name, _axisCount, _totalButtonCount)
72{
73 qCDebug(JoystickLog)
74 << name
75 << "axisCount:" << axisCount
76 << "buttonCount:" << buttonCount
77 << "hatCount:" << hatCount
78 << this;
79
80 if (QCoreApplication *const app = QCoreApplication::instance()) {
81 QThread *const guiThread = app->thread();
82 if (_joystickSettings.thread() != guiThread) {
83 _joystickSettings.moveToThread(guiThread);
84 }
85
86 const auto ensureFactThread = [guiThread](Fact *fact) {
87 if (fact && fact->thread() != guiThread) {
88 fact->moveToThread(guiThread);
89 }
90 };
91
92 ensureFactThread(_joystickSettings.calibrated());
93 ensureFactThread(_joystickSettings.circleCorrection());
94 ensureFactThread(_joystickSettings.useDeadband());
95 ensureFactThread(_joystickSettings.negativeThrust());
96 ensureFactThread(_joystickSettings.throttleSmoothing());
97 ensureFactThread(_joystickSettings.axisFrequencyHz());
98 ensureFactThread(_joystickSettings.buttonFrequencyHz());
99 ensureFactThread(_joystickSettings.throttleModeCenterZero());
100 ensureFactThread(_joystickSettings.transmitterMode());
101 ensureFactThread(_joystickSettings.exponentialPct());
102 ensureFactThread(_joystickSettings.enableManualControlAux1());
103 ensureFactThread(_joystickSettings.enableManualControlAux2());
104 ensureFactThread(_joystickSettings.enableManualControlAux3());
105 ensureFactThread(_joystickSettings.enableManualControlAux4());
106 ensureFactThread(_joystickSettings.enableManualControlAux5());
107 ensureFactThread(_joystickSettings.enableManualControlAux6());
108 }
109
110 // Changes to manual control extension settings require re-calibration
111 connect(_joystickSettings.enableManualControlPitchExtension(), &Fact::rawValueChanged, this, [this]() {
112 _joystickSettings.calibrated()->setRawValue(false);
113 });
114 connect(_joystickSettings.enableManualControlRollExtension(), &Fact::rawValueChanged, this, [this]() {
115 _joystickSettings.calibrated()->setRawValue(false);
116 });
117 connect(_joystickSettings.enableManualControlAux1(), &Fact::rawValueChanged, this, [this]() {
118 _joystickSettings.calibrated()->setRawValue(false);
119 });
120 connect(_joystickSettings.enableManualControlAux2(), &Fact::rawValueChanged, this, [this]() {
121 _joystickSettings.calibrated()->setRawValue(false);
122 });
123 connect(_joystickSettings.enableManualControlAux3(), &Fact::rawValueChanged, this, [this]() {
124 _joystickSettings.calibrated()->setRawValue(false);
125 });
126 connect(_joystickSettings.enableManualControlAux4(), &Fact::rawValueChanged, this, [this]() {
127 _joystickSettings.calibrated()->setRawValue(false);
128 });
129 connect(_joystickSettings.enableManualControlAux5(), &Fact::rawValueChanged, this, [this]() {
130 _joystickSettings.calibrated()->setRawValue(false);
131 });
132 connect(_joystickSettings.enableManualControlAux6(), &Fact::rawValueChanged, this, [this]() {
133 _joystickSettings.calibrated()->setRawValue(false);
134 });
135
136 _resetFunctionToAxisMap();
137 _resetAxisCalibrationData();
138 _resetButtonActionData();
139 _resetButtonEventStates();
140
141 _buildAvailableButtonsActionList(MultiVehicleManager::instance()->activeVehicle());
142
143 _loadFromSettingsIntoCalibrationData();
144}
145
146Joystick::~Joystick()
147{
148 _exitThread = true;
149 if (isRunning()) {
150 if (QThread::currentThread() == this) {
151 qCWarning(JoystickLog) << "Skipping wait() on joystick thread";
152 } else {
153 wait();
154 }
155 }
156
157 _resetButtonActionData();
158 _availableButtonActions->clearAndDeleteContents();
159}
160
161void Joystick::_resetFunctionToAxisMap()
162{
163 _axisFunctionToJoystickAxisMap.clear();
164 for (int i = 0; i < maxAxisFunction; i++) {
165 _axisFunctionToJoystickAxisMap[static_cast<AxisFunction_t>(i)] = kJoystickAxisNotAssigned;
166 }
167}
168
169void Joystick::_resetAxisCalibrationData()
170{
171 _resetFunctionToAxisMap();
172 for (int axis = 0; axis < _axisCount; axis++) {
173 auto& calibration = _rgCalibration[axis];
174 calibration.reset();
175 }
176}
177
178void Joystick::_resetButtonActionData()
179{
180 for (int i = 0; i < _assignedButtonActions.count(); i++) {
181 delete _assignedButtonActions[i];
182 _assignedButtonActions[i] = nullptr;
183 }
184}
185
186void Joystick::_resetButtonEventStates()
187{
188 for (int i = 0; i < _buttonEventStates.count(); i++) {
189 _buttonEventStates[i] = ButtonEventNone;
190 }
191}
192
193void Joystick::_loadButtonSettings()
194{
195 const QString actionNameKey = QString::fromLatin1(kButtonActionNameKey);
196 const QString repeatKey = QString::fromLatin1(kButtonRepeatKey);
197
198 QSettings buttonSettings;
199 buttonSettings.beginGroup(_joystickSettings.settingsGroup());
200
201 if (buttonSettings.childGroups().contains(QString::fromLatin1(kButtonActionArrayGroup))) {
202 buttonSettings.beginGroup(QString::fromLatin1(kButtonActionArrayGroup));
203
204 qCDebug(JoystickLog) << "Button Actions:";
205
206 bool foundButton = false;
207 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
208 if (!buttonSettings.childGroups().contains(QString::number(buttonIndex))) {
209 continue;
210 }
211
212 buttonSettings.beginGroup(QString::number(buttonIndex));
213 const QString actionName = buttonSettings.value(actionNameKey).toString();
214 const bool repeat = buttonSettings.value(repeatKey, false).toBool();
215 buttonSettings.endGroup();
216
217 if (actionName.isEmpty()) {
218 qCWarning(JoystickLog)
219 << " "
220 << "button:" << buttonIndex
221 << "has empty action name, clearing action from data";
222 buttonSettings.remove(QString::number(buttonIndex));
223 continue;
224 }
225 if (_findAvailableButtonActionIndex(actionName) == -1) {
226 qCWarning(JoystickLog)
227 << " "
228 << "button:" << buttonIndex
229 << "has unknown action name:" << actionName
230 << ", clearing action from data";
231 buttonSettings.remove(QString::number(buttonIndex));
232 continue;
233 }
234 if (actionName == _buttonActionNone) {
235 buttonSettings.remove(QString::number(buttonIndex));
236 continue;
237 }
238
239 AssignedButtonAction *buttonAction = new AssignedButtonAction(actionName, repeat);
240 _assignedButtonActions[buttonIndex] = buttonAction;
241 _assignedButtonActions[buttonIndex]->buttonElapsedTimer.start();
242 foundButton = true;
243
244 qCDebug(JoystickLog)
245 << " "
246 << "button:" << buttonIndex
247 << "actionName:" << actionName
248 << "repeat:" << repeat;
249 }
250
251 buttonSettings.endGroup();
252
253 if (!foundButton) {
254 buttonSettings.remove(QString::fromLatin1(kButtonActionArrayGroup));
255 }
256 }
257}
258
259void Joystick::_foundInvalidAxisSettingsCleanup()
260{
261 _clearAxisSettings();
262 _resetAxisCalibrationData();
263 _joystickSettings.calibrated()->setRawValue(false);
264}
265
266void Joystick::_loadAxisSettings(bool joystickCalibrated, int transmitterMode)
267{
268 QSettings axisSettings;
269 axisSettings.beginGroup(_joystickSettings.settingsGroup());
270
271 // If the joystick isn't calibrated, we skip loading calibration data.
272 // Also there shouldn't be stored axis information, so clear that if any
273 if (!joystickCalibrated) {
274 _clearAxisSettings();
275 _resetAxisCalibrationData();
276 return;
277 }
278
279 const QString functionKey = QString::fromLatin1(kAxisFunctionKey);
280 const QString minKey = QString::fromLatin1(kAxisMinKey);
281 const QString maxKey = QString::fromLatin1(kAxisMaxKey);
282 const QString centerKey = QString::fromLatin1(kAxisCenterKey);
283 const QString deadbandKey = QString::fromLatin1(kAxisDeadbandKey);
284 const QString reversedKey = QString::fromLatin1(kAxisReversedKey);
285
286 qCDebug(JoystickLog) << "Axis Settings:";
287
288 axisSettings.beginGroup(QString::fromLatin1(kAxisSettingsArrayGroup));
289 for (int axis = 0; axis < _axisCount; axis++) {
290 if (!axisSettings.childGroups().contains(QString::number(axis))) {
291 qCDebug(JoystickLog)
292 << " "
293 << "axis:" << axis
294 << "no settings found, skipping";
295 continue;
296 }
297
298 axisSettings.beginGroup(QString::number(axis));
299
300 auto& axisCalibration = _rgCalibration[axis];
301
302 const int axisFunction = axisSettings.value(functionKey, maxAxisFunction).toInt();
303 if (axisFunction < 0 || axisFunction > maxAxisFunction) {
304 qCWarning(JoystickLog) << "Invalid function" << axisFunction << "for axis" << axis;
305 axisSettings.endGroup();
306 continue;
307 }
308 _setJoystickAxisForAxisFunction(static_cast<AxisFunction_t>(axisFunction), axis);
309
310 axisCalibration.center = axisSettings.value(centerKey, axisCalibration.center).toInt();
311 axisCalibration.min = axisSettings.value(minKey, axisCalibration.min).toInt();
312 axisCalibration.max = axisSettings.value(maxKey, axisCalibration.max).toInt();
313 axisCalibration.deadband = axisSettings.value(deadbandKey, axisCalibration.deadband).toInt();
314 axisCalibration.reversed = axisSettings.value(reversedKey, axisCalibration.reversed).toBool();
315
316 qCDebug(JoystickLog)
317 << " "
318 << "axis:" << axis
319 << "min:" << axisCalibration.min
320 << "max:" << axisCalibration.max
321 << "center:" << axisCalibration.center
322 << "reversed:" << axisCalibration.reversed
323 << "deadband:" << axisCalibration.deadband
324 << "axisFunction:" << axisFunctionToString(static_cast<AxisFunction_t>(axisFunction));
325
326 axisSettings.endGroup();
327 }
328 axisSettings.endGroup();
329
330 // All axis information is loaded, verify that we have all the required control mappings
331 bool rollFunctionNotAssigned = _axisFunctionToJoystickAxisMap[rollFunction] == kJoystickAxisNotAssigned;
332 if (rollFunctionNotAssigned) {
333 qCWarning(JoystickLog) << "Roll axis function not assigned";
334 }
335 bool pitchFunctionNotAssigned = _axisFunctionToJoystickAxisMap[pitchFunction] == kJoystickAxisNotAssigned;
336 if (pitchFunctionNotAssigned) {
337 qCWarning(JoystickLog) << "Pitch axis function not assigned";
338 }
339 bool yawFunctionNotAssigned = _axisFunctionToJoystickAxisMap[yawFunction] == kJoystickAxisNotAssigned;
340 if (yawFunctionNotAssigned) {
341 qCWarning(JoystickLog) << "Yaw axis function not assigned";
342 }
343 bool throttleFunctionNotAssigned = _axisFunctionToJoystickAxisMap[throttleFunction] == kJoystickAxisNotAssigned;
344 if (throttleFunctionNotAssigned) {
345 qCWarning(JoystickLog) << "Throttle axis function not assigned";
346 }
347 bool pitchExtensionFunctionRequiredButNotAssigned = false;
348 if (_joystickSettings.enableManualControlPitchExtension()->rawValue().toBool()) {
349 pitchExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(pitchExtensionFunction) == kJoystickAxisNotAssigned;
350 if (pitchExtensionFunctionRequiredButNotAssigned) {
351 qCWarning(JoystickLog) << "Internal Error: Missing pitch extension axis function mapping!";
352 }
353 }
354 bool rollExtensionFunctionRequiredButNotAssigned = false;
355 if (_joystickSettings.enableManualControlRollExtension()->rawValue().toBool()) {
356 rollExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(rollExtensionFunction) == kJoystickAxisNotAssigned;
357 if (rollExtensionFunctionRequiredButNotAssigned) {
358 qCWarning(JoystickLog) << "Internal Error: Missing roll extension axis function mapping!";
359 }
360 }
361 bool aux1ExtensionFunctionRequiredButNotAssigned = false;
362 if (_joystickSettings.enableManualControlAux1()->rawValue().toBool()) {
363 aux1ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux1ExtensionFunction) == kJoystickAxisNotAssigned;
364 if (aux1ExtensionFunctionRequiredButNotAssigned) {
365 qCWarning(JoystickLog) << "Internal Error: Missing aux1 extension axis function mapping!";
366 }
367 }
368 bool aux2ExtensionFunctionRequiredButNotAssigned = false;
369 if (_joystickSettings.enableManualControlAux2()->rawValue().toBool()) {
370 aux2ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux2ExtensionFunction) == kJoystickAxisNotAssigned;
371 if (aux2ExtensionFunctionRequiredButNotAssigned) {
372 qCWarning(JoystickLog) << "Internal Error: Missing aux2 extension axis function mapping!";
373 }
374 }
375 bool aux3ExtensionFunctionRequiredButNotAssigned = false;
376 if (_joystickSettings.enableManualControlAux3()->rawValue().toBool()) {
377 aux3ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux3ExtensionFunction) == kJoystickAxisNotAssigned;
378 if (aux3ExtensionFunctionRequiredButNotAssigned) {
379 qCWarning(JoystickLog) << "Internal Error: Missing aux3 extension axis function mapping!";
380 }
381 }
382 bool aux4ExtensionFunctionRequiredButNotAssigned = false;
383 if (_joystickSettings.enableManualControlAux4()->rawValue().toBool()) {
384 aux4ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux4ExtensionFunction) == kJoystickAxisNotAssigned;
385 if (aux4ExtensionFunctionRequiredButNotAssigned) {
386 qCWarning(JoystickLog) << "Internal Error: Missing aux4 extension axis function mapping!";
387 }
388 }
389 bool aux5ExtensionFunctionRequiredButNotAssigned = false;
390 if (_joystickSettings.enableManualControlAux5()->rawValue().toBool()) {
391 aux5ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux5ExtensionFunction) == kJoystickAxisNotAssigned;
392 if (aux5ExtensionFunctionRequiredButNotAssigned) {
393 qCWarning(JoystickLog) << "Internal Error: Missing aux5 extension axis function mapping!";
394 }
395 }
396 bool aux6ExtensionFunctionRequiredButNotAssigned = false;
397 if (_joystickSettings.enableManualControlAux6()->rawValue().toBool()) {
398 aux6ExtensionFunctionRequiredButNotAssigned = _getJoystickAxisForAxisFunction(aux6ExtensionFunction) == kJoystickAxisNotAssigned;
399 if (aux6ExtensionFunctionRequiredButNotAssigned) {
400 qCWarning(JoystickLog) << "Internal Error: Missing aux6 extension axis function mapping!";
401 }
402 }
403 if (rollFunctionNotAssigned || pitchFunctionNotAssigned || yawFunctionNotAssigned || throttleFunctionNotAssigned ||
404 pitchExtensionFunctionRequiredButNotAssigned || rollExtensionFunctionRequiredButNotAssigned ||
405 aux1ExtensionFunctionRequiredButNotAssigned || aux2ExtensionFunctionRequiredButNotAssigned || aux3ExtensionFunctionRequiredButNotAssigned ||
406 aux4ExtensionFunctionRequiredButNotAssigned || aux5ExtensionFunctionRequiredButNotAssigned || aux6ExtensionFunctionRequiredButNotAssigned) {
407 qCWarning(JoystickLog) << "Missing control axis function(s), resetting all axis settings, marking joystick as uncalibrated and disabled";
408 _resetAxisCalibrationData();
409 _clearAxisSettings();
410 _joystickSettings.calibrated()->setRawValue(false);
411 _joystickManager->setActiveJoystickEnabledForActiveVehicle(false);
412 return;
413 }
414
415 // FunctionAxis mappings are always stored in TX mode 2
416 // Remap to stored TX mode in settings for UI display
417 _remapFunctionsInFunctionMapToNewTransmittedMode(2, transmitterMode);
418}
419
420void Joystick::_loadFromSettingsIntoCalibrationData()
421{
422 _resetAxisCalibrationData();
423 _resetButtonActionData();
424
425 // Top level joystick settings come through the JoystickSettings SettingsGroup
426
427 bool calibrated = _joystickSettings.calibrated()->rawValue().toBool();
428 int transmitterMode = _joystickSettings.transmitterMode()->rawValue().toInt();
429
430 qCDebug(JoystickLog) << name();
431 qCDebug(JoystickLog) << " calibrated:" << _joystickSettings.calibrated()->rawValue().toBool();
432 qCDebug(JoystickLog) << " throttleSmoothing:" << _joystickSettings.throttleSmoothing()->rawValue().toBool();
433 qCDebug(JoystickLog) << " axisFrequencyHz:" << _joystickSettings.axisFrequencyHz()->rawValue().toDouble();
434 qCDebug(JoystickLog) << " buttonFrequencyHz:" << _joystickSettings.buttonFrequencyHz()->rawValue().toDouble();
435 qCDebug(JoystickLog) << " throttleModeCenterZero:" << _joystickSettings.throttleModeCenterZero()->rawValue().toBool();
436 qCDebug(JoystickLog) << " negativeThrust:" << _joystickSettings.negativeThrust()->rawValue().toBool();
437 qCDebug(JoystickLog) << " circleCorrection:" << _joystickSettings.circleCorrection()->rawValue().toBool();
438 qCDebug(JoystickLog) << " exponentialPct:" << _joystickSettings.exponentialPct()->rawValue().toDouble();
439 qCDebug(JoystickLog) << " useDeadband:" << _joystickSettings.useDeadband()->rawValue().toBool();
440 qCDebug(JoystickLog) << " transmitterMode:" << transmitterMode;
441 qCDebug(JoystickLog) << " enableManualControlPitchExtension:" << _joystickSettings.enableManualControlPitchExtension()->rawValue().toBool();
442 qCDebug(JoystickLog) << " enableManualControlRollExtension:" << _joystickSettings.enableManualControlRollExtension()->rawValue().toBool();
443 qCDebug(JoystickLog) << " enableManualControlAux1:" << _joystickSettings.enableManualControlAux1()->rawValue().toBool();
444 qCDebug(JoystickLog) << " enableManualControlAux2:" << _joystickSettings.enableManualControlAux2()->rawValue().toBool();
445 qCDebug(JoystickLog) << " enableManualControlAux3:" << _joystickSettings.enableManualControlAux3()->rawValue().toBool();
446 qCDebug(JoystickLog) << " enableManualControlAux4:" << _joystickSettings.enableManualControlAux4()->rawValue().toBool();
447 qCDebug(JoystickLog) << " enableManualControlAux5:" << _joystickSettings.enableManualControlAux5()->rawValue().toBool();
448 qCDebug(JoystickLog) << " enableManualControlAux6:" << _joystickSettings.enableManualControlAux6()->rawValue().toBool();
449
450 _loadAxisSettings(calibrated, transmitterMode);
451 _loadButtonSettings();
452}
453
454void Joystick::_saveAxisSettings(int transmitterMode)
455{
456 qCDebug(JoystickLog) << name();
457
458 // FunctionAxis mappings are always stored in TX mode 2
459 // Temporarily remap from calibration TX mode to mode 2 for storage
460 _remapFunctionsInFunctionMapToNewTransmittedMode(transmitterMode, 2);
461
462 QSettings axisSettings;
463 axisSettings.beginGroup(_joystickSettings.settingsGroup());
464 axisSettings.beginGroup(QString::fromLatin1(kAxisSettingsArrayGroup));
465
466 const QString functionKey = QString::fromLatin1(kAxisFunctionKey);
467 const QString minKey = QString::fromLatin1(kAxisMinKey);
468 const QString maxKey = QString::fromLatin1(kAxisMaxKey);
469 const QString centerKey = QString::fromLatin1(kAxisCenterKey);
470 const QString deadbandKey = QString::fromLatin1(kAxisDeadbandKey);
471 const QString reversedKey = QString::fromLatin1(kAxisReversedKey);
472
473 for (int axis = 0; axis < _axisCount; axis++) {
474 AxisCalibration_t *const calibration = &_rgCalibration[axis];
475 axisSettings.beginGroup(QString::number(axis));
476
477 axisSettings.setValue(centerKey, calibration->center);
478 axisSettings.setValue(minKey, calibration->min);
479 axisSettings.setValue(maxKey, calibration->max);
480 axisSettings.setValue(deadbandKey, calibration->deadband);
481 axisSettings.setValue(reversedKey, calibration->reversed);
482 AxisFunction_t function = _getAxisFunctionForJoystickAxis(axis);
483 axisSettings.setValue(functionKey, static_cast<int>(function));
484
485 qCDebug(JoystickLog)
486 << " "
487 << "axis:" << axis
488 << "min:" << calibration->min
489 << "max:" << calibration->max
490 << "center:" << calibration->center
491 << "reversed:" << calibration->reversed
492 << "deadband:" << calibration->deadband
493 << "axisFunction:" << axisFunctionToString(function);
494
495 axisSettings.endGroup();
496 }
497
498 axisSettings.endGroup();
499 axisSettings.endGroup();
500
501 // Map back to current TX mode
502 _remapFunctionsInFunctionMapToNewTransmittedMode(2, transmitterMode);
503}
504
505void Joystick::_saveButtonSettings()
506{
507 qCDebug(JoystickLog) << name();
508
509 const QString actionNameKey = QString::fromLatin1(kButtonActionNameKey);
510 const QString repeatKey = QString::fromLatin1(kButtonRepeatKey);
511
512 QSettings buttonSettings;
513 buttonSettings.beginGroup(_joystickSettings.settingsGroup());
514 buttonSettings.beginGroup(QString::fromLatin1(kButtonActionArrayGroup));
515
516 bool anyButtonsSaved = false;
517 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
518 AssignedButtonAction *buttonAction = _assignedButtonActions[buttonIndex];
519
520 if (buttonAction) {
521 buttonSettings.beginGroup(QString::number(buttonIndex));
522 buttonSettings.setValue(actionNameKey, buttonAction->actionName);
523 buttonSettings.setValue(repeatKey, buttonAction->repeat);
524 buttonSettings.endGroup();
525
526 anyButtonsSaved = true;
527
528 qCDebug(JoystickLog)
529 << " "
530 << "button:" << buttonIndex
531 << "actionName:" << buttonAction->actionName
532 << "repeat:" << buttonAction->repeat;
533 } else {
534 // No action assigned, remove any existing settings
535 buttonSettings.remove(QString::number(buttonIndex));
536 }
537 }
538
539 buttonSettings.endGroup();
540 buttonSettings.endGroup();
541
542 if (!anyButtonsSaved) {
543 qCDebug(JoystickLog) << " No button actions to save, cleared button action settings.";
544 _clearButtonSettings();
545 }
546}
547
548void Joystick::_clearAxisSettings()
549{
550 QSettings axisSettings;
551 axisSettings.beginGroup(_joystickSettings.settingsGroup());
552 axisSettings.remove(QString::fromLatin1(kAxisSettingsArrayGroup));
553 axisSettings.endGroup();
554}
555
556void Joystick::_clearButtonSettings()
557{
558 QSettings buttonSettings;
559 buttonSettings.beginGroup(_joystickSettings.settingsGroup());
560 buttonSettings.remove(QString::fromLatin1(kButtonActionArrayGroup));
561 buttonSettings.endGroup();
562}
563
564void Joystick::_saveFromCalibrationDataIntoSettings()
565{
566 bool calibrated = _joystickSettings.calibrated()->rawValue().toBool();
567 int transmitterMode = _joystickSettings.transmitterMode()->rawValue().toInt();
568
569 qCDebug(JoystickLog) << name();
570 qCDebug(JoystickLog) << " calibrated:" << calibrated;
571 qCDebug(JoystickLog) << " throttleSmoothing:" << _joystickSettings.throttleSmoothing()->rawValue().toBool();
572 qCDebug(JoystickLog) << " axisFrequencyHz:" << _joystickSettings.axisFrequencyHz()->rawValue().toDouble();
573 qCDebug(JoystickLog) << " buttonFrequencyHz:" << _joystickSettings.buttonFrequencyHz()->rawValue().toDouble();
574 qCDebug(JoystickLog) << " throttleModeCenterZero:" << _joystickSettings.throttleModeCenterZero()->rawValue().toBool();
575 qCDebug(JoystickLog) << " negativeThrust:" << _joystickSettings.negativeThrust()->rawValue().toBool();
576 qCDebug(JoystickLog) << " circleCorrection:" << _joystickSettings.circleCorrection()->rawValue().toBool();
577 qCDebug(JoystickLog) << " exponentialPct:" << _joystickSettings.exponentialPct()->rawValue().toDouble();
578 qCDebug(JoystickLog) << " useDeadband:" << _joystickSettings.useDeadband()->rawValue().toBool();
579 qCDebug(JoystickLog) << " enableManualControlAux1:" << _joystickSettings.enableManualControlAux1()->rawValue().toBool();
580 qCDebug(JoystickLog) << " enableManualControlAux2:" << _joystickSettings.enableManualControlAux2()->rawValue().toBool();
581 qCDebug(JoystickLog) << " enableManualControlAux3:" << _joystickSettings.enableManualControlAux3()->rawValue().toBool();
582 qCDebug(JoystickLog) << " enableManualControlAux4:" << _joystickSettings.enableManualControlAux4()->rawValue().toBool();
583 qCDebug(JoystickLog) << " enableManualControlAux5:" << _joystickSettings.enableManualControlAux5()->rawValue().toBool();
584 qCDebug(JoystickLog) << " enableManualControlAux6:" << _joystickSettings.enableManualControlAux6()->rawValue().toBool();
585 qCDebug(JoystickLog) << " transmitterMode:" << transmitterMode;
586
587 _clearAxisSettings();
588 if (calibrated) {
589 _saveAxisSettings(transmitterMode);
590 }
591
592 _clearButtonSettings();
593 _saveButtonSettings();
594}
595
596void Joystick::_remapFunctionsInFunctionMapToNewTransmittedMode(int fromMode, int toMode)
597{
598 static constexpr const int modeCount = 4;
599 static constexpr const int attitudeControlCount = 4;
600 static constexpr const AxisFunction_t mapping[modeCount][attitudeControlCount] = {
601 { yawFunction, pitchFunction, rollFunction, throttleFunction },
602 { yawFunction, throttleFunction, rollFunction, pitchFunction },
603 { rollFunction, pitchFunction, yawFunction, throttleFunction },
604 { rollFunction, throttleFunction, yawFunction, pitchFunction }
605 };
606
607 // First make a direct copy so the extension stick function are set correctly
608 AxisFunctionMap_t tempMap = _axisFunctionToJoystickAxisMap;
609
610 for (int i=0; i<attitudeControlCount; i++) {
611 auto fromAxisFunction = mapping[fromMode - 1][i];
612 auto toAxisFunction = mapping[toMode - 1][i];
613
614 tempMap[toAxisFunction] = _axisFunctionToJoystickAxisMap[fromAxisFunction];
615 }
616
617 for (int i = 0; i < maxAxisFunction; i++) {
618 auto axisFunction = static_cast<AxisFunction_t>(i);
619 _axisFunctionToJoystickAxisMap[axisFunction] = tempMap[axisFunction];
620 }
621}
622
623void Joystick::run()
624{
625 bool openFailed = false;
626 bool updateFailed = false;
627
628 if (!_open()) {
629 qCWarning(JoystickLog) << "Failed to open joystick:" << _name;
630 openFailed = true;
631 }
632
633 if (!openFailed) {
634 _axisElapsedTimer.start();
635
636 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
637 if (_assignedButtonActions[buttonIndex]) {
638 _assignedButtonActions[buttonIndex]->buttonElapsedTimer.start();
639 }
640 }
641
642 while (!_exitThread) {
643 if (!_update()) {
644 qCWarning(JoystickLog) << "Joystick disconnected or update failed:" << _name;
645 updateFailed = true;
646 break;
647 }
648
649 _handleButtons();
650
651 if (axisCount() != 0) {
652 _handleAxis();
653 }
654
655 const double axisFrequencyHz = _joystickSettings.axisFrequencyHz()->rawValue().toDouble();
656 const double buttonFrequencyHz = _joystickSettings.buttonFrequencyHz()->rawValue().toDouble();
657
658 const int sleep = qMin(static_cast<int>(1000.0 / axisFrequencyHz), static_cast<int>(1000.0 / buttonFrequencyHz)) / 2;
659 QThread::msleep(sleep);
660 }
661
662 _close();
663 }
664
665 if ((openFailed || updateFailed) && !_exitThread) {
666 qCDebug(JoystickLog) << "Triggering joystick rescan after failure";
667 QMetaObject::invokeMethod(JoystickManager::instance(), "_checkForAddedOrRemovedJoysticks", Qt::QueuedConnection);
668 }
669}
670
671void Joystick::_updateButtonEventState(int buttonIndex, const bool buttonPressed, ButtonEvent_t &buttonEventState)
672{
673 if (buttonPressed) {
674 if (buttonEventState == ButtonEventNone) {
675 qCDebug(JoystickLog) << "Button" << buttonIndex << "down transition";
676 buttonEventState = ButtonEventDownTransition;
677 } else if (buttonEventState == ButtonEventDownTransition) {
678 qCDebug(JoystickLog) << "Button" << buttonIndex << "repeat";
679 buttonEventState = ButtonEventRepeat;
680 }
681 } else {
682 if (buttonEventState == ButtonEventDownTransition || buttonEventState == ButtonEventRepeat) {
683 qCDebug(JoystickLog) << "Button" << buttonIndex << "up transition";
684 buttonEventState = ButtonEventUpTransition;
685 } else if (buttonEventState == ButtonEventUpTransition) {
686 qCDebug(JoystickLog) << "Button" << buttonIndex << "none";
687 buttonEventState = ButtonEventNone;
688 }
689 }
690}
691
692void Joystick::_updateButtonEventStates(QVector<ButtonEvent_t> &buttonEventStates)
693{
694 if (buttonEventStates.size() < _totalButtonCount) {
695 qCWarning(JoystickLog) << "Internal Error: buttonEventStates size incorrect!";
696 return;
697 }
698
699 // Update standard buttons - index 0 to buttonCount-1
700 for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) {
701 const bool buttonIsPressed = _getButton(buttonIndex);
702 _updateButtonEventState(buttonIndex, buttonIsPressed, buttonEventStates[buttonIndex]);
703 }
704
705 // Update hat buttons - indexes buttonCount to totalButtonCount-1
706 const int numHatButtons = 4;
707 int nextIndex = _buttonCount;
708 for (int hatIndex = 0; hatIndex < _hatCount; hatIndex++) {
709 for (int hatButtonIndex = 0; hatButtonIndex < numHatButtons; hatButtonIndex++) {
710 const bool buttonIsPressed = _getHat(hatIndex, hatButtonIndex);
711 const int currentIndex = nextIndex;
712 _updateButtonEventState(currentIndex, buttonIsPressed, buttonEventStates[currentIndex]);
713 ++nextIndex;
714 }
715 }
716}
717
718void Joystick::_handleButtons()
719{
720 if (_currentPollingType == NotPolling) {
721 qCWarning(JoystickLog) << "Internal Error: Joystick not polling!";
722 return;
723 }
724
725 _updateButtonEventStates(_buttonEventStates);
726
727 if (_currentPollingType == PollingForConfiguration) {
728 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
729 if (_buttonEventStates[buttonIndex] == ButtonEventDownTransition) {
730 qCDebug(JoystickLog) << "Button pressed - button" << buttonIndex;
731 emit rawButtonPressedChanged(buttonIndex, true);
732 } else if (_buttonEventStates[buttonIndex] == ButtonEventUpTransition) {
733 qCDebug(JoystickLog) << "Button released - button" << buttonIndex;
734 emit rawButtonPressedChanged(buttonIndex, false);
735 }
736 }
737 } else if (_currentPollingType == PollingForVehicle) {
738 Vehicle *const vehicle = _pollingVehicle;
739 if (!vehicle) {
740 qCWarning(JoystickLog) << "Internal Error: No vehicle for joystick!";
741 return;
742 }
743 if (!_joystickManager->activeJoystickEnabledForActiveVehicle()) {
744 qCWarning(JoystickLog) << "Internal Error: Joystick not enabled for vehicle!";
745 return;
746 }
747 if (!_joystickSettings.calibrated()->rawValue().toBool()) {
748 return;
749 }
750
751 //-- Process button press/release
752 const int buttonDelay = static_cast<int>(1000.0 / _joystickSettings.buttonFrequencyHz()->rawValue().toDouble());
753 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
754 if (!_assignedButtonActions[buttonIndex]) {
755 continue;
756 }
757 auto assignedAction = _assignedButtonActions[buttonIndex];
758
759 const QString buttonAction = assignedAction->actionName;
760 if (buttonAction.isEmpty() || (buttonAction == _buttonActionNone)) {
761 continue;
762 }
763
764 auto buttonEventState = _buttonEventStates[buttonIndex];
765 if (buttonEventState == ButtonEventDownTransition || buttonEventState == ButtonEventRepeat) {
766 if (assignedAction->repeat) {
767 if (assignedAction->buttonElapsedTimer.elapsed() > buttonDelay) {
768 assignedAction->buttonElapsedTimer.start();
769 qCDebug(JoystickLog) << "Repeat - button:action" << buttonIndex << buttonAction;
770 _executeButtonAction(buttonAction, ButtonEventRepeat);
771 }
772 } else {
773 if (buttonEventState == ButtonEventDownTransition) {
774 // Check for multi-button action
775 QList<int> multiActionButtons = { buttonIndex };
776 for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) {
777 if (_assignedButtonActions[multiIndex] && (_assignedButtonActions[multiIndex]->actionName == buttonAction)) {
778 // We found a multi-button action
779 if (_buttonEventStates[multiIndex] == ButtonEventDownTransition || _buttonEventStates[multiIndex] == ButtonEventRepeat) {
780 // So far so good
781 multiActionButtons.append(multiIndex);
782 continue;
783 } else {
784 // We are missing a press we need
785 return;
786 }
787 }
788 }
789
790 if (multiActionButtons.size() > 1) {
791 qCDebug(JoystickLog) << "Multi-button action - buttons:action" << multiActionButtons << buttonAction;
792 } else {
793 qCDebug(JoystickLog) << "Action triggered - button:Action" << buttonIndex << buttonAction;
794 }
795 _executeButtonAction(buttonAction, ButtonEventDownTransition);
796 return;
797 }
798 }
799 } else if (buttonEventState == ButtonEventUpTransition) {
800 qCDebug(JoystickLog) << "Button up - button:action" << buttonIndex << buttonAction;
801 _executeButtonAction(buttonAction, ButtonEventUpTransition);
802 return;
803 }
804 }
805 }
806}
807
808float Joystick::_adjustRange(int value, const AxisCalibration_t &calibration, bool withDeadbands)
809{
810 float valueNormalized;
811 float axisLength;
812 float axisBasis;
813
814 if (value > calibration.center) {
815 axisBasis = 1.0f;
816 valueNormalized = value - calibration.center;
817 axisLength = calibration.max - calibration.center;
818 } else {
819 axisBasis = -1.0f;
820 valueNormalized = calibration.center - value;
821 axisLength = calibration.center - calibration.min;
822 }
823
824 float axisPercent;
825 if (withDeadbands) {
826 if (valueNormalized > calibration.deadband) {
827 axisPercent = (valueNormalized - calibration.deadband) / (axisLength - calibration.deadband);
828 } else if (valueNormalized<-calibration.deadband) {
829 axisPercent = (valueNormalized + calibration.deadband) / (axisLength - calibration.deadband);
830 } else {
831 axisPercent = 0.f;
832 }
833 } else {
834 axisPercent = valueNormalized / axisLength;
835 }
836
837 float correctedValue = axisBasis * axisPercent;
838 if (calibration.reversed) {
839 correctedValue *= -1.0f;
840 }
841
842 return std::max(-1.0f, std::min(correctedValue, 1.0f));
843}
844
845void Joystick::_handleAxis()
846{
847 const int axisDelay = static_cast<int>(1000.0 / _joystickSettings.axisFrequencyHz()->rawValue().toDouble());
848 if (_axisElapsedTimer.elapsed() <= axisDelay) {
849 return;
850 }
851
852 _axisElapsedTimer.start();
853
854 if (_currentPollingType == NotPolling) {
855 qCWarning(JoystickLog) << "Internal Error: Joystick not polling!";
856 return;
857 }
858
859 if (_currentPollingType == PollingForConfiguration) {
860 // Signal the axis values to Joystick Config/Cal. Axes in Joystick terminology are the same as Channels in
861 // RemoteControlCalibrationController terminology.
862 QVector<int> channelValues(_axisCount);
863 for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) {
864 channelValues[axisIndex] = _getAxisValue(axisIndex);
865 }
866 emit rawChannelValuesChanged(channelValues);
867 } else if (_currentPollingType == PollingForVehicle) {
868 Vehicle *const vehicle = _pollingVehicle;
869 if (!vehicle) {
870 qCWarning(JoystickLog) << "Internal Error: No vehicle for joystick!";
871 return;
872 }
873 if (!_joystickManager->activeJoystickEnabledForActiveVehicle()) {
874 qCWarning(JoystickLog) << "Internal Error: Joystick not enabled for vehicle!";
875 return;
876 }
877 if (!_joystickSettings.calibrated()->rawValue().toBool()) {
878 return;
879 }
880
881 bool useDeadband = _joystickSettings.useDeadband()->rawValue().toBool();
882 bool throttleModeCenterZero = _joystickSettings.throttleModeCenterZero()->rawValue().toBool();
883 bool negativeThrust = _joystickSettings.negativeThrust()->rawValue().toBool();
884 bool circleCorrection = _joystickSettings.circleCorrection()->rawValue().toBool();
885 bool throttleSmoothing = _joystickSettings.throttleSmoothing()->rawValue().toBool();
886 double exponentialPercent = _joystickSettings.exponentialPct()->rawValue().toDouble();
887
888 if (_getJoystickAxisForAxisFunction(rollFunction) == kJoystickAxisNotAssigned ||
889 _getJoystickAxisForAxisFunction(pitchFunction) == kJoystickAxisNotAssigned ||
890 _getJoystickAxisForAxisFunction(yawFunction) == kJoystickAxisNotAssigned ||
891 _getJoystickAxisForAxisFunction(throttleFunction) == kJoystickAxisNotAssigned) {
892 qCWarning(JoystickLog) << "Internal Error: Missing attitude control axis function mapping!";
893 return;
894 }
895 int axisIndex = _getJoystickAxisForAxisFunction(rollFunction);
896 float roll = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
897 axisIndex = _getJoystickAxisForAxisFunction(pitchFunction);
898 float pitch = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
899 axisIndex = _getJoystickAxisForAxisFunction(yawFunction);
900 float yaw = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex],useDeadband);
901 axisIndex = _getJoystickAxisForAxisFunction(throttleFunction);
902 float throttle = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], throttleModeCenterZero ? useDeadband : false);
903
904 float pitchExtension = qQNaN();
905 if (_joystickSettings.enableManualControlPitchExtension()->rawValue().toBool()) {
906 if (_getJoystickAxisForAxisFunction(pitchExtensionFunction) == kJoystickAxisNotAssigned) {
907 qCWarning(JoystickLog) << "Internal Error: Missing pitch extension axis function mapping!";
908 return;
909 }
910 axisIndex = _getJoystickAxisForAxisFunction(pitchExtensionFunction);
911 pitchExtension = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
912 }
913 float rollExtension = qQNaN();
914 if (_joystickSettings.enableManualControlRollExtension()->rawValue().toBool()) {
915 if (_getJoystickAxisForAxisFunction(rollExtensionFunction) == kJoystickAxisNotAssigned) {
916 qCWarning(JoystickLog) << "Internal Error: Missing roll extension axis function mapping!";
917 return;
918 }
919 axisIndex = _getJoystickAxisForAxisFunction(rollExtensionFunction);
920 rollExtension = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
921 }
922 float aux1 = qQNaN();
923 if (_joystickSettings.enableManualControlAux1()->rawValue().toBool()) {
924 if (_getJoystickAxisForAxisFunction(aux1ExtensionFunction) == kJoystickAxisNotAssigned) {
925 qCWarning(JoystickLog) << "Internal Error: Missing aux1 extension axis function mapping!";
926 return;
927 }
928 axisIndex = _getJoystickAxisForAxisFunction(aux1ExtensionFunction);
929 aux1 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
930 }
931 float aux2 = qQNaN();
932 if (_joystickSettings.enableManualControlAux2()->rawValue().toBool()) {
933 if (_getJoystickAxisForAxisFunction(aux2ExtensionFunction) == kJoystickAxisNotAssigned) {
934 qCWarning(JoystickLog) << "Internal Error: Missing aux2 extension axis function mapping!";
935 return;
936 }
937 axisIndex = _getJoystickAxisForAxisFunction(aux2ExtensionFunction);
938 aux2 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
939 }
940 float aux3 = qQNaN();
941 if (_joystickSettings.enableManualControlAux3()->rawValue().toBool()) {
942 if (_getJoystickAxisForAxisFunction(aux3ExtensionFunction) == kJoystickAxisNotAssigned) {
943 qCWarning(JoystickLog) << "Internal Error: Missing aux3 extension axis function mapping!";
944 return;
945 }
946 axisIndex = _getJoystickAxisForAxisFunction(aux3ExtensionFunction);
947 aux3 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
948 }
949 float aux4 = qQNaN();
950 if (_joystickSettings.enableManualControlAux4()->rawValue().toBool()) {
951 if (_getJoystickAxisForAxisFunction(aux4ExtensionFunction) == kJoystickAxisNotAssigned) {
952 qCWarning(JoystickLog) << "Internal Error: Missing aux4 extension axis function mapping!";
953 return;
954 }
955 axisIndex = _getJoystickAxisForAxisFunction(aux4ExtensionFunction);
956 aux4 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
957 }
958 float aux5 = qQNaN();
959 if (_joystickSettings.enableManualControlAux5()->rawValue().toBool()) {
960 if (_getJoystickAxisForAxisFunction(aux5ExtensionFunction) == kJoystickAxisNotAssigned) {
961 qCWarning(JoystickLog) << "Internal Error: Missing aux5 extension axis function mapping!";
962 return;
963 }
964 axisIndex = _getJoystickAxisForAxisFunction(aux5ExtensionFunction);
965 aux5 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
966 }
967 float aux6 = qQNaN();
968 if (_joystickSettings.enableManualControlAux6()->rawValue().toBool()) {
969 if (_getJoystickAxisForAxisFunction(aux6ExtensionFunction) == kJoystickAxisNotAssigned) {
970 qCWarning(JoystickLog) << "Internal Error: Missing aux6 extension axis function mapping!";
971 return;
972 }
973 axisIndex = _getJoystickAxisForAxisFunction(aux6ExtensionFunction);
974 aux6 = _adjustRange(_getAxisValue(axisIndex), _rgCalibration[axisIndex], useDeadband);
975 }
976
977 if (throttleSmoothing) {
978 static float throttleSmoothingValue = 0.f;
979 throttleSmoothingValue += (throttle * (40 / 1000.f)); // for throttle to change from min to max it will take 1000ms (40ms is a loop time)
980 throttleSmoothingValue = std::max(static_cast<float>(-1.f), std::min(throttleSmoothingValue, static_cast<float>(1.f)));
981 throttle = throttleSmoothingValue;
982 }
983
984 if (circleCorrection) {
985 const float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
986 const float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
987 const float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
988 const float throttle_limited = std::max(static_cast<float>(-M_PI_4), std::min(throttle, static_cast<float>(M_PI_4)));
989
990 // Map from unit circle to linear range and limit
991 roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f));
992 pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f));
993 yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f));
994 throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f));
995 }
996
997 if ( exponentialPercent > 0.0 ) {
998 const float exponential = -static_cast<float>(exponentialPercent / 100.0);
999 roll = -exponential * powf(roll, 3) + ((1 + exponential) * roll);
1000 pitch = -exponential * powf(pitch,3) + ((1 + exponential) * pitch);
1001 yaw = -exponential * powf(yaw, 3) + ((1 + exponential) * yaw);
1002 }
1003
1004 // Adjust throttle to 0:1 range
1005 if (throttleModeCenterZero && vehicle->supports()->throttleModeCenterZero()) {
1006 if (!vehicle->supports()->negativeThrust() || !negativeThrust) {
1007 throttle = std::max(0.0f, throttle);
1008 }
1009 } else {
1010 throttle = (throttle + 1.0f) / 2.0f;
1011 }
1012
1013 qCDebug(JoystickVerboseLog)
1014 << name()
1015 << "roll:" << roll
1016 << "pitch:" << -pitch
1017 << "yaw:" << yaw
1018 << "throttle:" << throttle
1019 << "pitchExtension:" << pitchExtension
1020 << "rollExtension:" << rollExtension
1021 << "aux1:" << aux1
1022 << "aux2:" << aux2
1023 << "aux3:" << aux3
1024 << "aux4:" << aux4
1025 << "aux5:" << aux5
1026 << "aux6:" << aux6;
1027
1028 // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits)
1029 // Set up button bitmap
1030 quint64 buttonPressedBits = 0;
1031 for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
1032 const quint64 buttonBit = static_cast<quint64>(1LL << buttonIndex);
1033 if (_buttonEventStates[buttonIndex] == ButtonEventDownTransition || _buttonEventStates[buttonIndex] == ButtonEventRepeat) {
1034 buttonPressedBits |= buttonBit;
1035 }
1036 }
1037
1038 emit axisValues(roll, pitch, yaw, throttle);
1039
1040 const uint16_t lowButtons = static_cast<uint16_t>(buttonPressedBits & 0xFFFF);
1041 const uint16_t highButtons = static_cast<uint16_t>((buttonPressedBits >> 16) & 0xFFFF);
1042
1043
1044 vehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, lowButtons, highButtons, pitchExtension, rollExtension, aux1, aux2, aux3, aux4, aux5, aux6);
1045 }
1046}
1047
1048void Joystick::_startPollingForActiveVehicle()
1049{
1050 Vehicle *activeVehicle = MultiVehicleManager::instance()->activeVehicle();
1051 if (!activeVehicle) {
1052 qCWarning(JoystickLog) << "Internal Error: No active vehicle to poll for";
1053 return;
1054 }
1055 _startPollingForVehicle(*activeVehicle);
1056}
1057
1058void Joystick::_startPollingForVehicle(Vehicle &vehicle)
1059{
1060 if (_currentPollingType == PollingForVehicle) {
1061 qCWarning(JoystickLog) << "Internal Error: Joystick already polling for vehicle!";
1062 return;
1063 }
1064 if (_previousPollingType != NotPolling) {
1065 qCWarning(JoystickLog) << "Internal Error: Joystick previous polling type not None:" << _pollingTypeToString(_previousPollingType);
1066 return;
1067 }
1068 if (_currentPollingType == NotPolling && isRunning()) {
1069 qCWarning(JoystickLog) << "Internal Error: Joystick polling should not be running!";
1070 return;
1071 }
1072
1073 _currentPollingType = PollingForVehicle;
1074 _pollingVehicle = &vehicle;
1075
1076 _buildAvailableButtonsActionList(_pollingVehicle);
1077
1078 (void) connect(this, &Joystick::setArmed, _pollingVehicle, &Vehicle::setArmedShowError);
1079 (void) connect(this, &Joystick::setVtolInFwdFlight, _pollingVehicle, &Vehicle::setVtolInFwdFlight);
1080 (void) connect(this, &Joystick::setFlightMode, _pollingVehicle, &Vehicle::setFlightMode);
1081 (void) connect(this, &Joystick::emergencyStop, _pollingVehicle, &Vehicle::emergencyStop);
1082 (void) connect(this, &Joystick::gripperAction, _pollingVehicle, &Vehicle::sendGripperAction);
1083 (void) connect(this, &Joystick::landingGearDeploy, _pollingVehicle, &Vehicle::landingGearDeploy);
1084 (void) connect(this, &Joystick::landingGearRetract, _pollingVehicle, &Vehicle::landingGearRetract);
1085 (void) connect(this, &Joystick::motorInterlock, _pollingVehicle, &Vehicle::motorInterlock);
1086
1087 (void) connect(_pollingVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged);
1088
1089 if (GimbalController *const gimbal = _pollingVehicle->gimbalController()) {
1090 (void) connect(this, &Joystick::gimbalYawLock, gimbal, &GimbalController::gimbalYawLock);
1091 (void) connect(this, &Joystick::centerGimbal, gimbal, &GimbalController::centerGimbal);
1092 (void) connect(this, &Joystick::gimbalPitchStart, gimbal, &GimbalController::gimbalPitchStart);
1093 (void) connect(this, &Joystick::gimbalYawStart, gimbal, &GimbalController::gimbalYawStart);
1094 (void) connect(this, &Joystick::gimbalPitchStop, gimbal, &GimbalController::gimbalPitchStop);
1095 (void) connect(this, &Joystick::gimbalYawStop, gimbal, &GimbalController::gimbalYawStop);
1096 }
1097
1098 qDebug(JoystickLog) << "Started joystick polling for vehicle" << _pollingVehicle->id();
1099
1100 start();
1101}
1102
1103void Joystick::_startPollingForConfiguration()
1104{
1105 if (_currentPollingType == PollingForConfiguration) {
1106 qCWarning(JoystickLog) << "Internal Error: Joystick already polling for configuration!";
1107 return;
1108 }
1109 if (_previousPollingType != NotPolling) {
1110 qCWarning(JoystickLog) << "Internal Error: Joystick previous polling type not None:" << _pollingTypeToString(_previousPollingType);
1111 return;
1112 }
1113
1114 qCDebug(JoystickLog) << "Started joystick polling for configuration. Saved previous polling type:" << _pollingTypeToString(_currentPollingType);
1115
1116 _previousPollingType = _currentPollingType;
1117 _currentPollingType = PollingForConfiguration;
1118
1119 if (!isRunning()) {
1120 start();
1121 }
1122}
1123
1124void Joystick::_stopPollingForConfiguration()
1125{
1126 if (_currentPollingType != PollingForConfiguration) {
1127 qCWarning(JoystickLog) << "Internal Error: Joystick not polling for configuration!";
1128 return;
1129 }
1130 if (!isRunning()) {
1131 qCWarning(JoystickLog) << "Internal Error: Joystick polling not running!";
1132 return;
1133 }
1134
1135 qCDebug(JoystickLog) << "Stopped joystick polling for configuration. Restored previous polling type:" << _pollingTypeToString(_previousPollingType);
1136
1137 _currentPollingType = _previousPollingType;
1138 _previousPollingType = NotPolling;
1139
1140 if (_currentPollingType == NotPolling) {
1141 _exitThread = true;
1142 }
1143}
1144
1145void Joystick::_stopAllPolling()
1146{
1147 if (_pollingVehicle) {
1148 (void) disconnect(this, nullptr, _pollingVehicle, nullptr);
1149 (void) disconnect(_pollingVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged);
1150 if (GimbalController *const gimbal = _pollingVehicle->gimbalController()) {
1151 (void) disconnect(this, nullptr, gimbal, nullptr);
1152 }
1153 _pollingVehicle = nullptr;
1154 }
1155
1156 qCDebug(JoystickLog) << "Stopped all joystick polling";
1157
1158 _currentPollingType = NotPolling;
1159 _previousPollingType = NotPolling;
1160
1161 if (isRunning()) {
1162 _exitThread = true;
1163 }
1164}
1165
1166QString Joystick::_pollingTypeToString(PollingType pollingType) const
1167{
1168 switch (pollingType) {
1169 case NotPolling:
1170 return QStringLiteral("NotPolling");
1171 case PollingForConfiguration:
1172 return QStringLiteral("PollingForConfiguration");
1173 case PollingForVehicle:
1174 return QStringLiteral("PollingForVehicle");
1175 default:
1176 break;
1177 }
1178
1179 return QStringLiteral("UnknownPollingType");
1180}
1181
1182void Joystick::setAxisCalibration(int axis, const AxisCalibration_t &calibration)
1183{
1184 if (!_validAxis(axis)) {
1185 return;
1186 }
1187
1188 _rgCalibration[axis] = calibration;
1189}
1190
1191Joystick::AxisCalibration_t Joystick::getAxisCalibration(int axis) const
1192{
1193 if (!_validAxis(axis)) {
1194 return AxisCalibration_t{};
1195 }
1196
1197 return _rgCalibration[axis];
1198}
1199
1200void Joystick::_setJoystickAxisForAxisFunction(AxisFunction_t axisFunction, int joystickAxis)
1201{
1202 if (axisFunction == maxAxisFunction) {
1203 qCWarning(JoystickLog) << "Internal Error: maxAxisFunction passed to _setJoystickAxisForAxisFunction";
1204 return;
1205 }
1206 if (joystickAxis == kJoystickAxisNotAssigned) {
1207 qCWarning(JoystickLog) << "Internal Error: kJoystickAxisNotAssigned passed to _setJoystickAxisForAxisFunction";
1208 return;
1209 }
1210
1211 if (!_validAxis(joystickAxis)) {
1212 return;
1213 }
1214
1215 _axisFunctionToJoystickAxisMap[axisFunction] = joystickAxis;
1216}
1217
1218int Joystick::_getJoystickAxisForAxisFunction(AxisFunction_t axisFunction) const
1219{
1220 if (axisFunction == maxAxisFunction) {
1221 qCWarning(JoystickLog) << "Internal Error: maxAxisFunction passed to _getJoystickAxisForAxisFunction";
1222 return kJoystickAxisNotAssigned;
1223 }
1224
1225 return _axisFunctionToJoystickAxisMap.value(axisFunction, kJoystickAxisNotAssigned);
1226}
1227
1228RemoteControlCalibrationController::StickFunction Joystick::mapAxisFunctionToRCCStickFunction(Joystick::AxisFunction_t axisFunction) const
1229{
1230 switch (axisFunction) {
1231 case rollFunction:
1232 return RemoteControlCalibrationController::stickFunctionRoll;
1233 case pitchFunction:
1234 return RemoteControlCalibrationController::stickFunctionPitch;
1235 case yawFunction:
1236 return RemoteControlCalibrationController::stickFunctionYaw;
1237 case throttleFunction:
1238 return RemoteControlCalibrationController::stickFunctionThrottle;
1239 case aux1ExtensionFunction:
1240 return RemoteControlCalibrationController::stickFunctionAux1Extension;
1241 case aux2ExtensionFunction:
1242 return RemoteControlCalibrationController::stickFunctionAux2Extension;
1243 case aux3ExtensionFunction:
1244 return RemoteControlCalibrationController::stickFunctionAux3Extension;
1245 case aux4ExtensionFunction:
1246 return RemoteControlCalibrationController::stickFunctionAux4Extension;
1247 case aux5ExtensionFunction:
1248 return RemoteControlCalibrationController::stickFunctionAux5Extension;
1249 case aux6ExtensionFunction:
1250 return RemoteControlCalibrationController::stickFunctionAux6Extension;
1251 case pitchExtensionFunction:
1252 return RemoteControlCalibrationController::stickFunctionPitchExtension;
1253 case rollExtensionFunction:
1254 return RemoteControlCalibrationController::stickFunctionRollExtension;
1255 case maxAxisFunction:
1256 return RemoteControlCalibrationController::stickFunctionMax;
1257 }
1258 Q_UNREACHABLE();
1259}
1260
1261Joystick::AxisFunction_t Joystick::mapRCCStickFunctionToAxisFunction(RemoteControlCalibrationController::StickFunction stickFunction) const
1262{
1263 switch (stickFunction) {
1264 case RemoteControlCalibrationController::stickFunctionRoll:
1265 return rollFunction;
1266 case RemoteControlCalibrationController::stickFunctionPitch:
1267 return pitchFunction;
1268 case RemoteControlCalibrationController::stickFunctionYaw:
1269 return yawFunction;
1270 case RemoteControlCalibrationController::stickFunctionThrottle:
1271 return throttleFunction;
1272 case RemoteControlCalibrationController::stickFunctionAux1Extension:
1273 return aux1ExtensionFunction;
1274 case RemoteControlCalibrationController::stickFunctionAux2Extension:
1275 return aux2ExtensionFunction;
1276 case RemoteControlCalibrationController::stickFunctionAux3Extension:
1277 return aux3ExtensionFunction;
1278 case RemoteControlCalibrationController::stickFunctionAux4Extension:
1279 return aux4ExtensionFunction;
1280 case RemoteControlCalibrationController::stickFunctionAux5Extension:
1281 return aux5ExtensionFunction;
1282 case RemoteControlCalibrationController::stickFunctionAux6Extension:
1283 return aux6ExtensionFunction;
1284 case RemoteControlCalibrationController::stickFunctionPitchExtension:
1285 return pitchExtensionFunction;
1286 case RemoteControlCalibrationController::stickFunctionRollExtension:
1287 return rollExtensionFunction;
1288 case RemoteControlCalibrationController::stickFunctionMax:
1289 return maxAxisFunction;
1290 }
1291 Q_UNREACHABLE();
1292}
1293
1294void Joystick::setFunctionForChannel(RemoteControlCalibrationController::StickFunction stickFunction, int channel)
1295{
1296 _setJoystickAxisForAxisFunction(mapRCCStickFunctionToAxisFunction(stickFunction), channel);
1297}
1298
1299int Joystick::getChannelForFunction(RemoteControlCalibrationController::StickFunction stickFunction) const
1300{
1301 return _getJoystickAxisForAxisFunction(mapRCCStickFunctionToAxisFunction(stickFunction));
1302}
1303
1304void Joystick::setButtonRepeat(int button, bool repeat)
1305{
1306 if (!_validButton(button) || !_assignedButtonActions[button]) {
1307 return;
1308 }
1309
1310 _assignedButtonActions[button]->repeat = repeat;
1311 _assignedButtonActions[button]->buttonElapsedTimer.start();
1312
1313 QSettings settings;
1314 settings.beginGroup(_joystickSettings.settingsGroup());
1315 settings.beginGroup(QString::fromLatin1(kButtonActionArrayGroup));
1316 settings.beginGroup(QString::number(button));
1317 const QString repeatKey = QString::fromLatin1(kButtonRepeatKey);
1318 settings.setValue(repeatKey, repeat);
1319 settings.endGroup();
1320 settings.endGroup();
1321 settings.endGroup();
1322}
1323
1324bool Joystick::getButtonRepeat(int button)
1325{
1326 if (!_validButton(button) || !_assignedButtonActions[button]) {
1327 return false;
1328 }
1329
1330 return _assignedButtonActions[button]->repeat;
1331}
1332
1333void Joystick::setButtonAction(int button, const QString& actionName)
1334{
1335 if (!_validButton(button)) {
1336 return;
1337 }
1338
1339 qCDebug(JoystickLog) << "button:actionName" << button << actionName;
1340
1341 QSettings settings;
1342 settings.beginGroup(_joystickSettings.settingsGroup());
1343 settings.beginGroup(QString::fromLatin1(kButtonActionArrayGroup));
1344 settings.beginGroup(QString::number(button));
1345 const QString actionNameKey = QString::fromLatin1(kButtonActionNameKey);
1346 const QString repeatKey = QString::fromLatin1(kButtonRepeatKey);
1347
1348 if (actionName.isEmpty() || actionName == _buttonActionNone) {
1349 if (_assignedButtonActions[button]) {
1350 delete _assignedButtonActions[button];
1351 _assignedButtonActions[button] = nullptr;
1352 }
1353 settings.setValue(actionNameKey, _buttonActionNone);
1354 settings.setValue(repeatKey, false);
1355 } else {
1356 if (!_assignedButtonActions[button]) {
1357 _assignedButtonActions[button] = new AssignedButtonAction(actionName, false /* repeat */);
1358 } else {
1359 _assignedButtonActions[button]->actionName = actionName;
1360 //-- Make sure repeat is off if this action doesn't support repeats
1361 const int idx = _findAvailableButtonActionIndex(actionName);
1362 if (idx >= 0) {
1363 const AvailableButtonAction *const buttonAction = qobject_cast<const AvailableButtonAction*>(_availableButtonActions->get(idx));
1364 if (!buttonAction->canRepeat()) {
1365 _assignedButtonActions[button]->repeat = false;
1366 }
1367 }
1368 }
1369
1370 settings.setValue(actionNameKey, _assignedButtonActions[button]->actionName);
1371 settings.setValue(repeatKey, _assignedButtonActions[button]->repeat);
1372 }
1373
1374 settings.endGroup();
1375 settings.endGroup();
1376 settings.endGroup();
1377
1378 emit buttonActionsChanged();
1379}
1380
1381QString Joystick::getButtonAction(int button) const
1382{
1383 if (_validButton(button)) {
1384 if (_assignedButtonActions[button]) {
1385 return _assignedButtonActions[button]->actionName;
1386 }
1387 }
1388
1389 return QString(_buttonActionNone);
1390}
1391
1392QStringList Joystick::buttonActions() const
1393{
1394 QStringList list;
1395
1396 list.reserve(_totalButtonCount);
1397
1398 for (int button = 0; button < _totalButtonCount; button++) {
1399 if (_assignedButtonActions[button]) {
1400 list << _assignedButtonActions[button]->actionName;
1401 } else {
1402 list << _buttonActionNone;
1403 }
1404 }
1405
1406 return list;
1407}
1408
1409void Joystick::_executeButtonAction(const QString &action, const ButtonEvent_t buttonEvent)
1410{
1411 Vehicle *const vehicle = _pollingVehicle;
1412 if (!vehicle) {
1413 qCWarning(JoystickLog) << "Internal Error: No vehicle for joystick!";
1414 return;
1415 }
1416 if (!_joystickManager->activeJoystickEnabledForActiveVehicle()) {
1417 qCWarning(JoystickLog) << "Internal Error: Joystick not enabled for vehicle!";
1418 return;
1419 }
1420 if (action == _buttonActionNone) {
1421 return;
1422 }
1423
1424 struct ActionInfo {
1425 const QString action;
1426 const ButtonEvent_t event;
1427 std::function<void()> func;
1428 };
1429 auto actionInfo = std::to_array<ActionInfo>({
1430 { _buttonActionArm, ButtonEventDownTransition, [this]() { emit setArmed(true); } },
1431 { _buttonActionDisarm, ButtonEventDownTransition, [this]() { emit setArmed(false); } },
1432 { _buttonActionToggleArm, ButtonEventDownTransition, [this, vehicle]() { emit setArmed(!vehicle->armed()); } },
1433 { _buttonActionVTOLFixedWing, ButtonEventDownTransition, [this]() { emit setVtolInFwdFlight(true); } },
1434 { _buttonActionVTOLMultiRotor, ButtonEventDownTransition, [this]() { emit setVtolInFwdFlight(false); } },
1435 { _buttonActionTriggerCamera, ButtonEventDownTransition, [this]() { emit triggerCamera(); } },
1436 { _buttonActionContinuousZoomIn, ButtonEventDownTransition, [this]() { emit startContinuousZoom(1); } },
1437 { _buttonActionContinuousZoomIn, ButtonEventRepeat, [this]() { emit startContinuousZoom(1); } },
1438 { _buttonActionContinuousZoomOut, ButtonEventDownTransition, [this]() { emit startContinuousZoom(-1); } },
1439 { _buttonActionContinuousZoomOut, ButtonEventRepeat, [this]() { emit startContinuousZoom(-1); } },
1440 { _buttonActionStepZoomIn, ButtonEventDownTransition, [this]() { emit stepZoom(1); } },
1441 { _buttonActionStepZoomIn, ButtonEventRepeat, [this]() { emit stepZoom(1); } },
1442 { _buttonActionStepZoomOut, ButtonEventDownTransition, [this]() { emit stepZoom(-1); } },
1443 { _buttonActionStepZoomOut, ButtonEventRepeat, [this]() { emit stepZoom(-1); } },
1444 { _buttonActionNextStream, ButtonEventDownTransition, [this]() { emit stepStream(1); } },
1445 { _buttonActionPreviousStream, ButtonEventDownTransition, [this]() { emit stepStream(-1); } },
1446 { _buttonActionNextCamera, ButtonEventDownTransition, [this]() { emit stepCamera(1); } },
1447 { _buttonActionPreviousCamera, ButtonEventDownTransition, [this]() { emit stepCamera(-1); } },
1448 { _buttonActionGimbalUp, ButtonEventDownTransition, [this]() { emit gimbalPitchStart(1); } },
1449 { _buttonActionGimbalUp, ButtonEventUpTransition, [this]() { emit gimbalPitchStop(); } },
1450 { _buttonActionGimbalDown, ButtonEventDownTransition, [this]() { emit gimbalPitchStart(-1); } },
1451 { _buttonActionGimbalDown, ButtonEventUpTransition, [this]() { emit gimbalPitchStop(); } },
1452 { _buttonActionGimbalLeft, ButtonEventDownTransition, [this]() { emit gimbalYawStart(-1); } },
1453 { _buttonActionGimbalLeft, ButtonEventUpTransition, [this]() { emit gimbalYawStop(); } },
1454 { _buttonActionGimbalRight, ButtonEventDownTransition, [this]() { emit gimbalYawStart(1); } },
1455 { _buttonActionGimbalRight, ButtonEventUpTransition, [this]() { emit gimbalYawStop(); } },
1456 { _buttonActionStartVideoRecord, ButtonEventDownTransition, [this]() { emit startVideoRecord(); } },
1457 { _buttonActionStopVideoRecord, ButtonEventDownTransition, [this]() { emit stopVideoRecord(); } },
1458 { _buttonActionToggleVideoRecord, ButtonEventDownTransition, [this]() { emit toggleVideoRecord(); } },
1459 { _buttonActionGimbalCenter, ButtonEventDownTransition, [this]() { emit centerGimbal(); } },
1460 { _buttonActionGimbalYawLock, ButtonEventDownTransition, [this]() { emit gimbalYawLock(true); } },
1461 { _buttonActionGimbalYawFollow, ButtonEventDownTransition, [this]() { emit gimbalYawLock(false); } },
1462 { _buttonActionEmergencyStop, ButtonEventDownTransition, [this]() { emit emergencyStop(); } },
1463 { _buttonActionGripperGrab, ButtonEventDownTransition, [this]() { emit gripperAction(QGCMAVLink::GripperActionGrab); } },
1464 { _buttonActionGripperRelease, ButtonEventDownTransition, [this]() { emit gripperAction(QGCMAVLink::GripperActionRelease); } },
1465 { _buttonActionGripperHold, ButtonEventDownTransition, [this]() { emit gripperAction(QGCMAVLink::GripperActionHold); } },
1466 { _buttonActionLandingGearDeploy, ButtonEventDownTransition, [this]() { emit landingGearDeploy(); } },
1467 { _buttonActionLandingGearRetract, ButtonEventDownTransition, [this]() { emit landingGearRetract(); } },
1468 { _buttonActionMotorInterlockEnable, ButtonEventDownTransition, [this]() { emit motorInterlock(true); } },
1469 { _buttonActionMotorInterlockDisable, ButtonEventDownTransition, [this]() { emit motorInterlock(false); } },
1470 });
1471
1472 // First check for flight mode match
1473 if (vehicle->flightModes().contains(action)) {
1474 if (buttonEvent == ButtonEventDownTransition) {
1475 emit setFlightMode(action);
1476 return;
1477 }
1478 }
1479
1480 // Now look for an action match
1481 auto it = std::find_if(actionInfo.begin(), actionInfo.end(), [&action, &buttonEvent](const ActionInfo &info) {
1482 return (info.action == action) && (info.event == buttonEvent);
1483 });
1484 if (it != actionInfo.end()) {
1485 it->func();
1486 return;
1487 }
1488
1489 // Finally let mavlink actions have a go
1490 if (buttonEvent == ButtonEventDownTransition) {
1491 emit unknownAction(action);
1492 for (int i = 0; i<_mavlinkActionManager->actions()->count(); i++) {
1493 MavlinkAction *const mavlinkAction = _mavlinkActionManager->actions()->value<MavlinkAction*>(i);
1494 if (action == mavlinkAction->label()) {
1495 mavlinkAction->sendTo(vehicle);
1496 return;
1497 }
1498 }
1499 }
1500}
1501
1502bool Joystick::_validAxis(int axis) const
1503{
1504 if ((axis >= 0) && (axis < _axisCount)) {
1505 return true;
1506 }
1507
1508 qCWarning(JoystickLog) << "Invalid axis index" << axis;
1509 return false;
1510}
1511
1512bool Joystick::_validButton(int button) const
1513{
1514 if ((button >= 0) && (button < _totalButtonCount)) {
1515 return true;
1516 }
1517
1518 qCWarning(JoystickLog) << "Invalid button index" << button;
1519 return false;
1520}
1521
1522int Joystick::_findAvailableButtonActionIndex(const QString &action)
1523{
1524 for (int i = 0; i < _availableButtonActions->count(); i++) {
1525 const AvailableButtonAction *const buttonAction = qobject_cast<const AvailableButtonAction*>(_availableButtonActions->get(i));
1526 if (buttonAction->action() == action) {
1527 return i;
1528 }
1529 }
1530
1531 return -1;
1532}
1533
1534void Joystick::_buildAvailableButtonsActionList(Vehicle *vehicle)
1535{
1536 if (_availableButtonActions->count()) {
1537 _availableButtonActions->clearAndDeleteContents();
1538 }
1539 _availableActionTitles.clear();
1540
1541 _availableButtonActions->append(new AvailableButtonAction(_buttonActionNone, false));
1542 _availableButtonActions->append(new AvailableButtonAction(_buttonActionArm, false));
1543 _availableButtonActions->append(new AvailableButtonAction(_buttonActionDisarm, false));
1544 _availableButtonActions->append(new AvailableButtonAction(_buttonActionToggleArm, false));
1545 if (vehicle) {
1546 const QStringList list = vehicle->flightModes();
1547 for (const QString &mode : list) {
1548 _availableButtonActions->append(new AvailableButtonAction(mode, false));
1549 }
1550 }
1551
1552 _availableButtonActions->append(new AvailableButtonAction(_buttonActionVTOLFixedWing, false));
1553 _availableButtonActions->append(new AvailableButtonAction(_buttonActionVTOLMultiRotor, false));
1554 _availableButtonActions->append(new AvailableButtonAction(_buttonActionContinuousZoomIn, true));
1555 _availableButtonActions->append(new AvailableButtonAction(_buttonActionContinuousZoomOut, true));
1556 _availableButtonActions->append(new AvailableButtonAction(_buttonActionStepZoomIn, true));
1557 _availableButtonActions->append(new AvailableButtonAction(_buttonActionStepZoomOut, true));
1558 _availableButtonActions->append(new AvailableButtonAction(_buttonActionNextStream, false));
1559 _availableButtonActions->append(new AvailableButtonAction(_buttonActionPreviousStream, false));
1560 _availableButtonActions->append(new AvailableButtonAction(_buttonActionNextCamera, false));
1561 _availableButtonActions->append(new AvailableButtonAction(_buttonActionPreviousCamera, false));
1562 _availableButtonActions->append(new AvailableButtonAction(_buttonActionTriggerCamera, false));
1563 _availableButtonActions->append(new AvailableButtonAction(_buttonActionStartVideoRecord, false));
1564 _availableButtonActions->append(new AvailableButtonAction(_buttonActionStopVideoRecord, false));
1565 _availableButtonActions->append(new AvailableButtonAction(_buttonActionToggleVideoRecord, false));
1566 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalDown, false));
1567 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalUp, false));
1568 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalLeft, false));
1569 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalRight, false));
1570 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalCenter, false));
1571 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalYawLock, false));
1572 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGimbalYawFollow, false));
1573 _availableButtonActions->append(new AvailableButtonAction(_buttonActionEmergencyStop, false));
1574 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGripperGrab, false));
1575 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGripperRelease, false));
1576 _availableButtonActions->append(new AvailableButtonAction(_buttonActionGripperHold, false));
1577 _availableButtonActions->append(new AvailableButtonAction(_buttonActionLandingGearDeploy, false));
1578 _availableButtonActions->append(new AvailableButtonAction(_buttonActionLandingGearRetract, false));
1579#ifndef QGC_NO_ARDUPILOT_DIALECT
1580 _availableButtonActions->append(new AvailableButtonAction(_buttonActionMotorInterlockEnable, false));
1581 _availableButtonActions->append(new AvailableButtonAction(_buttonActionMotorInterlockDisable, false));
1582#endif
1583
1584 const auto customActions = QGCCorePlugin::instance()->joystickActions();
1585 for (const auto &action : customActions) {
1586 _availableButtonActions->append(new AvailableButtonAction(action.name, action.canRepeat));
1587 }
1588
1589 for (int i = 0; i < _mavlinkActionManager->actions()->count(); i++) {
1590 const MavlinkAction *const mavlinkAction = _mavlinkActionManager->actions()->value<const MavlinkAction*>(i);
1591 _availableButtonActions->append(new AvailableButtonAction(mavlinkAction->label(), false));
1592 }
1593
1594 for (int i = 0; i < _availableButtonActions->count(); i++) {
1595 const AvailableButtonAction *const buttonAction = qobject_cast<const AvailableButtonAction*>(_availableButtonActions->get(i));
1596 _availableActionTitles << buttonAction->action();
1597 }
1598
1599 emit assignableActionsChanged();
1600}
1601
1602QString Joystick::axisFunctionToString(AxisFunction_t function)
1603{
1604 switch (function) {
1605 case rollFunction:
1606 return QStringLiteral("Roll");
1607 case pitchFunction:
1608 return QStringLiteral("Pitch");
1609 case yawFunction:
1610 return QStringLiteral("Yaw");
1611 case throttleFunction:
1612 return QStringLiteral("Throttle");
1613 case aux1ExtensionFunction:
1614 return QStringLiteral("Aux 1 Extension");
1615 case aux2ExtensionFunction:
1616 return QStringLiteral("Aux 2 Extension");
1617 case aux3ExtensionFunction:
1618 return QStringLiteral("Aux 3 Extension");
1619 case aux4ExtensionFunction:
1620 return QStringLiteral("Aux 4 Extension");
1621 case aux5ExtensionFunction:
1622 return QStringLiteral("Aux 5 Extension");
1623 case aux6ExtensionFunction:
1624 return QStringLiteral("Aux 6 Extension");
1625 case pitchExtensionFunction:
1626 return QStringLiteral("Pitch Extension");
1627 case rollExtensionFunction:
1628 return QStringLiteral("Roll Extension");
1629 case maxAxisFunction:
1630 return QStringLiteral("Unassigned");
1631 }
1632 Q_UNREACHABLE();
1633}
1634
1635void Joystick::startConfiguration()
1636{
1637 _startPollingForConfiguration();
1638}
1639
1640void Joystick::stopConfiguration()
1641{
1642 _stopPollingForConfiguration();
1643}
1644
1645Joystick::AxisFunction_t Joystick::_getAxisFunctionForJoystickAxis(int joystickAxis) const
1646{
1647 if (!_validAxis(joystickAxis)) {
1648 return maxAxisFunction;
1649 }
1650
1651 for (int i = 0; i < maxAxisFunction; i++) {
1652 auto axisFunction = static_cast<AxisFunction_t>(i);
1653 if (_axisFunctionToJoystickAxisMap.value(axisFunction, kJoystickAxisNotAssigned) == joystickAxis) {
1654 return axisFunction;
1655 }
1656 }
1657
1658 return maxAxisFunction;
1659}
1660
1661void Joystick::stop()
1662{
1663 _exitThread = true;
1664 if (isRunning()) {
1665 if (QThread::currentThread() == this) {
1666 qCWarning(JoystickLog) << "Skipping wait() on joystick thread";
1667 } else {
1668 wait();
1669 }
1670 }
1671}
1672
1673void Joystick::setLinkedGroupId(const QString &groupId)
1674{
1675 if (_linkedGroupId != groupId) {
1676 _linkedGroupId = groupId;
1677
1678 // Persist to settings
1679 QSettings settings;
1680 settings.beginGroup(QStringLiteral("Joystick"));
1681 settings.beginGroup(_name);
1682 if (groupId.isEmpty()) {
1683 settings.remove(QStringLiteral("LinkedGroupId"));
1684 } else {
1685 settings.setValue(QStringLiteral("LinkedGroupId"), groupId);
1686 }
1687 settings.endGroup();
1688 settings.endGroup();
1689
1690 emit linkedGroupChanged();
1691 }
1692}
1693
1694void Joystick::setLinkedGroupRole(const QString &role)
1695{
1696 if (_linkedGroupRole != role) {
1697 _linkedGroupRole = role;
1698
1699 // Persist to settings
1700 QSettings settings;
1701 settings.beginGroup(QStringLiteral("Joystick"));
1702 settings.beginGroup(_name);
1703 if (role.isEmpty()) {
1704 settings.remove(QStringLiteral("LinkedGroupRole"));
1705 } else {
1706 settings.setValue(QStringLiteral("LinkedGroupRole"), role);
1707 }
1708 settings.endGroup();
1709 settings.endGroup();
1710
1711 emit linkedGroupChanged();
1712 }
1713}
QGC_LOGGING_CATEGORY
#define QGC_LOGGING_CATEGORY(name, categoryStr)
Definition QGCLoggingCategory.h:14
AssignedButtonAction::AssignedButtonAction
AssignedButtonAction(const QString &name, bool repeat)
Definition Joystick.cc:40
Fact
A Fact is used to hold a single value within the system.
Definition Fact.h:19
Fact::rawValueChanged
void rawValueChanged(const QVariant &value)
GimbalController::gimbalYawLock
void gimbalYawLock(bool yawLock)
Definition GimbalController.h:55
GimbalController::gimbalYawStart
void gimbalYawStart(int direction)
Definition GimbalController.cc:388
GimbalController::gimbalPitchStart
void gimbalPitchStart(int direction)
Definition GimbalController.cc:375
JoystickSettings::negativeThrust
Fact *negativeThrust READ negativeThrust CONSTANT Fact * negativeThrust()
Definition JoystickSettings.cc:24
JoystickSettings::axisFrequencyHz
Fact *axisFrequencyHz READ axisFrequencyHz CONSTANT Fact * axisFrequencyHz()
Definition JoystickSettings.cc:26
JoystickSettings::throttleModeCenterZero
Fact *throttleModeCenterZero READ throttleModeCenterZero CONSTANT Fact * throttleModeCenterZero()
Definition JoystickSettings.cc:28
JoystickSettings::enableManualControlAux2
Fact *enableManualControlAux2 READ enableManualControlAux2 CONSTANT Fact * enableManualControlAux2()
Definition JoystickSettings.cc:32
JoystickSettings::circleCorrection
Fact *circleCorrection READ circleCorrection CONSTANT Fact * circleCorrection()
Definition JoystickSettings.cc:22
JoystickSettings::throttleSmoothing
Fact *throttleSmoothing READ throttleSmoothing CONSTANT Fact * throttleSmoothing()
Definition JoystickSettings.cc:25
JoystickSettings::transmitterMode
Fact *transmitterMode READ transmitterMode CONSTANT Fact * transmitterMode()
Definition JoystickSettings.cc:29
JoystickSettings::enableManualControlAux6
Fact *enableManualControlAux6 READ enableManualControlAux6 CONSTANT Fact * enableManualControlAux6()
Definition JoystickSettings.cc:36
JoystickSettings::calibrated
Fact *calibrated READ calibrated CONSTANT Fact * calibrated()
Definition JoystickSettings.cc:21
JoystickSettings::buttonFrequencyHz
Fact *buttonFrequencyHz READ buttonFrequencyHz CONSTANT Fact * buttonFrequencyHz()
Definition JoystickSettings.cc:27
JoystickSettings::enableManualControlRollExtension
Fact *enableManualControlRollExtension READ enableManualControlRollExtension CONSTANT Fact * enableManualControlRollExtension()
Definition JoystickSettings.cc:38
JoystickSettings::enableManualControlPitchExtension
Fact *enableManualControlPitchExtension READ enableManualControlPitchExtension CONSTANT Fact * enableManualControlPitchExtension()
Definition JoystickSettings.cc:37
JoystickSettings::exponentialPct
Fact *exponentialPct READ exponentialPct CONSTANT Fact * exponentialPct()
Definition JoystickSettings.cc:30
JoystickSettings::enableManualControlAux3
Fact *enableManualControlAux3 READ enableManualControlAux3 CONSTANT Fact * enableManualControlAux3()
Definition JoystickSettings.cc:33
JoystickSettings::enableManualControlAux4
Fact *enableManualControlAux4 READ enableManualControlAux4 CONSTANT Fact * enableManualControlAux4()
Definition JoystickSettings.cc:34
JoystickSettings::enableManualControlAux5
Fact *enableManualControlAux5 READ enableManualControlAux5 CONSTANT Fact * enableManualControlAux5()
Definition JoystickSettings.cc:35
JoystickSettings::useDeadband
Fact *useDeadband READ useDeadband CONSTANT Fact * useDeadband()
Definition JoystickSettings.cc:23
JoystickSettings::enableManualControlAux1
Fact *enableManualControlAux1 READ enableManualControlAux1 CONSTANT Fact * enableManualControlAux1()
Definition JoystickSettings.cc:31
Joystick::setLinkedGroupId
void setLinkedGroupId(const QString &groupId)
Definition Joystick.cc:1673
Joystick::stopConfiguration
void stopConfiguration()
Tells the joystick that the configuration UI is being closed so it can do any special processing requ...
Definition Joystick.cc:1640
Joystick::rawButtonPressedChanged
void rawButtonPressedChanged(int index, bool pressed)
Signalled during PollingForConfiguration.
Joystick::setArmed
void setArmed(bool arm)
Joystick::gimbalYawLock
void gimbalYawLock(bool lock)
Joystick::_buttonCount
int _buttonCount
Definition Joystick.h:378
Joystick::triggerCamera
void triggerCamera()
Joystick::~Joystick
virtual ~Joystick()
Definition Joystick.cc:146
Joystick::aux6ExtensionFunction
@ aux6ExtensionFunction
Definition Joystick.h:142
Joystick::aux2ExtensionFunction
@ aux2ExtensionFunction
Definition Joystick.h:138
Joystick::pitchExtensionFunction
@ pitchExtensionFunction
Definition Joystick.h:135
Joystick::rollFunction
@ rollFunction
Definition Joystick.h:131
Joystick::yawFunction
@ yawFunction
Definition Joystick.h:133
Joystick::maxAxisFunction
@ maxAxisFunction
Definition Joystick.h:143
Joystick::aux5ExtensionFunction
@ aux5ExtensionFunction
Definition Joystick.h:141
Joystick::aux3ExtensionFunction
@ aux3ExtensionFunction
Definition Joystick.h:139
Joystick::throttleFunction
@ throttleFunction
Definition Joystick.h:134
Joystick::rollExtensionFunction
@ rollExtensionFunction
Definition Joystick.h:136
Joystick::aux4ExtensionFunction
@ aux4ExtensionFunction
Definition Joystick.h:140
Joystick::aux1ExtensionFunction
@ aux1ExtensionFunction
Definition Joystick.h:137
Joystick::pitchFunction
@ pitchFunction
Definition Joystick.h:132
Joystick::name
QString name() const
Definition Joystick.h:198
Joystick::mapAxisFunctionToRCCStickFunction
RemoteControlCalibrationController::StickFunction mapAxisFunctionToRCCStickFunction(AxisFunction_t axisFunction) const
Definition Joystick.cc:1228
Joystick::startVideoRecord
void startVideoRecord()
Joystick::_name
QString _name
Definition Joystick.h:376
Joystick::_hatCount
int _hatCount
Definition Joystick.h:379
Joystick::setLinkedGroupRole
void setLinkedGroupRole(const QString &role)
Definition Joystick.cc:1694
Joystick::setFlightMode
void setFlightMode(const QString &flightMode)
Joystick::motorInterlock
void motorInterlock(bool enable)
Joystick::axisFunctionToString
static QString axisFunctionToString(AxisFunction_t function)
Definition Joystick.cc:1602
Joystick::setFunctionForChannel
void setFunctionForChannel(RemoteControlCalibrationController::StickFunction stickFunction, int channel)
Definition Joystick.cc:1294
Joystick::axisValues
void axisValues(float roll, float pitch, float yaw, float throttle)
Joystick::landingGearDeploy
void landingGearDeploy()
Joystick::buttonActionsChanged
void buttonActionsChanged()
Joystick::stop
void stop()
Definition Joystick.cc:1661
Joystick::ButtonEventDownTransition
@ ButtonEventDownTransition
Definition Joystick.h:109
Joystick::ButtonEventNone
@ ButtonEventNone
Definition Joystick.h:111
Joystick::ButtonEventRepeat
@ ButtonEventRepeat
Definition Joystick.h:110
Joystick::ButtonEventUpTransition
@ ButtonEventUpTransition
Definition Joystick.h:108
Joystick::assignableActionsChanged
void assignableActionsChanged()
Joystick::centerGimbal
void centerGimbal()
Joystick::unknownAction
void unknownAction(const QString &action)
Joystick::getChannelForFunction
int getChannelForFunction(RemoteControlCalibrationController::StickFunction stickFunction) const
Definition Joystick.cc:1299
Joystick::getButtonAction
QString getButtonAction(int button) const
Definition Joystick.cc:1381
Joystick::stopVideoRecord
void stopVideoRecord()
Joystick::stepZoom
void stepZoom(int direction)
Joystick::buttonActions
QStringList buttonActions() const
Definition Joystick.cc:1392
Joystick::mapRCCStickFunctionToAxisFunction
AxisFunction_t mapRCCStickFunctionToAxisFunction(RemoteControlCalibrationController::StickFunction stickFunction) const
Definition Joystick.cc:1261
Joystick::startConfiguration
void startConfiguration()
Tells the joystick that the configuration UI is being displayed so it can do any special processing r...
Definition Joystick.cc:1635
Joystick::axisCount
QString name READ name int axisCount
Definition Joystick.h:101
Joystick::getAxisCalibration
Joystick::AxisCalibration_t getAxisCalibration(int axis) const
Definition Joystick.cc:1191
Joystick::setAxisCalibration
void setAxisCalibration(int axis, const AxisCalibration_t &calibration)
Definition Joystick.cc:1182
Joystick::gripperAction
void gripperAction(QGCMAVLink::GripperActions gripperAction)
Joystick::gimbalPitchStop
void gimbalPitchStop()
Joystick::_axisCount
int _axisCount
Definition Joystick.h:377
Joystick::gimbalYawStop
void gimbalYawStop()
Joystick::startContinuousZoom
void startContinuousZoom(int direction)
Joystick::settings
JoystickSettings * settings()
Definition Joystick.h:197
Joystick::emergencyStop
void emergencyStop()
Joystick::getButtonRepeat
bool getButtonRepeat(int button)
Definition Joystick.cc:1324
Joystick::setButtonAction
void setButtonAction(int button, const QString &action)
Definition Joystick.cc:1333
Joystick::stepCamera
void stepCamera(int direction)
Joystick::setVtolInFwdFlight
void setVtolInFwdFlight(bool set)
Joystick::stepStream
void stepStream(int direction)
Joystick::landingGearRetract
void landingGearRetract()
Joystick::toggleVideoRecord
void toggleVideoRecord()
Joystick::gimbalPitchStart
void gimbalPitchStart(int direction)
Joystick::linkedGroupChanged
void linkedGroupChanged()
Joystick::rawChannelValuesChanged
void rawChannelValuesChanged(QVector< int > channelValues)
Signalled during PollingForConfiguration.
Joystick::gimbalYawStart
void gimbalYawStart(int direction)
Joystick::setButtonRepeat
void setButtonRepeat(int button, bool repeat)
Definition Joystick.cc:1304
QmlObjectListModel::append
void append(QObject *object)
Caller maintains responsibility for object ownership and deletion.
Definition QmlObjectListModel.cc:233
QmlObjectListModel::value
T value(int index) const
Definition QmlObjectListModel.h:39
QmlObjectListModel::get
QObject * get(int index)
Definition QmlObjectListModel.cc:59
QmlObjectListModel::count
int count() const override final
Definition QmlObjectListModel.cc:252
QmlObjectListModel::clearAndDeleteContents
void clearAndDeleteContents() override final
Clears the list and calls deleteLater on each entry.
Definition QmlObjectListModel.cc:273
SettingsGroup::settingsGroup
QString settingsGroup() const
Definition SettingsGroup.h:55
SettingsManager
Provides access to all app settings.
Definition SettingsManager.h:38
VehicleSupports::negativeThrust
bool negativeThrust() const
Definition VehicleSupports.cc:18
Vehicle::motorInterlock
void motorInterlock(bool enable)
Command vehicle to Enable/Disable Motor Interlock.
Definition Vehicle.cc:4395
Vehicle::sendGripperAction
void sendGripperAction(QGCMAVLink::GripperActions gripperOption)
Definition Vehicle.cc:4057
Vehicle::emergencyStop
void emergencyStop()
Command vehicle to kill all motors no matter what state.
Definition Vehicle.cc:2244
Vehicle::id
int id() const
Definition Vehicle.h:425
Vehicle::setFlightMode
void setFlightMode(const QString &flightMode)
Definition Vehicle.cc:1559
Vehicle::landingGearDeploy
void landingGearDeploy()
Command vichecle to deploy landing gear.
Definition Vehicle.cc:2254
Vehicle::flightModesChanged
void flightModesChanged()
Vehicle::gimbalController
GimbalController * gimbalController()
Definition Vehicle.h:775
Vehicle::setVtolInFwdFlight
void setVtolInFwdFlight(bool vtolInFwdFlight)
Definition Vehicle.cc:3360
Vehicle::sendJoystickDataThreadSafe
void sendJoystickDataThreadSafe(float roll, float pitch, float yaw, float thrust, quint16 buttons, quint16 buttons2, float pitchExtension, float rollExtension, float aux1, float aux2, float aux3, float aux4, float aux5, float aux6)
Definition Vehicle.cc:3990
Vehicle::armed
bool armed() const
Definition Vehicle.h:449
Vehicle::landingGearRetract
void landingGearRetract()
Command vichecle to retract landing gear.
Definition Vehicle.cc:2264
Vehicle::supports
VehicleSupports * supports()
Definition Vehicle.h:405
Vehicle::flightModes
QStringList flightModes()
Definition Vehicle.cc:1543
Vehicle::setArmedShowError
void setArmedShowError(bool armed)
Definition Vehicle.h:451
Joystick::AxisCalibration_t
Definition Joystick.h:114