diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index c8f411967d7..406877c7fae 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -9,102 +9,114 @@ #include "Joystick.h" -#include "QGC.h" -#include "QGCApplication.h" #include "CustomAction.h" -#include "SettingsManager.h" +#include "CustomActionManager.h" #include "CustomMavlinkActionsSettings.h" -#include "Vehicle.h" -#include "MultiVehicleManager.h" #include "FirmwarePlugin.h" -#include "QGCLoggingCategory.h" #include "GimbalController.h" +#include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" +#include "SettingsManager.h" +#include "Vehicle.h" #include -// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle -QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") +QGC_LOGGING_CATEGORY(JoystickLog, "qgc.joystick.joystick") +QGC_LOGGING_CATEGORY(JoystickValuesLog, "qgc.joystick.joystickvalues") int Joystick::_transmitterMode = 2; -AssignedButtonAction::AssignedButtonAction(QObject* parent, const QString name) - : QObject(parent) - , action(name) +/*===========================================================================*/ + +AssignedButtonAction::AssignedButtonAction(const QString &name) + : action(name) { + // qCDebug(JoystickLog) << Q_FUNC_INFO << this; } -AssignableButtonAction::AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_) +AssignableButtonAction::AssignableButtonAction(const QString &action_, bool canRepeat_, QObject *parent) : QObject(parent) , _action(action_) , _repeat(canRepeat_) { + // qCDebug(JoystickLog) << Q_FUNC_INFO << this; } -Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager) - : _name (name) - , _axisCount (axisCount) - , _buttonCount (buttonCount) - , _hatCount (hatCount) - , _hatButtonCount (4 * hatCount) - , _totalButtonCount (_buttonCount+_hatButtonCount) - , _multiVehicleManager (multiVehicleManager) - , _customActionManager (qgcApp()->toolbox()->settingsManager()->customMavlinkActionsSettings()->joystickActionsFile()) +/*===========================================================================*/ + +Joystick::Joystick(const QString &name, int axisCount, int buttonCount, int hatCount, QObject *parent) + : QThread(parent) + , _name(name) + , _axisCount(axisCount) + , _buttonCount(buttonCount) + , _hatCount(hatCount) + , _hatButtonCount(4 * hatCount) + , _totalButtonCount(_buttonCount + _hatButtonCount) + , _rgAxisValues(new int[static_cast(_axisCount)]) + , _rgCalibration(new Calibration_t[static_cast(_axisCount)]) + , _rgButtonValues(new uint8_t[static_cast(_totalButtonCount)]) + , _customActionManager(new CustomActionManager(qgcApp()->toolbox()->settingsManager()->customMavlinkActionsSettings()->joystickActionsFile(), this)) + , _assignableButtonActions(new QmlObjectListModel(this)) { // qCDebug(JoystickLog) << Q_FUNC_INFO << this; - qRegisterMetaType(); - - _rgAxisValues = new int[static_cast(_axisCount)]; - _rgCalibration = new Calibration_t[static_cast(_axisCount)]; - _rgButtonValues = new uint8_t[static_cast(_totalButtonCount)]; for (int i = 0; i < _axisCount; i++) { _rgAxisValues[i] = 0; } + for (int i = 0; i < _totalButtonCount; i++) { _rgButtonValues[i] = BUTTON_UP; - _buttonActionArray.append(nullptr); + (void) _buttonActionArray.append(nullptr); } - _buildActionList(_multiVehicleManager->activeVehicle()); - _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); + + MultiVehicleManager *const multiVehicleManager = qgcApp()->toolbox()->multiVehicleManager(); + _buildActionList(multiVehicleManager->activeVehicle()); + _updateTXModeSettingsKey(multiVehicleManager->activeVehicle()); + _loadSettings(); - connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); - connect(qgcApp()->toolbox()->multiVehicleManager()->vehicles(), &QmlObjectListModel::countChanged, this, &Joystick::_vehicleCountChanged); -} -void Joystick::stop() -{ - _exitThread = true; - wait(); + (void) connect(multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); + (void) connect(multiVehicleManager->vehicles(), &QmlObjectListModel::countChanged, this, &Joystick::_vehicleCountChanged); } Joystick::~Joystick() { if (!_exitThread) { - qWarning() << "Joystick thread still running!"; + qCWarning(JoystickLog) << "thread still running!"; } + delete[] _rgAxisValues; delete[] _rgCalibration; delete[] _rgButtonValues; - _assignableButtonActions.clearAndDeleteContents(); - for (int button = 0; button < _totalButtonCount; button++) { - if(_buttonActionArray[button]) { - _buttonActionArray[button]->deleteLater(); - } - } + + _assignableButtonActions->clearAndDeleteContents(); + qDeleteAll(_buttonActionArray); // qCDebug(JoystickLog) << Q_FUNC_INFO << this; } -void Joystick::_setDefaultCalibration(void) { +void Joystick::stop() +{ + _exitThread = true; + wait(); +} + +void Joystick::_setDefaultCalibration() +{ QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); // Only set default calibrations if we do not have a calibration for this gamecontroller - if(_calibrated) return; + if (_calibrated) { + return; + } - for(int axis = 0; axis < _axisCount; axis++) { + for (int axis = 0; axis < _axisCount; axis++) { Joystick::Calibration_t calibration; _rgCalibration[axis] = calibration; } @@ -113,68 +125,64 @@ void Joystick::_setDefaultCalibration(void) { _rgCalibration[3].reversed = true; // Default TX Mode 2 axis assignments for gamecontrollers - _rgFunctionAxis[rollFunction] = 2; - _rgFunctionAxis[pitchFunction] = 3; - _rgFunctionAxis[yawFunction] = 0; - _rgFunctionAxis[throttleFunction] = 1; - - _rgFunctionAxis[gimbalPitchFunction]= 4; - _rgFunctionAxis[gimbalYawFunction] = 5; - - _exponential = 0; - _accumulator = false; - _deadband = false; - _axisFrequencyHz = _defaultAxisFrequencyHz; - _buttonFrequencyHz = _defaultButtonFrequencyHz; - _throttleMode = ThrottleModeDownZero; - _calibrated = true; - _circleCorrection = false; + _rgFunctionAxis[rollFunction] = 2; + _rgFunctionAxis[pitchFunction] = 3; + _rgFunctionAxis[yawFunction] = 0; + _rgFunctionAxis[throttleFunction] = 1; + + _rgFunctionAxis[gimbalPitchFunction] = 4; + _rgFunctionAxis[gimbalYawFunction] = 5; + + _exponential = 0; + _accumulator = false; + _deadband = false; + _axisFrequencyHz = _defaultAxisFrequencyHz; + _buttonFrequencyHz = _defaultButtonFrequencyHz; + _throttleMode = ThrottleModeDownZero; + _calibrated = true; + _circleCorrection = false; _saveSettings(); } -void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) +void Joystick::_updateTXModeSettingsKey(Vehicle *activeVehicle) { - if(activeVehicle) { - if(activeVehicle->fixedWing()) { + if (activeVehicle) { + if (activeVehicle->fixedWing()) { _txModeSettingsKey = _fixedWingTXModeSettingsKey; - } else if(activeVehicle->multiRotor()) { + } else if (activeVehicle->multiRotor()) { _txModeSettingsKey = _multiRotorTXModeSettingsKey; - } else if(activeVehicle->rover()) { + } else if (activeVehicle->rover()) { _txModeSettingsKey = _roverTXModeSettingsKey; - } else if(activeVehicle->vtol()) { + } else if (activeVehicle->vtol()) { _txModeSettingsKey = _vtolTXModeSettingsKey; - } else if(activeVehicle->sub()) { + } else if (activeVehicle->sub()) { _txModeSettingsKey = _submarineTXModeSettingsKey; } else { _txModeSettingsKey = nullptr; - qWarning() << "No valid joystick TXmode settings key for selected vehicle"; - return; + qCWarning(JoystickLog) << "No valid joystick TXmode settings key for selected vehicle"; } } else { _txModeSettingsKey = nullptr; } } -void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) +void Joystick::_activeVehicleChanged(Vehicle *activeVehicle) { _updateTXModeSettingsKey(activeVehicle); - if(activeVehicle) { + + if (activeVehicle) { QSettings settings; settings.beginGroup(_settingsGroup); - int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + + const int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); setTXMode(mode); } } -void Joystick::_flightModesChanged() -{ - _buildActionList(_activeVehicle); -} void Joystick::_vehicleCountChanged(int count) { - if(count == 0) - { + if (count == 0) { // then the last vehicle has been deleted qCDebug(JoystickLog) << "Stopping joystick thread due to last active vehicle deletion"; this->stopPolling(); @@ -185,41 +193,39 @@ void Joystick::_loadSettings() { QSettings settings; settings.beginGroup(_settingsGroup); - Vehicle* activeVehicle = _multiVehicleManager->activeVehicle(); - if(_txModeSettingsKey && activeVehicle) + Vehicle *const activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle(); + if (_txModeSettingsKey && activeVehicle) { _transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); + } settings.beginGroup(_name); bool badSettings = false; bool convertOk; - qCDebug(JoystickLog) << "_loadSettings " << _name; - - _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); - _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); - _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); - _deadband = settings.value(_deadbandSettingsKey, false).toBool(); - _axisFrequencyHz = settings.value(_axisFrequencySettingsKey, _defaultAxisFrequencyHz).toFloat(); - _buttonFrequencyHz = settings.value(_buttonFrequencySettingsKey, _defaultButtonFrequencyHz).toFloat(); - _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); - _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool(); - + _calibrated = settings.value(_calibratedSettingsKey, false).toBool(); + _exponential = settings.value(_exponentialSettingsKey, 0).toFloat(); + _accumulator = settings.value(_accumulatorSettingsKey, false).toBool(); + _deadband = settings.value(_deadbandSettingsKey, false).toBool(); + _axisFrequencyHz = settings.value(_axisFrequencySettingsKey, _defaultAxisFrequencyHz).toFloat(&convertOk); + _buttonFrequencyHz = settings.value(_buttonFrequencySettingsKey, _defaultButtonFrequencyHz).toFloat(&convertOk); + _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); + _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool(); + _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); - _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); badSettings |= !convertOk; - qCDebug(JoystickLog) << "_loadSettings calibrated:txmode:throttlemode:exponential:deadband:badsettings" << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings; + qCDebug(JoystickLog) << Q_FUNC_INFO << "calibrated:txmode:throttlemode:exponential:deadband:badsettings" << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings; - QString minTpl ("Axis%1Min"); - QString maxTpl ("Axis%1Max"); - QString trimTpl ("Axis%1Trim"); - QString revTpl ("Axis%1Rev"); - QString deadbndTpl ("Axis%1Deadbnd"); + const QString minTpl("Axis%1Min"); + const QString maxTpl("Axis%1Max"); + const QString trimTpl("Axis%1Trim"); + const QString revTpl("Axis%1Rev"); + const QString deadbndTpl("Axis%1Deadbnd"); for (int axis = 0; axis < _axisCount; axis++) { - Calibration_t* calibration = &_rgCalibration[axis]; + Calibration_t *const calibration = &_rgCalibration[axis]; calibration->center = settings.value(trimTpl.arg(axis), 0).toInt(&convertOk); badSettings |= !convertOk; @@ -235,40 +241,48 @@ void Joystick::_loadSettings() calibration->reversed = settings.value(revTpl.arg(axis), false).toBool(); - qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings; + qCDebug(JoystickLog) << Q_FUNC_INFO << "axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings; } int workingAxis = 0; for (int function = 0; function < maxFunction; function++) { - int functionAxis; - functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk); - badSettings |= !convertOk || (functionAxis >= _axisCount); - if(functionAxis >= 0) { + int functionAxis = settings.value(_rgFunctionSettingsKey[function], -1).toInt(&convertOk); + badSettings |= (!convertOk || (functionAxis >= _axisCount)); + + if (functionAxis >= 0) { workingAxis++; } - if(functionAxis < _axisCount) { + + if (functionAxis < _axisCount) { _rgFunctionAxis[function] = functionAxis; } - qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings; + + qCDebug(JoystickLog) << Q_FUNC_INFO << "function:axis:badsettings" << function << functionAxis << badSettings; } - badSettings |= workingAxis < 4; + + badSettings |= (workingAxis < 4); // FunctionAxis mappings are always stored in TX mode 2 // Remap to stored TX mode in settings - _remapAxes(2, _transmitterMode, _rgFunctionAxis); + _remapAxes (2, _transmitterMode, _rgFunctionAxis); for (int button = 0; button < _totalButtonCount; button++) { - QString a = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); - if(!a.isEmpty() && a != _buttonActionNone) { - if(_buttonActionArray[button]) { - _buttonActionArray[button]->deleteLater(); - } - AssignedButtonAction* ap = new AssignedButtonAction(this, a); - ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); - _buttonActionArray[button] = ap; - _buttonActionArray[button]->buttonTime.start(); - qCDebug(JoystickLog) << "_loadSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + const QString buttonAction = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); + if (buttonAction.isEmpty() || (buttonAction == _buttonActionNone)) { + continue; + } + + if (_buttonActionArray[button]) { + delete _buttonActionArray[button]; + _buttonActionArray[button] = nullptr; } + + AssignedButtonAction *const ap = new AssignedButtonAction(buttonAction); + ap->repeat = settings.value(QString(_buttonActionRepeatKey).arg(button), false).toBool(); + _buttonActionArray[button] = ap; + _buttonActionArray[button]->buttonTime.start(); + + qCDebug(JoystickLog) << Q_FUNC_INFO << "button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; } if (badSettings) { @@ -282,11 +296,12 @@ void Joystick::_saveButtonSettings() QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); + for (int button = 0; button < _totalButtonCount; button++) { - if(_buttonActionArray[button]) { - settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); - settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); - qCDebug(JoystickLog) << "_saveButtonSettings button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; + if (_buttonActionArray[button]) { + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); + qCDebug(JoystickLog) << Q_FUNC_INFO << "button:action:repeat" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; } } } @@ -296,29 +311,28 @@ void Joystick::_saveSettings() QSettings settings; settings.beginGroup(_settingsGroup); - // Transmitter mode is static - // Save the mode we are using - if(_txModeSettingsKey) - settings.setValue(_txModeSettingsKey, _transmitterMode); + if (_txModeSettingsKey) { + settings.setValue(_txModeSettingsKey, _transmitterMode); + } settings.beginGroup(_name); - settings.setValue(_calibratedSettingsKey, _calibrated); - settings.setValue(_exponentialSettingsKey, _exponential); - settings.setValue(_accumulatorSettingsKey, _accumulator); - settings.setValue(_deadbandSettingsKey, _deadband); - settings.setValue(_axisFrequencySettingsKey, _axisFrequencyHz); - settings.setValue(_buttonFrequencySettingsKey, _buttonFrequencyHz); - settings.setValue(_throttleModeSettingsKey, _throttleMode); - settings.setValue(_negativeThrustSettingsKey, _negativeThrust); + settings.setValue(_calibratedSettingsKey, _calibrated); + settings.setValue(_exponentialSettingsKey, _exponential); + settings.setValue(_accumulatorSettingsKey, _accumulator); + settings.setValue(_deadbandSettingsKey, _deadband); + settings.setValue(_axisFrequencySettingsKey, _axisFrequencyHz); + settings.setValue(_buttonFrequencySettingsKey, _buttonFrequencyHz); + settings.setValue(_throttleModeSettingsKey, _throttleMode); + settings.setValue(_negativeThrustSettingsKey, _negativeThrust); settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); - qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; + qCDebug(JoystickLog) << Q_FUNC_INFO << "calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; - QString minTpl ("Axis%1Min"); - QString maxTpl ("Axis%1Max"); - QString trimTpl ("Axis%1Trim"); - QString revTpl ("Axis%1Rev"); - QString deadbndTpl ("Axis%1Deadbnd"); + const QString minTpl("Axis%1Min"); + const QString maxTpl("Axis%1Max"); + const QString trimTpl("Axis%1Trim"); + const QString revTpl("Axis%1Rev"); + const QString deadbndTpl("Axis%1Deadbnd"); for (int axis = 0; axis < _axisCount; axis++) { Calibration_t* calibration = &_rgCalibration[axis]; @@ -327,50 +341,61 @@ void Joystick::_saveSettings() settings.setValue(maxTpl.arg(axis), calibration->max); settings.setValue(revTpl.arg(axis), calibration->reversed); settings.setValue(deadbndTpl.arg(axis), calibration->deadband); - qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed:deadband" - << _name - << axis - << calibration->min - << calibration->max - << calibration->center - << calibration->reversed - << calibration->deadband; + qCDebug(JoystickLog) << Q_FUNC_INFO + << "name:axis:min:max:trim:reversed:deadband" + << _name + << axis + << calibration->min + << calibration->max + << calibration->center + << calibration->reversed + << calibration->deadband; } - // Always save function Axis mappings in TX Mode 2 // Write mode 2 mappings without changing mapping currently in use int temp[maxFunction]; _remapAxes(_transmitterMode, 2, temp); + for (int function = 0; function < maxFunction; function++) { settings.setValue(_rgFunctionSettingsKey[function], temp[function]); - qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function]; + qCDebug(JoystickLog) << Q_FUNC_INFO << "name:function:axis" << _name << function << _rgFunctionSettingsKey[function]; } + _saveButtonSettings(); } -// Relative mappings of axis functions between different TX modes -int Joystick::_mapFunctionMode(int mode, int function) { - static const int mapping[][6] = { +int Joystick::_mapFunctionMode(int mode, int function) +{ + static constexpr const int mapping[][6] = { { yawFunction, pitchFunction, rollFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, { yawFunction, throttleFunction, rollFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }, { rollFunction, pitchFunction, yawFunction, throttleFunction, gimbalPitchFunction, gimbalYawFunction }, - { rollFunction, throttleFunction, yawFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction }}; - return mapping[mode-1][function]; + { rollFunction, throttleFunction, yawFunction, pitchFunction, gimbalPitchFunction, gimbalYawFunction } + }; + + if ((mode > 0) && (mode <= 4)) { + return mapping[mode - 1][function]; + } + + return -1; } -// Remap current axis functions from current TX mode to new TX mode -void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) { +void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) +{ int temp[maxFunction]; - for(int function = 0; function < maxFunction; function++) { + + for (int function = 0; function < maxFunction; function++) { temp[_mapFunctionMode(newMode, function)] = _rgFunctionAxis[_mapFunctionMode(currentMode, function)]; } - for(int function = 0; function < maxFunction; function++) { + + for (int function = 0; function < maxFunction; function++) { newMapping[function] = temp[function]; } } -void Joystick::setTXMode(int mode) { - if(mode > 0 && mode <= 4) { +void Joystick::setTXMode(int mode) +{ + if ((mode > 0) && (mode <= 4)) { _remapAxes(_transmitterMode, mode, _rgFunctionAxis); _transmitterMode = mode; _saveSettings(); @@ -379,8 +404,7 @@ void Joystick::setTXMode(int mode) { } } -/// Adjust the raw axis value to the -1:1 range given calibration information -float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands) +float Joystick::_adjustRange(int value, const Calibration_t &calibration, bool withDeadbands) { float valueNormalized; float axisLength; @@ -397,129 +421,127 @@ float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDead } float axisPercent; - if (withDeadbands) { - if (valueNormalized>calibration.deadband) { + if (valueNormalized > calibration.deadband) { axisPercent = (valueNormalized - calibration.deadband) / (axisLength - calibration.deadband); } else if (valueNormalized<-calibration.deadband) { axisPercent = (valueNormalized + calibration.deadband) / (axisLength - calibration.deadband); } else { axisPercent = 0.f; } - } - else { + } else { axisPercent = valueNormalized / axisLength; } float correctedValue = axisBasis * axisPercent; - if (calibration.reversed) { correctedValue *= -1.0f; } -#if 0 - qCDebug(JoystickLog) << "_adjustRange corrected:value:min:max:center:reversed:deadband:basis:normalized:length" - << correctedValue - << value - << calibration.min - << calibration.max - << calibration.center - << calibration.reversed - << calibration.deadband - << axisBasis - << valueNormalized - << axisLength; -#endif - return std::max(-1.0f, std::min(correctedValue, 1.0f)); } void Joystick::run() { - //-- Joystick thread _open(); - //-- Reset timers + _axisTime.start(); + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { - if(_buttonActionArray[buttonIndex]) { + if (_buttonActionArray[buttonIndex]) { _buttonActionArray[buttonIndex]->buttonTime.start(); } } + while (!_exitThread) { _update(); _handleButtons(); if (axisCount() != 0) { _handleAxis(); } - QGC::SLEEP::msleep(qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2); + + const int sleep = qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2; + QThread::msleep(sleep); } + _close(); } void Joystick::_handleButtons() { - int lastBbuttonValues[256]; + int lastBbuttonValues[256]{}; + //-- Update button states for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { - bool newButtonValue = _getButton(buttonIndex); - if(buttonIndex < 256) + const bool newButtonValue = _getButton(buttonIndex); + if (buttonIndex < 256) { lastBbuttonValues[buttonIndex] = _rgButtonValues[buttonIndex]; - if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) { + } + + if (newButtonValue && (_rgButtonValues[buttonIndex] == BUTTON_UP)) { _rgButtonValues[buttonIndex] = BUTTON_DOWN; emit rawButtonPressedChanged(buttonIndex, newButtonValue); - } else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) { + } else if (!newButtonValue && (_rgButtonValues[buttonIndex] != BUTTON_UP)) { _rgButtonValues[buttonIndex] = BUTTON_UP; emit rawButtonPressedChanged(buttonIndex, newButtonValue); } } + //-- Update hat - append hat buttons to the end of the normal button list int numHatButtons = 4; for (int hatIndex = 0; hatIndex < _hatCount; hatIndex++) { - for (int hatButtonIndex = 0; hatButtonIndexaction; - if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + if ((_rgButtonValues[buttonIndex] == BUTTON_DOWN) || (_rgButtonValues[buttonIndex] == BUTTON_REPEAT)) { + if (_buttonActionArray[buttonIndex]) { + const QString buttonAction = _buttonActionArray[buttonIndex]->action; + if (buttonAction.isEmpty() || (buttonAction == _buttonActionNone)) { continue; - if(!_buttonActionArray[buttonIndex]->repeat) { - //-- This button just went down - if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { + } + + if (!_buttonActionArray[buttonIndex]->repeat) { + if (_rgButtonValues[buttonIndex] == BUTTON_DOWN) { // Check for a multi-button action QList rgButtons = { buttonIndex }; bool executeButtonAction = true; for (int multiIndex = 0; multiIndex < _totalButtonCount; multiIndex++) { - if (multiIndex != buttonIndex) { - if (_buttonActionArray[multiIndex] && _buttonActionArray[multiIndex]->action == buttonAction) { - // We found a multi-button action - if (_rgButtonValues[multiIndex] == BUTTON_DOWN || _rgButtonValues[multiIndex] == BUTTON_REPEAT) { - // So far so good - rgButtons.append(multiIndex); - continue; - } else { - // We are missing a press we need - executeButtonAction = false; - break; - } + if (multiIndex == buttonIndex) { + continue; + } + + if (_buttonActionArray[multiIndex] && (_buttonActionArray[multiIndex]->action == buttonAction)) { + // We found a multi-button action + if ((_rgButtonValues[multiIndex] == BUTTON_DOWN) || (_rgButtonValues[multiIndex] == BUTTON_REPEAT)) { + // So far so good + rgButtons.append(multiIndex); + continue; + } else { + // We are missing a press we need + executeButtonAction = false; + break; } } } + if (executeButtonAction) { qCDebug(JoystickLog) << "Action triggered" << rgButtons << buttonAction; _executeButtonAction(buttonAction, true); @@ -527,8 +549,8 @@ void Joystick::_handleButtons() } } else { //-- Process repeat buttons - int buttonDelay = static_cast(1000.0f / _buttonFrequencyHz); - if(_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { + const int buttonDelay = static_cast(1000.0f / _buttonFrequencyHz); + if (_buttonActionArray[buttonIndex]->buttonTime.elapsed() > buttonDelay) { _buttonActionArray[buttonIndex]->buttonTime.start(); qCDebug(JoystickLog) << "Repeat button triggered" << buttonIndex << buttonAction; _executeButtonAction(buttonAction, true); @@ -537,14 +559,15 @@ void Joystick::_handleButtons() } //-- Flag it as processed _rgButtonValues[buttonIndex] = BUTTON_REPEAT; - } else if(_rgButtonValues[buttonIndex] == BUTTON_UP) { + } else if (_rgButtonValues[buttonIndex] == BUTTON_UP) { //-- Button up transition - if(buttonIndex < 256) { - if(lastBbuttonValues[buttonIndex] == BUTTON_DOWN || lastBbuttonValues[buttonIndex] == BUTTON_REPEAT) { - if(_buttonActionArray[buttonIndex]) { - QString buttonAction = _buttonActionArray[buttonIndex]->action; - if(buttonAction.isEmpty() || buttonAction == _buttonActionNone) + if (buttonIndex < 256) { + if ((lastBbuttonValues[buttonIndex] == BUTTON_DOWN) || (lastBbuttonValues[buttonIndex] == BUTTON_REPEAT)) { + if (_buttonActionArray[buttonIndex]) { + const QString buttonAction = _buttonActionArray[buttonIndex]->action; + if (buttonAction.isEmpty() || buttonAction == _buttonActionNone) { continue; + } qCDebug(JoystickLog) << "Button up" << buttonIndex << buttonAction; _executeButtonAction(buttonAction, false); } @@ -556,189 +579,200 @@ void Joystick::_handleButtons() void Joystick::_handleAxis() { - //-- Get frequency - int axisDelay = static_cast(1000.0f / _axisFrequencyHz); - //-- Check elapsed time since last run - if(_axisTime.elapsed() > axisDelay) { - _axisTime.start(); - //-- Update axis - for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) { - int newAxisValue = _getAxis(axisIndex); - // Calibration code requires signal to be emitted even if value hasn't changed - _rgAxisValues[axisIndex] = newAxisValue; - emit rawAxisValueChanged(axisIndex, newAxisValue); - } - if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { - int axis = _rgFunctionAxis[rollFunction]; - float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + const int axisDelay = static_cast(1000.0f / _axisFrequencyHz); + if (_axisTime.elapsed() <= axisDelay) { + return; + } - axis = _rgFunctionAxis[pitchFunction]; - float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + _axisTime.start(); - axis = _rgFunctionAxis[yawFunction]; - float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) { + int newAxisValue = _getAxis(axisIndex); + // Calibration code requires signal to be emitted even if value hasn't changed + _rgAxisValues[axisIndex] = newAxisValue; + emit rawAxisValueChanged(axisIndex, newAxisValue); + } - axis = _rgFunctionAxis[throttleFunction]; - float throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband); + if (!_activeVehicle->joystickEnabled() || _calibrationMode || !_calibrated) { + return; + } - // These are only used for printing JoystickValuesLog - float gimbalPitch = 0.0f; - float gimbalYaw = 0.0f; + int axis = _rgFunctionAxis[rollFunction]; + float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); - if(_axisCount > 4) { - axis = _rgFunctionAxis[gimbalPitchFunction]; - gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); - } + axis = _rgFunctionAxis[pitchFunction]; + float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); - if(_axisCount > 5) { - axis = _rgFunctionAxis[gimbalYawFunction]; - gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); - } + axis = _rgFunctionAxis[yawFunction]; + float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); - if (_accumulator) { - static float throttle_accu = 0.f; - throttle_accu += throttle * (40 / 1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time) - throttle_accu = std::max(static_cast(-1.f), std::min(throttle_accu, static_cast(1.f))); - throttle = throttle_accu; - } + axis = _rgFunctionAxis[throttleFunction]; + float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband); - if (_circleCorrection) { - float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); - float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); - float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); - float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); - - // Map from unit circle to linear range and limit - roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); - pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); - yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); - throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); - } + float gimbalPitch = 0.0f; + if (_axisCount > 4) { + axis = _rgFunctionAxis[gimbalPitchFunction]; + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } - if ( _exponential < -0.01f) { - // Exponential (0% to -50% range like most RC radios) - // _exponential is set by a slider in joystickConfigAdvanced.qml - // Calculate new RPY with exponential applied - roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; - pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; - yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; - } + float gimbalYaw = 0.0f; + if (_axisCount > 5) { + axis = _rgFunctionAxis[gimbalYawFunction]; + gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } - // Adjust throttle to 0:1 range - if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) { - if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) { - throttle = std::max(0.0f, throttle); - } - } else { - throttle = (throttle + 1.0f) / 2.0f; - } - qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw; - // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits) - // Set up button bitmap - quint64 buttonPressedBits = 0; // Buttons pressed for manualControl signal - for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { - quint64 buttonBit = static_cast(1LL << buttonIndex); - if (_rgButtonValues[buttonIndex] != BUTTON_UP) { - // Mark the button as pressed as long as its pressed - buttonPressedBits |= buttonBit; - } - } - emit axisValues(roll, pitch, yaw, throttle); + if (_accumulator) { + static float throttle_accu = 0.f; + throttle_accu += (throttle * (40 / 1000.f)); // for throttle to change from min to max it will take 1000ms (40ms is a loop time) + throttle_accu = std::max(static_cast(-1.f), std::min(throttle_accu, static_cast(1.f))); + throttle = throttle_accu; + } + + if (_circleCorrection) { + const float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + const float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + const float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + const float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + + // Map from unit circle to linear range and limit + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); + yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); + throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); + } + + if ( _exponential < -0.01f) { + // Exponential (0% to -50% range like most RC radios) + // _exponential is set by a slider in joystickConfigAdvanced.qml + // Calculate new RPY with exponential applied + roll = -_exponential*powf(roll, 3) + ((1+_exponential) * roll); + pitch = -_exponential*powf(pitch,3) + ((1+_exponential) * pitch); + yaw = -_exponential*powf(yaw, 3) + ((1+_exponential) * yaw); + } + + // Adjust throttle to 0:1 range + if ((_throttleMode == ThrottleModeCenterZero) && (_activeVehicle->supportsThrottleModeCenterZero())) { + if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) { + throttle = std::max(0.0f, throttle); + } + } else { + throttle = (throttle + 1.0f) / 2.0f; + } - uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); - _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons); + qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw; + + // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits) + // Set up button bitmap + quint64 buttonPressedBits = 0; ///< Buttons pressed for manualControl signal + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + const quint64 buttonBit = static_cast(1LL << buttonIndex); + if (_rgButtonValues[buttonIndex] != BUTTON_UP) { + buttonPressedBits |= buttonBit; } } + + emit axisValues(roll, pitch, yaw, throttle); + + const uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); + _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, shortButtons); } void Joystick::startPolling(Vehicle* vehicle) { if (vehicle) { - // If a vehicle is connected, disconnect it if (_activeVehicle) { - disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); - disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); - disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); - disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); - disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); - disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); - disconnect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); - disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); - disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); - disconnect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); - disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + (void) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + (void) disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + (void) disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + (void) disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + (void) disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + (void) disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + (void) disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); + (void) disconnect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + (void) disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + (void) disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); + (void) disconnect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); + (void) disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); } - // Always set up the new vehicle + _activeVehicle = vehicle; + // If joystick is not calibrated, disable it - if ( axisCount() != 0 && !_calibrated ) { + if ((axisCount() != 0) && !_calibrated) { vehicle->setJoystickEnabled(false); } + // Update qml in case of joystick transition emit calibratedChanged(_calibrated); - // Build action list + _buildActionList(vehicle); - // Only connect the new vehicle if it wants joystick data + if (vehicle->joystickEnabled()) { _pollingStartedForCalibration = false; - connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); - connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); - connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - connect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); - connect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); - connect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); - connect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); - connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); - connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); - connect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); - connect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); - connect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + (void) connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + (void) connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + (void) connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + (void) connect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + (void) connect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + (void) connect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + (void) connect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); + (void) connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); + (void) connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + (void) connect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); + (void) connect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); + (void) connect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); } } + if (!isRunning()) { _exitThread = false; start(); } } -void Joystick::stopPolling(void) +void Joystick::stopPolling() { - if (isRunning()) { - if (_activeVehicle && _activeVehicle->joystickEnabled()) { - disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); - disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); - disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); - disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); - disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); - disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); - disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); - disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); - disconnect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); - disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); - } - _exitThread = true; + if (!isRunning()) { + _activeVehicle = nullptr; + return; + } + + if (_activeVehicle && _activeVehicle->joystickEnabled()) { + (void) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); + (void) disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + (void) disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + (void) disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + (void) disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + (void) disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + (void) disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); + (void) disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); + (void) disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); + (void) disconnect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); + (void) disconnect(_activeVehicle, &Vehicle::flightModesChanged, this, &Joystick::_flightModesChanged); + _activeVehicle = nullptr; } - _activeVehicle = nullptr; + + _exitThread = true; } -void Joystick::setCalibration(int axis, Calibration_t& calibration) +void Joystick::setCalibration(int axis, const Calibration_t &calibration) { if (!_validAxis(axis)) { return; } + _calibrated = true; _rgCalibration[axis] = calibration; _saveSettings(); emit calibratedChanged(_calibrated); } -Joystick::Calibration_t Joystick::getCalibration(int axis) +Joystick::Calibration_t Joystick::getCalibration(int axis) const { if (!_validAxis(axis)) { - return Calibration_t(); + return Calibration_t{}; } + return _rgCalibration[axis]; } @@ -747,17 +781,19 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis) if (!_validAxis(axis)) { return; } + _calibrated = true; _rgFunctionAxis[function] = axis; _saveSettings(); emit calibratedChanged(_calibrated); } -int Joystick::getFunctionAxis(AxisFunction_t function) +int Joystick::getFunctionAxis(AxisFunction_t function) const { - if (static_cast(function) < 0 || function >= maxFunction) { + if ((static_cast(function) < 0) || (function >= maxFunction)) { qCWarning(JoystickLog) << "Invalid function" << function; } + return _rgFunctionAxis[function]; } @@ -766,9 +802,10 @@ void Joystick::setButtonRepeat(int button, bool repeat) if (!_validButton(button) || !_buttonActionArray[button]) { return; } + _buttonActionArray[button]->repeat = repeat; _buttonActionArray[button]->buttonTime.start(); - //-- Save to settings + QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); @@ -780,6 +817,7 @@ bool Joystick::getButtonRepeat(int button) if (!_validButton(button) || !_buttonActionArray[button]) { return false; } + return _buttonActionArray[button]->repeat; } @@ -788,157 +826,148 @@ void Joystick::setButtonAction(int button, const QString& action) if (!_validButton(button)) { return; } - qCWarning(JoystickLog) << "setButtonAction:" << button << action; + + qCWarning(JoystickLog) << Q_FUNC_INFO << button << action; + QSettings settings; settings.beginGroup(_settingsGroup); settings.beginGroup(_name); - if(action.isEmpty() || action == _buttonActionNone) { - if(_buttonActionArray[button]) { - _buttonActionArray[button]->deleteLater(); + if (action.isEmpty() || action == _buttonActionNone) { + if (_buttonActionArray[button]) { + delete _buttonActionArray[button]; _buttonActionArray[button] = nullptr; - //-- Clear from settings settings.remove(QString(_buttonActionNameKey).arg(button)); settings.remove(QString(_buttonActionRepeatKey).arg(button)); } } else { - if(!_buttonActionArray[button]) { - _buttonActionArray[button] = new AssignedButtonAction(this, action); + if (!_buttonActionArray[button]) { + _buttonActionArray[button] = new AssignedButtonAction(action); } else { _buttonActionArray[button]->action = action; //-- Make sure repeat is off if this action doesn't support repeats - int idx = _findAssignableButtonAction(action); - if(idx >= 0) { - AssignableButtonAction* p = qobject_cast(_assignableButtonActions[idx]); - if(!p->canRepeat()) { + const int idx = _findAssignableButtonAction(action); + if (idx >= 0) { + const AssignableButtonAction *const buttonAction = qobject_cast(_assignableButtonActions->get(idx)); + if (!buttonAction->canRepeat()) { _buttonActionArray[button]->repeat = false; } } } - //-- Save to settings - settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); + + settings.setValue(QString(_buttonActionNameKey).arg(button), _buttonActionArray[button]->action); settings.setValue(QString(_buttonActionRepeatKey).arg(button), _buttonActionArray[button]->repeat); } + emit buttonActionsChanged(); } -QString Joystick::getButtonAction(int button) +QString Joystick::getButtonAction(int button) const { if (_validButton(button)) { - if(_buttonActionArray[button]) { + if (_buttonActionArray[button]) { return _buttonActionArray[button]->action; } } + return QString(_buttonActionNone); } -QStringList Joystick::buttonActions() +QStringList Joystick::buttonActions() const { QStringList list; for (int button = 0; button < _totalButtonCount; button++) { list << getButtonAction(button); } - return list; -} -int Joystick::throttleMode() -{ - return _throttleMode; + return list; } void Joystick::setThrottleMode(int mode) { - if (mode < 0 || mode >= ThrottleModeMax) { + if (mode < 0 || (mode >= ThrottleModeMax)) { qCWarning(JoystickLog) << "Invalid throttle mode" << mode; return; } - _throttleMode = static_cast(mode); - if (_throttleMode == ThrottleModeDownZero) { - setAccumulator(false); - } - _saveSettings(); - emit throttleModeChanged(_throttleMode); -} -bool Joystick::negativeThrust() const -{ - return _negativeThrust; -} + if (mode != _throttleMode) { + _throttleMode = static_cast(mode); + if (_throttleMode == ThrottleModeDownZero) { + setAccumulator(false); + } -void Joystick::setNegativeThrust(bool allowNegative) -{ - if (_negativeThrust == allowNegative) { - return; + _saveSettings(); + emit throttleModeChanged(_throttleMode); } - _negativeThrust = allowNegative; - _saveSettings(); - emit negativeThrustChanged(_negativeThrust); } -float Joystick::exponential() const +void Joystick::setNegativeThrust(bool allowNegative) { - return _exponential; + if (_negativeThrust != allowNegative) { + _negativeThrust = allowNegative; + _saveSettings(); + emit negativeThrustChanged(_negativeThrust); + } } void Joystick::setExponential(float expo) { - _exponential = expo; - _saveSettings(); - emit exponentialChanged(_exponential); -} - -bool Joystick::accumulator() const -{ - return _accumulator; + if (expo != _exponential) { + _exponential = expo; + _saveSettings(); + emit exponentialChanged(_exponential); + } } void Joystick::setAccumulator(bool accu) { - _accumulator = accu; - _saveSettings(); - emit accumulatorChanged(_accumulator); -} - -bool Joystick::deadband() const -{ - return _deadband; + if (accu != _accumulator) { + _accumulator = accu; + _saveSettings(); + emit accumulatorChanged(_accumulator); + } } void Joystick::setDeadband(bool deadband) { - _deadband = deadband; - _saveSettings(); -} - -bool Joystick::circleCorrection() const -{ - return _circleCorrection; + if (deadband != _deadband) { + _deadband = deadband; + _saveSettings(); + } } void Joystick::setCircleCorrection(bool circleCorrection) { - _circleCorrection = circleCorrection; - _saveSettings(); - emit circleCorrectionChanged(_circleCorrection); + if (circleCorrection != _circleCorrection) { + _circleCorrection = circleCorrection; + _saveSettings(); + emit circleCorrectionChanged(_circleCorrection); + } } void Joystick::setAxisFrequency(float val) { - //-- Arbitrary limits - val = qMax(_minAxisFrequencyHz, val); - val = qMin(_maxAxisFrequencyHz, val); - _axisFrequencyHz = val; - _saveSettings(); - emit axisFrequencyHzChanged(); + float result = val; + result = qMax(_minAxisFrequencyHz, result); + result = qMin(_maxAxisFrequencyHz, result); + + if (result != _axisFrequencyHz) { + _axisFrequencyHz = result; + _saveSettings(); + emit axisFrequencyHzChanged(); + } } void Joystick::setButtonFrequency(float val) { - //-- Arbitrary limits - val = qMax(_minButtonFrequencyHz, val); - val = qMin(_maxButtonFrequencyHz, val); - _buttonFrequencyHz = val; - _saveSettings(); - emit buttonFrequencyHzChanged(); + float result = val; + result = qMax(_minButtonFrequencyHz, result); + result = qMin(_maxButtonFrequencyHz, result); + + if (result != _buttonFrequencyHz) { + _buttonFrequencyHz = result; + _saveSettings(); + emit buttonFrequencyHzChanged(); + } } void Joystick::setCalibrationMode(bool calibrating) @@ -946,83 +975,128 @@ void Joystick::setCalibrationMode(bool calibrating) _calibrationMode = calibrating; if (calibrating && !isRunning()) { _pollingStartedForCalibration = true; - startPolling(_multiVehicleManager->activeVehicle()); - } - else if (_pollingStartedForCalibration) { + startPolling(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()); + } else if (_pollingStartedForCalibration) { stopPolling(); } } - -void Joystick::_executeButtonAction(const QString& action, bool buttonDown) +void Joystick::_executeButtonAction(const QString &action, bool buttonDown) { - if (!_activeVehicle || !_activeVehicle->joystickEnabled() || action == _buttonActionNone) { + if (!_activeVehicle || !_activeVehicle->joystickEnabled() || (action == _buttonActionNone)) { return; } + if (action == _buttonActionArm) { - if (buttonDown) emit setArmed(true); + if (buttonDown) { + emit setArmed(true); + } } else if (action == _buttonActionDisarm) { - if (buttonDown) emit setArmed(false); + if (buttonDown) { + emit setArmed(false); + } } else if (action == _buttonActionToggleArm) { - if (buttonDown) emit setArmed(!_activeVehicle->armed()); + if (buttonDown) { + emit setArmed(!_activeVehicle->armed()); + } } else if (action == _buttonActionVTOLFixedWing) { - if (buttonDown) emit setVtolInFwdFlight(true); + if (buttonDown) { + emit setVtolInFwdFlight(true); + } } else if (action == _buttonActionVTOLMultiRotor) { - if (buttonDown) emit setVtolInFwdFlight(false); + if (buttonDown) { + emit setVtolInFwdFlight(false); + } } else if (_activeVehicle->flightModes().contains(action)) { - if (buttonDown) emit setFlightMode(action); - } else if(action == _buttonActionContinuousZoomIn || action == _buttonActionContinuousZoomOut) { if (buttonDown) { - emit startContinuousZoom(action == _buttonActionContinuousZoomIn ? 1 : -1); + emit setFlightMode(action); + } + } else if ((action == _buttonActionContinuousZoomIn) || (action == _buttonActionContinuousZoomOut)) { + if (buttonDown) { + emit startContinuousZoom((action == _buttonActionContinuousZoomIn) ? 1 : -1); } else { emit stopContinuousZoom(); } - } else if(action == _buttonActionStepZoomIn || action == _buttonActionStepZoomOut) { - if (buttonDown) emit stepZoom(action == _buttonActionStepZoomIn ? 1 : -1); - } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { - if (buttonDown) emit stepStream(action == _buttonActionNextStream ? 1 : -1); - } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) { - if (buttonDown) emit stepCamera(action == _buttonActionNextCamera ? 1 : -1); - } else if(action == _buttonActionTriggerCamera) { - if (buttonDown) emit triggerCamera(); - } else if(action == _buttonActionStartVideoRecord) { - if (buttonDown) emit startVideoRecord(); - } else if(action == _buttonActionStopVideoRecord) { - if (buttonDown) emit stopVideoRecord(); - } else if(action == _buttonActionToggleVideoRecord) { - if (buttonDown) emit toggleVideoRecord(); - } else if(action == _buttonActionGimbalUp) { - if (buttonDown) emit gimbalPitchStep(1); - } else if(action == _buttonActionGimbalDown) { - if (buttonDown) emit gimbalPitchStep(-1); - } else if(action == _buttonActionGimbalLeft) { - if (buttonDown) emit gimbalYawStep(-1); - } else if(action == _buttonActionGimbalRight) { - if (buttonDown) emit gimbalYawStep(1); - } else if(action == _buttonActionGimbalCenter) { - if (buttonDown) emit centerGimbal(); - } else if(action == _buttonActionGimbalYawLock) { - if (buttonDown) emit gimbalYawLock(true); - } else if(action == _buttonActionGimbalYawFollow) { - if (buttonDown) emit gimbalYawLock(false); - } else if(action == _buttonActionEmergencyStop) { - if (buttonDown) emit emergencyStop(); - } else if(action == _buttonActionGripperGrab) { - if(buttonDown) { + } else if ((action == _buttonActionStepZoomIn) || (action == _buttonActionStepZoomOut)) { + if (buttonDown) { + emit stepZoom((action == _buttonActionStepZoomIn) ? 1 : -1); + } + } else if ((action == _buttonActionNextStream) || (action == _buttonActionPreviousStream)) { + if (buttonDown) { + emit stepStream((action == _buttonActionNextStream) ? 1 : -1); + } + } else if ((action == _buttonActionNextCamera) || (action == _buttonActionPreviousCamera)) { + if (buttonDown) { + emit stepCamera((action == _buttonActionNextCamera) ? 1 : -1); + } + } else if (action == _buttonActionTriggerCamera) { + if (buttonDown) { + emit triggerCamera(); + } + } else if (action == _buttonActionStartVideoRecord) { + if (buttonDown) { + emit startVideoRecord(); + } + } else if (action == _buttonActionStopVideoRecord) { + if (buttonDown) { + emit stopVideoRecord(); + } + } else if (action == _buttonActionToggleVideoRecord) { + if (buttonDown) { + emit toggleVideoRecord(); + } + } else if (action == _buttonActionGimbalUp) { + if (buttonDown) { + emit gimbalPitchStep(1); + } + } else if (action == _buttonActionGimbalDown) { + if (buttonDown) { + emit gimbalPitchStep(-1); + } + } else if (action == _buttonActionGimbalLeft) { + if (buttonDown) { + emit gimbalYawStep(-1); + } + } else if (action == _buttonActionGimbalRight) { + if (buttonDown) { + emit gimbalYawStep(1); + } + } else if (action == _buttonActionGimbalCenter) { + if (buttonDown) { + emit centerGimbal(); + } + } else if (action == _buttonActionGimbalYawLock) { + if (buttonDown) { + emit gimbalYawLock(true); + } + } else if (action == _buttonActionGimbalYawFollow) { + if (buttonDown) { + emit gimbalYawLock(false); + } + } else if (action == _buttonActionEmergencyStop) { + if (buttonDown) { + emit emergencyStop(); + } + } else if (action == _buttonActionGripperGrab) { + if (buttonDown) { emit gripperAction(GRIPPER_ACTION_GRAB); } - } else if(action == _buttonActionGripperRelease) { - if(buttonDown) { + } else if (action == _buttonActionGripperRelease) { + if (buttonDown) { emit gripperAction(GRIPPER_ACTION_RELEASE); } - } else if(action == _buttonActionLandingGearDeploy) { - if (buttonDown) emit landingGearDeploy(); - } else if(action == _buttonActionLandingGearRetract) { - if (buttonDown) emit landingGearRetract(); + } else if (action == _buttonActionLandingGearDeploy) { + if (buttonDown) { + emit landingGearDeploy(); + } + } else if (action == _buttonActionLandingGearRetract) { + if (buttonDown) { + emit landingGearRetract(); + } } else { if (buttonDown && _activeVehicle) { - for (int i=0; i<_customActionManager.actions()->count(); i++) { - auto customAction = _customActionManager.actions()->value(i); + for (int i = 0; i<_customActionManager->actions()->count(); i++) { + CustomAction *const customAction = _customActionManager->actions()->value(i); if (action == customAction->label()) { customAction->sendTo(_activeVehicle); return; @@ -1034,82 +1108,89 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown) bool Joystick::_validAxis(int axis) const { - if(axis >= 0 && axis < _axisCount) { + if ((axis >= 0) && (axis < _axisCount)) { return true; } + qCWarning(JoystickLog) << "Invalid axis index" << axis; return false; } bool Joystick::_validButton(int button) const { - if(button >= 0 && button < _totalButtonCount) + if ((button >= 0) && (button < _totalButtonCount)) { return true; + } + qCWarning(JoystickLog) << "Invalid button index" << button; return false; } -int Joystick::_findAssignableButtonAction(const QString& action) +int Joystick::_findAssignableButtonAction(const QString &action) { - for(int i = 0; i < _assignableButtonActions.count(); i++) { - AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); - if(p->action() == action) + for (int i = 0; i < _assignableButtonActions->count(); i++) { + const AssignableButtonAction *const buttonAction = qobject_cast(_assignableButtonActions->get(i)); + if (buttonAction->action() == action) { return i; + } } + return -1; } -void Joystick::_buildActionList(Vehicle* activeVehicle) +void Joystick::_buildActionList(Vehicle *activeVehicle) { - if(_assignableButtonActions.count()) - _assignableButtonActions.clearAndDeleteContents(); + if (_assignableButtonActions->count()) { + _assignableButtonActions->clearAndDeleteContents(); + } _availableActionTitles.clear(); - //-- Available Actions - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNone)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionArm)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionDisarm)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleArm)); + + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionNone)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionArm)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionDisarm)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionToggleArm)); if (activeVehicle) { - QStringList list = activeVehicle->flightModes(); - foreach(auto mode, list) { - _assignableButtonActions.append(new AssignableButtonAction(this, mode)); + const QStringList list = activeVehicle->flightModes(); + for (const QString &mode : list) { + (void) _assignableButtonActions->append(new AssignableButtonAction(mode)); } } - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLFixedWing)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionVTOLMultiRotor)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomIn, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionContinuousZoomOut, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomIn, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStepZoomOut, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextStream)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousStream)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionNextCamera)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionPreviousCamera)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionTriggerCamera)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStartVideoRecord)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionStopVideoRecord)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionToggleVideoRecord)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalDown, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalUp, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalLeft, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight, true)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalYawLock)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalYawFollow)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionLandingGearDeploy)); - _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionLandingGearRetract)); - - for (int i=0; i<_customActionManager.actions()->count(); i++) { - auto customAction = _customActionManager.actions()->value(i); - _assignableButtonActions.append(new AssignableButtonAction(this, customAction->label())); - } - - for(int i = 0; i < _assignableButtonActions.count(); i++) { - AssignableButtonAction* p = qobject_cast(_assignableButtonActions[i]); - _availableActionTitles << p->action(); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionVTOLFixedWing)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionVTOLMultiRotor)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionContinuousZoomIn, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionContinuousZoomOut, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionStepZoomIn, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionStepZoomOut, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionNextStream)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionPreviousStream)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionNextCamera)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionPreviousCamera)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionTriggerCamera)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionStartVideoRecord)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionStopVideoRecord)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionToggleVideoRecord)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalDown, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalUp, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalLeft, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalRight, true)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalCenter)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalYawLock)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGimbalYawFollow)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionEmergencyStop)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGripperGrab)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionGripperRelease)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionLandingGearDeploy)); + (void) _assignableButtonActions->append(new AssignableButtonAction(_buttonActionLandingGearRetract)); + + for (int i = 0; i < _customActionManager->actions()->count(); i++) { + const CustomAction *const customAction = _customActionManager->actions()->value(i); + (void) _assignableButtonActions->append(new AssignableButtonAction(customAction->label())); + } + + for (int i = 0; i < _assignableButtonActions->count(); i++) { + const AssignableButtonAction *const buttonAction = qobject_cast(_assignableButtonActions->get(i)); + _availableActionTitles << buttonAction->action(); } + emit assignableActionsChanged(); } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 4d0b8bfffac..9ae061b0d82 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -7,73 +7,97 @@ * ****************************************************************************/ -/// @file -/// @brief Joystick Controller - #pragma once #include "QGCMAVLink.h" -#include "CustomActionManager.h" +#include #include #include -#include -// JoystickLog Category declaration moved to QGCLoggingCategory.cc to allow access in Vehicle +Q_DECLARE_LOGGING_CATEGORY(JoystickLog) Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog) -Q_DECLARE_METATYPE(GRIPPER_ACTIONS) -class MultiVehicleManager; +class CustomActionManager; +class QmlObjectListModel; class Vehicle; -/// Action assigned to button -class AssignedButtonAction : public QObject { - Q_OBJECT +/*===========================================================================*/ + +class AssignedButtonAction +{ public: - AssignedButtonAction(QObject* parent, const QString name); + AssignedButtonAction(const QString &name); + QString action; QElapsedTimer buttonTime; - bool repeat = false; + bool repeat = false; }; -/// Assignable Button Action -class AssignableButtonAction : public QObject { +// TODO: Q_GADGET +class AssignableButtonAction : public QObject +{ Q_OBJECT -public: - AssignableButtonAction(QObject* parent, QString action_, bool canRepeat_ = false); Q_PROPERTY(QString action READ action CONSTANT) Q_PROPERTY(bool canRepeat READ canRepeat CONSTANT) - QString action () { return _action; } - bool canRepeat () const{ return _repeat; } + +public: + AssignableButtonAction(const QString &action_, bool canRepeat_ = false, QObject *parent = nullptr); + + QString action() const { return _action; } + bool canRepeat() const { return _repeat; } + private: - QString _action; - bool _repeat = false; + const QString _action; + const bool _repeat = false; }; -/// Joystick Controller +/*===========================================================================*/ + class Joystick : public QThread { Q_OBJECT -public: - Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager); + Q_MOC_INCLUDE("QmlObjectListModel.h") + Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) + Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) + Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) + Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) + Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) + Q_PROPERTY(float axisFrequencyHz READ axisFrequencyHz WRITE setAxisFrequency NOTIFY axisFrequencyHzChanged) + Q_PROPERTY(float buttonFrequencyHz READ buttonFrequencyHz WRITE setButtonFrequency NOTIFY buttonFrequencyHzChanged) + Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged) + Q_PROPERTY(float maxAxisFrequencyHz MEMBER _maxAxisFrequencyHz CONSTANT) + Q_PROPERTY(float maxButtonFrequencyHz MEMBER _maxButtonFrequencyHz CONSTANT) + Q_PROPERTY(float minAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT) + Q_PROPERTY(float minButtonFrequencyHz MEMBER _minButtonFrequencyHz CONSTANT) + Q_PROPERTY(int axisCount READ axisCount CONSTANT) + Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) + Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) + Q_PROPERTY(const QmlObjectListModel *assignableActions READ assignableActions NOTIFY assignableActionsChanged) + Q_PROPERTY(QString disabledActionName READ disabledActionName CONSTANT) + Q_PROPERTY(QString name READ name CONSTANT) + Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged) + Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged) + + enum ButtonEvent_t { + BUTTON_UP, + BUTTON_DOWN, + BUTTON_REPEAT + }; +public: + Joystick(const QString &name, int axisCount, int buttonCount, int hatCount, QObject *parent = nullptr); virtual ~Joystick(); - typedef struct Calibration_t { - int min; - int max; - int center; - int deadband; - bool reversed; - Calibration_t() - : min(-32767) - , max(32767) - , center(0) - , deadband(0) - , reversed(false) {} - } Calibration_t; - - typedef enum { + struct Calibration_t { + int min = -32767; + int max = 32767; + int center = 0; + int deadband = 0; + bool reversed = false; + }; + + enum AxisFunction_t { rollFunction, pitchFunction, yawFunction, @@ -81,235 +105,202 @@ class Joystick : public QThread gimbalPitchFunction, gimbalYawFunction, maxFunction - } AxisFunction_t; + }; - typedef enum { + enum ThrottleMode_t { ThrottleModeCenterZero, ThrottleModeDownZero, ThrottleModeMax - } ThrottleMode_t; - - Q_PROPERTY(QString name READ name CONSTANT) - Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) - Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) - Q_PROPERTY(int axisCount READ axisCount CONSTANT) - Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) - - //-- Actions assigned to buttons - Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged) - - //-- Actions that can be assigned to buttons - Q_PROPERTY(QmlObjectListModel* assignableActions READ assignableActions NOTIFY assignableActionsChanged) - Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged) - Q_PROPERTY(QString disabledActionName READ disabledActionName CONSTANT) - - Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) - Q_PROPERTY(float axisFrequencyHz READ axisFrequencyHz WRITE setAxisFrequency NOTIFY axisFrequencyHzChanged) - Q_PROPERTY(float minAxisFrequencyHz MEMBER _minAxisFrequencyHz CONSTANT) - Q_PROPERTY(float maxAxisFrequencyHz MEMBER _maxAxisFrequencyHz CONSTANT) - Q_PROPERTY(float buttonFrequencyHz READ buttonFrequencyHz WRITE setButtonFrequency NOTIFY buttonFrequencyHzChanged) - Q_PROPERTY(float minButtonFrequencyHz MEMBER _minButtonFrequencyHz CONSTANT) - Q_PROPERTY(float maxButtonFrequencyHz MEMBER _maxButtonFrequencyHz CONSTANT) - Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) - Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged) - Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) - Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) - - Q_INVOKABLE void setButtonRepeat (int button, bool repeat); - Q_INVOKABLE bool getButtonRepeat (int button); - Q_INVOKABLE void setButtonAction (int button, const QString& action); - Q_INVOKABLE QString getButtonAction (int button); - - // Property accessors - - QString name () { return _name; } - int totalButtonCount () const{ return _totalButtonCount; } - int axisCount () const{ return _axisCount; } - QStringList buttonActions (); - - QmlObjectListModel* assignableActions () { return &_assignableButtonActions; } - QStringList assignableActionTitles () { return _availableActionTitles; } - QString disabledActionName () { return _buttonActionNone; } + }; + + Q_INVOKABLE void setButtonRepeat(int button, bool repeat); + Q_INVOKABLE bool getButtonRepeat(int button); + Q_INVOKABLE void setButtonAction(int button, const QString &action); + Q_INVOKABLE QString getButtonAction(int button) const; + + QString name() const { return _name; } + int totalButtonCount() const { return _totalButtonCount; } + int axisCount() const { return _axisCount; } + QStringList buttonActions() const; + const QmlObjectListModel *assignableActions() const { return _assignableButtonActions; } + QStringList assignableActionTitles() const { return _availableActionTitles; } + QString disabledActionName() const { return _buttonActionNone; } + + void stop(); /// Start the polling thread which will in turn emit joystick signals - void startPolling(Vehicle* vehicle); - void stopPolling(void); + void startPolling(Vehicle *vehicle); + void stopPolling(); - void setCalibration(int axis, Calibration_t& calibration); - Calibration_t getCalibration(int axis); + void setCalibration(int axis, const Calibration_t &calibration); + Calibration_t getCalibration(int axis) const; void setFunctionAxis(AxisFunction_t function, int axis); - int getFunctionAxis(AxisFunction_t function); - - void stop(); + int getFunctionAxis(AxisFunction_t function) const; -/* // Joystick index used by sdl library // Settable because sdl library remaps indices after certain events - virtual int index(void) = 0; - virtual void setIndex(int index) = 0; -*/ - virtual bool requiresCalibration(void) { return true; } + // virtual int index(void) = 0; + // virtual void setIndex(int index) = 0; - int throttleMode (); - void setThrottleMode (int mode); + virtual bool requiresCalibration() const { return true; } - bool negativeThrust () const; - void setNegativeThrust (bool allowNegative); + int throttleMode() const { return _throttleMode; } + void setThrottleMode(int mode); - float exponential () const; - void setExponential (float expo); + bool negativeThrust() const { return _negativeThrust; } + void setNegativeThrust(bool allowNegative); - bool accumulator () const; - void setAccumulator (bool accu); + float exponential() const { return _exponential; } + void setExponential(float expo); - bool deadband () const; - void setDeadband (bool accu); + bool accumulator() const { return _accumulator; } + void setAccumulator(bool accu); - bool circleCorrection () const; - void setCircleCorrection(bool circleCorrection); + bool deadband() const { return _deadband; } + void setDeadband(bool accu); - void setTXMode (int mode); - int getTXMode () { return _transmitterMode; } + bool circleCorrection() const { return _circleCorrection; } + void setCircleCorrection(bool circleCorrection); + + int getTXMode() const { return _transmitterMode; } + void setTXMode(int mode); /// Set the current calibration mode - void setCalibrationMode (bool calibrating); + void setCalibrationMode(bool calibrating); /// Get joystick message rate (in Hz) - float axisFrequencyHz () const{ return _axisFrequencyHz; } + float axisFrequencyHz() const { return _axisFrequencyHz; } /// Set joystick message rate (in Hz) - void setAxisFrequency (float val); + void setAxisFrequency(float val); /// Get joystick button repeat rate (in Hz) - float buttonFrequencyHz () const{ return _buttonFrequencyHz; } + float buttonFrequencyHz() const { return _buttonFrequencyHz; } /// Set joystick button repeat rate (in Hz) - void setButtonFrequency(float val); + void setButtonFrequency(float val); signals: // The raw signals are only meant for use by calibration - void rawAxisValueChanged (int index, int value); - void rawButtonPressedChanged (int index, int pressed); - void calibratedChanged (bool calibrated); - void buttonActionsChanged (); - void assignableActionsChanged (); - void throttleModeChanged (int mode); - void negativeThrustChanged (bool allowNegative); - void exponentialChanged (float exponential); - void accumulatorChanged (bool accumulator); - void enabledChanged (bool enabled); - void circleCorrectionChanged (bool circleCorrection); - void axisValues (float roll, float pitch, float yaw, float throttle); - - void axisFrequencyHzChanged (); - void buttonFrequencyHzChanged (); - void startContinuousZoom (int direction); - void stopContinuousZoom (); - void stepZoom (int direction); - void stepCamera (int direction); - void stepStream (int direction); - void triggerCamera (); - void startVideoRecord (); - void stopVideoRecord (); - void toggleVideoRecord (); - void gimbalPitchStep (int direction); - void gimbalYawStep (int direction); - void centerGimbal (); - void gimbalYawLock (bool lock); - void setArmed (bool arm); - void setVtolInFwdFlight (bool set); - void setFlightMode (const QString& flightMode); - void emergencyStop (); - void gripperAction (GRIPPER_ACTIONS gripperAction); - void landingGearDeploy (); - void landingGearRetract (); + void rawAxisValueChanged(int index, int value); + void rawButtonPressedChanged(int index, int pressed); + void calibratedChanged(bool calibrated); + void buttonActionsChanged(); + void assignableActionsChanged(); + void throttleModeChanged(int mode); + void negativeThrustChanged(bool allowNegative); + void exponentialChanged(float exponential); + void accumulatorChanged(bool accumulator); + void enabledChanged(bool enabled); + void circleCorrectionChanged(bool circleCorrection); + void axisValues(float roll, float pitch, float yaw, float throttle); + + void axisFrequencyHzChanged(); + void buttonFrequencyHzChanged(); + void startContinuousZoom(int direction); + void stopContinuousZoom(); + void stepZoom(int direction); + void stepCamera(int direction); + void stepStream(int direction); + void triggerCamera(); + void startVideoRecord(); + void stopVideoRecord(); + void toggleVideoRecord(); + void gimbalPitchStep(int direction); + void gimbalYawStep(int direction); + void centerGimbal(); + void gimbalYawLock(bool lock); + void setArmed(bool arm); + void setVtolInFwdFlight(bool set); + void setFlightMode(const QString &flightMode); + void emergencyStop(); + void gripperAction(GRIPPER_ACTIONS gripperAction); + void landingGearDeploy(); + void landingGearRetract(); protected: - void _setDefaultCalibration (); - void _saveSettings (); - void _saveButtonSettings (); - void _loadSettings (); - float _adjustRange (int value, Calibration_t calibration, bool withDeadbands); - void _executeButtonAction (const QString& action, bool buttonDown); - int _findAssignableButtonAction(const QString& action); - bool _validAxis (int axis) const; - bool _validButton (int button) const; - void _handleAxis (); - void _handleButtons (); - void _buildActionList (Vehicle* activeVehicle); - -private: - virtual bool _open () = 0; - virtual void _close () = 0; - virtual bool _update () = 0; - - virtual bool _getButton (int i) = 0; - virtual int _getAxis (int i) = 0; - virtual bool _getHat (int hat,int i) = 0; - - void _updateTXModeSettingsKey(Vehicle* activeVehicle); - int _mapFunctionMode(int mode, int function); - void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); - - // Override from QThread - virtual void run(); - -protected: - - enum { - BUTTON_UP, - BUTTON_DOWN, - BUTTON_REPEAT - }; - - static constexpr const float _defaultAxisFrequencyHz = 25.0f; - static constexpr const float _defaultButtonFrequencyHz = 5.0f; - - uint8_t*_rgButtonValues = nullptr; - - std::atomic _exitThread{false}; ///< true: signal thread to exit - bool _calibrationMode = false; - int* _rgAxisValues = nullptr; - Calibration_t* _rgCalibration = nullptr; - ThrottleMode_t _throttleMode = ThrottleModeDownZero; - bool _negativeThrust = false; - float _exponential = 0; - bool _accumulator = false; - bool _deadband = false; - bool _circleCorrection = true; - float _axisFrequencyHz = _defaultAxisFrequencyHz; - float _buttonFrequencyHz = _defaultButtonFrequencyHz; - Vehicle* _activeVehicle = nullptr; - - bool _pollingStartedForCalibration = false; + void _setDefaultCalibration(); QString _name; - bool _calibrated; - int _axisCount; - int _buttonCount; - int _hatCount; - int _hatButtonCount; - int _totalButtonCount; + int _axisCount = 0; + int _buttonCount = 0; + int _hatCount = 0; - static int _transmitterMode; - int _rgFunctionAxis[maxFunction] = {}; - QElapsedTimer _axisTime; - - QmlObjectListModel _assignableButtonActions; - QList _buttonActionArray; - QStringList _availableActionTitles; - MultiVehicleManager* _multiVehicleManager = nullptr; - - CustomActionManager _customActionManager; - - static constexpr const float _minAxisFrequencyHz = 0.25f; - static constexpr const float _maxAxisFrequencyHz = 200.0f; - static constexpr const float _minButtonFrequencyHz = 0.25f; - static constexpr const float _maxButtonFrequencyHz = 50.0f; +private slots: + void _activeVehicleChanged(Vehicle *activeVehicle); + void _vehicleCountChanged(int count); + void _flightModesChanged() { _buildActionList(_activeVehicle); } private: - const char* _txModeSettingsKey = nullptr; + void _saveSettings(); + void _saveButtonSettings(); + void _loadSettings(); + + /// Adjust the raw axis value to the -1:1 range given calibration information + float _adjustRange(int value, const Calibration_t &calibration, bool withDeadbands); + void _executeButtonAction(const QString &action, bool buttonDown); + int _findAssignableButtonAction(const QString &action); + bool _validAxis(int axis) const; + bool _validButton(int button) const; + void _handleAxis(); + void _handleButtons(); + void _buildActionList(Vehicle *activeVehicle); + + void _updateTXModeSettingsKey(Vehicle *activeVehicle); + + /// Relative mappings of axis functions between different TX modes + int _mapFunctionMode(int mode, int function); + + /// Remap current axis functions from current TX mode to new TX mode + void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); - static constexpr const char* _rgFunctionSettingsKey[maxFunction] = { + void run() override; + + virtual bool _open() = 0; + virtual void _close() = 0; + virtual bool _update() = 0; + + virtual bool _getButton(int i) const = 0; + virtual int _getAxis(int i) const = 0; + virtual bool _getHat(int hat, int i) const = 0; + + int _hatButtonCount = 0; + int _totalButtonCount = 0; + int *_rgAxisValues = nullptr; + Calibration_t *_rgCalibration = nullptr; + uint8_t *_rgButtonValues = nullptr; + CustomActionManager *_customActionManager = nullptr; + QmlObjectListModel *_assignableButtonActions = nullptr; + + bool _accumulator = false; + bool _calibrated = false; + bool _calibrationMode = false; + bool _circleCorrection = true; + bool _deadband = false; + bool _negativeThrust = false; + bool _pollingStartedForCalibration = false; + float _axisFrequencyHz = _defaultAxisFrequencyHz; + float _buttonFrequencyHz = _defaultButtonFrequencyHz; + float _exponential = 0; + int _rgFunctionAxis[maxFunction] = {}; + QElapsedTimer _axisTime; + QList _buttonActionArray; + QStringList _availableActionTitles; + std::atomic _exitThread = false; ///< true: signal thread to exit + ThrottleMode_t _throttleMode = ThrottleModeDownZero; + Vehicle *_activeVehicle = nullptr; + const char *_txModeSettingsKey = nullptr; + + static int _transmitterMode; + + static constexpr float _defaultAxisFrequencyHz = 25.0f; + static constexpr float _defaultButtonFrequencyHz = 5.0f; + // Arbitrary Limits + static constexpr float _minAxisFrequencyHz = 0.25f; + static constexpr float _maxAxisFrequencyHz = 200.0f; + static constexpr float _minButtonFrequencyHz = 0.25f; + static constexpr float _maxButtonFrequencyHz = 50.0f; + + static constexpr const char *_rgFunctionSettingsKey[maxFunction] = { "RollAxis", "PitchAxis", "YawAxis", @@ -318,57 +309,52 @@ class Joystick : public QThread "GimbalYawAxis" }; - static constexpr const char* _settingsGroup = "Joysticks"; - static constexpr const char* _calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration - static constexpr const char* _buttonActionNameKey = "ButtonActionName%1"; - static constexpr const char* _buttonActionRepeatKey = "ButtonActionRepeat%1"; - static constexpr const char* _throttleModeSettingsKey = "ThrottleMode"; - static constexpr const char* _negativeThrustSettingsKey = "NegativeThrust"; - static constexpr const char* _exponentialSettingsKey = "Exponential"; - static constexpr const char* _accumulatorSettingsKey = "Accumulator"; - static constexpr const char* _deadbandSettingsKey = "Deadband"; - static constexpr const char* _circleCorrectionSettingsKey = "Circle_Correction"; - static constexpr const char* _axisFrequencySettingsKey = "AxisFrequency"; - static constexpr const char* _buttonFrequencySettingsKey = "ButtonFrequency"; - static constexpr const char* _fixedWingTXModeSettingsKey = "TXMode_FixedWing"; - static constexpr const char* _multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; - static constexpr const char* _roverTXModeSettingsKey = "TXMode_Rover"; - static constexpr const char* _vtolTXModeSettingsKey = "TXMode_VTOL"; - static constexpr const char* _submarineTXModeSettingsKey = "TXMode_Submarine"; - - static constexpr const char* _buttonActionNone = QT_TR_NOOP("No Action"); - static constexpr const char* _buttonActionArm = QT_TR_NOOP("Arm"); - static constexpr const char* _buttonActionDisarm = QT_TR_NOOP("Disarm"); - static constexpr const char* _buttonActionToggleArm = QT_TR_NOOP("Toggle Arm"); - static constexpr const char* _buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); - static constexpr const char* _buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); - static constexpr const char* _buttonActionContinuousZoomIn = QT_TR_NOOP("Continuous Zoom In"); - static constexpr const char* _buttonActionContinuousZoomOut = QT_TR_NOOP("Continuous Zoom Out"); - static constexpr const char* _buttonActionStepZoomIn = QT_TR_NOOP("Step Zoom In"); - static constexpr const char* _buttonActionStepZoomOut = QT_TR_NOOP("Step Zoom Out"); - static constexpr const char* _buttonActionNextStream = QT_TR_NOOP("Next Video Stream"); - static constexpr const char* _buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); - static constexpr const char* _buttonActionNextCamera = QT_TR_NOOP("Next Camera"); - static constexpr const char* _buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); - static constexpr const char* _buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera"); - static constexpr const char* _buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); - static constexpr const char* _buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); - static constexpr const char* _buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); - static constexpr const char* _buttonActionGimbalDown = QT_TR_NOOP("Gimbal Down"); - static constexpr const char* _buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up"); - static constexpr const char* _buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left"); - static constexpr const char* _buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right"); - static constexpr const char* _buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center"); - static constexpr const char* _buttonActionGimbalYawLock = QT_TR_NOOP("Gimbal Yaw Lock"); - static constexpr const char* _buttonActionGimbalYawFollow = QT_TR_NOOP("Gimbal Yaw Follow"); - static constexpr const char* _buttonActionEmergencyStop = QT_TR_NOOP("Emergency Stop"); - static constexpr const char* _buttonActionGripperGrab = QT_TR_NOOP("Gripper Close"); - static constexpr const char* _buttonActionGripperRelease = QT_TR_NOOP("Gripper Open"); - static constexpr const char* _buttonActionLandingGearDeploy= QT_TR_NOOP("Landing gear deploy"); - static constexpr const char* _buttonActionLandingGearRetract= QT_TR_NOOP("Landing gear retract"); - -private slots: - void _activeVehicleChanged(Vehicle* activeVehicle); - void _vehicleCountChanged(int count); - void _flightModesChanged(); + static constexpr const char *_settingsGroup = "Joysticks"; + static constexpr const char *_calibratedSettingsKey = "Calibrated4"; // Increment number to force recalibration + static constexpr const char *_buttonActionNameKey = "ButtonActionName%1"; + static constexpr const char *_buttonActionRepeatKey = "ButtonActionRepeat%1"; + static constexpr const char *_throttleModeSettingsKey = "ThrottleMode"; + static constexpr const char *_negativeThrustSettingsKey = "NegativeThrust"; + static constexpr const char *_exponentialSettingsKey = "Exponential"; + static constexpr const char *_accumulatorSettingsKey = "Accumulator"; + static constexpr const char *_deadbandSettingsKey = "Deadband"; + static constexpr const char *_circleCorrectionSettingsKey = "Circle_Correction"; + static constexpr const char *_axisFrequencySettingsKey = "AxisFrequency"; + static constexpr const char *_buttonFrequencySettingsKey = "ButtonFrequency"; + static constexpr const char *_fixedWingTXModeSettingsKey = "TXMode_FixedWing"; + static constexpr const char *_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; + static constexpr const char *_roverTXModeSettingsKey = "TXMode_Rover"; + static constexpr const char *_vtolTXModeSettingsKey = "TXMode_VTOL"; + static constexpr const char *_submarineTXModeSettingsKey = "TXMode_Submarine"; + + static constexpr const char *_buttonActionNone = QT_TR_NOOP("No Action"); + static constexpr const char *_buttonActionArm = QT_TR_NOOP("Arm"); + static constexpr const char *_buttonActionDisarm = QT_TR_NOOP("Disarm"); + static constexpr const char *_buttonActionToggleArm = QT_TR_NOOP("Toggle Arm"); + static constexpr const char *_buttonActionVTOLFixedWing = QT_TR_NOOP("VTOL: Fixed Wing"); + static constexpr const char *_buttonActionVTOLMultiRotor = QT_TR_NOOP("VTOL: Multi-Rotor"); + static constexpr const char *_buttonActionContinuousZoomIn = QT_TR_NOOP("Continuous Zoom In"); + static constexpr const char *_buttonActionContinuousZoomOut = QT_TR_NOOP("Continuous Zoom Out"); + static constexpr const char *_buttonActionStepZoomIn = QT_TR_NOOP("Step Zoom In"); + static constexpr const char *_buttonActionStepZoomOut = QT_TR_NOOP("Step Zoom Out"); + static constexpr const char *_buttonActionNextStream = QT_TR_NOOP("Next Video Stream"); + static constexpr const char *_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); + static constexpr const char *_buttonActionNextCamera = QT_TR_NOOP("Next Camera"); + static constexpr const char *_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); + static constexpr const char *_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera"); + static constexpr const char *_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); + static constexpr const char *_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); + static constexpr const char *_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); + static constexpr const char *_buttonActionGimbalDown = QT_TR_NOOP("Gimbal Down"); + static constexpr const char *_buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up"); + static constexpr const char *_buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left"); + static constexpr const char *_buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right"); + static constexpr const char *_buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center"); + static constexpr const char *_buttonActionGimbalYawLock = QT_TR_NOOP("Gimbal Yaw Lock"); + static constexpr const char *_buttonActionGimbalYawFollow = QT_TR_NOOP("Gimbal Yaw Follow"); + static constexpr const char *_buttonActionEmergencyStop = QT_TR_NOOP("Emergency Stop"); + static constexpr const char *_buttonActionGripperGrab = QT_TR_NOOP("Gripper Close"); + static constexpr const char *_buttonActionGripperRelease = QT_TR_NOOP("Gripper Open"); + static constexpr const char *_buttonActionLandingGearDeploy= QT_TR_NOOP("Landing gear deploy"); + static constexpr const char *_buttonActionLandingGearRetract= QT_TR_NOOP("Landing gear retract"); }; diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc index e0cc24f7f2a..58b5cbd9b25 100644 --- a/src/Joystick/JoystickAndroid.cc +++ b/src/Joystick/JoystickAndroid.cc @@ -9,7 +9,6 @@ #include "JoystickAndroid.h" #include "JoystickManager.h" -#include "MultiVehicleManager.h" #include "QGCLoggingCategory.h" #include @@ -32,8 +31,8 @@ static void clear_jni_exception() } } -JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager) - : Joystick(name,axisCount,buttonCount,0,multiVehicleManager) +JoystickAndroid::JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id) + : Joystick(name,axisCount,buttonCount,0) , deviceId(id) { int i; @@ -95,7 +94,7 @@ JoystickAndroid::~JoystickAndroid() { } -QMap JoystickAndroid::discover(MultiVehicleManager* _multiVehicleManager) { +QMap JoystickAndroid::discover() { static QMap ret; QMutexLocker lock(&m_mutex); @@ -144,7 +143,7 @@ QMap JoystickAndroid::discover(MultiVehicleManager* _multiVe qCDebug(JoystickLog) << "\t" << name << "id:" << buff[i] << "axes:" << axisCount << "buttons:" << buttonCount; - ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i], _multiVehicleManager); + ret[name] = new JoystickAndroid(name, axisCount, buttonCount, buff[i]); } for (auto i = ret.begin(); i != ret.end();) { @@ -205,15 +204,18 @@ bool JoystickAndroid::_update(void) return true; } -bool JoystickAndroid::_getButton(int i) { +bool JoystickAndroid::_getButton(int i) const +{ return btnValue[ i ]; } -int JoystickAndroid::_getAxis(int i) { +int JoystickAndroid::_getAxis(int i) const +{ return axisValue[ i ]; } -int JoystickAndroid::_getAndroidHatAxis(int axisHatCode) { +int JoystickAndroid::_getAndroidHatAxis(int axisHatCode) const +{ for(int i = 0; i < _axisCount; i++) { if (axisCode[i] == axisHatCode) { return _getAxis(i); @@ -222,7 +224,8 @@ int JoystickAndroid::_getAndroidHatAxis(int axisHatCode) { return 0; } -bool JoystickAndroid::_getHat(int hat,int i) { +bool JoystickAndroid::_getHat(int hat,int i) const +{ // Android supports only one hat button if (hat != 0) { return false; diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h index 8209d7751e8..0d22d38957b 100644 --- a/src/Joystick/JoystickAndroid.h +++ b/src/Joystick/JoystickAndroid.h @@ -12,13 +12,12 @@ #include "Joystick.h" #include -class MultiVehicleManager; class JoystickManager; class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionEventListener, public QtAndroidPrivate::KeyEventListener { public: - JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id, MultiVehicleManager* multiVehicleManager); + JoystickAndroid(const QString& name, int axisCount, int buttonCount, int id); ~JoystickAndroid(); @@ -26,20 +25,20 @@ class JoystickAndroid : public Joystick, public QtAndroidPrivate::GenericMotionE static void setNativeMethods(); - static QMap discover(MultiVehicleManager* _multiVehicleManager); + static QMap discover(); private: bool handleKeyEvent(jobject event); bool handleGenericMotionEvent(jobject event); - int _getAndroidHatAxis(int axisHatCode); + int _getAndroidHatAxis(int axisHatCode) const; - virtual bool _open (); - virtual void _close (); - virtual bool _update (); + bool _open () final; + void _close () final; + bool _update () final; - virtual bool _getButton (int i); - virtual int _getAxis (int i); - virtual bool _getHat (int hat,int i); + bool _getButton (int i) const final; + int _getAxis (int i) const final; + bool _getHat (int hat,int i) const final; int *btnCode; int *axisCode; diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc index c2ae0893c42..5f83bb01ae2 100644 --- a/src/Joystick/JoystickManager.cc +++ b/src/Joystick/JoystickManager.cc @@ -9,7 +9,6 @@ #include "JoystickManager.h" -#include "MultiVehicleManager.h" #include "Joystick.h" #if defined(QGC_SDL_JOYSTICK) #include "JoystickSDL.h" @@ -27,7 +26,6 @@ QGC_LOGGING_CATEGORY(JoystickManagerLog, "JoystickManagerLog") JoystickManager::JoystickManager(QGCApplication* app, QGCToolbox* toolbox) : QGCTool(app, toolbox) , _activeJoystick(nullptr) - , _multiVehicleManager(nullptr) { // qCDebug(JoystickManagerLog) << Q_FUNC_INFO << this; } @@ -48,8 +46,6 @@ void JoystickManager::setToolbox(QGCToolbox *toolbox) { QGCTool::setToolbox(toolbox); - _multiVehicleManager = _toolbox->multiVehicleManager(); - QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); qmlRegisterUncreatableType("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); @@ -78,9 +74,9 @@ void JoystickManager::_setActiveJoystickFromSettings(void) #ifdef QGC_SDL_JOYSTICK // Get the latest joystick mapping - newMap = JoystickSDL::discover(_multiVehicleManager); + newMap = JoystickSDL::discover(); #elif defined(Q_OS_ANDROID) - newMap = JoystickAndroid::discover(_multiVehicleManager); + newMap = JoystickAndroid::discover(); #endif if (_activeJoystick && !newMap.contains(_activeJoystick->name())) { diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h index b36a0510bd0..3521107e417 100644 --- a/src/Joystick/JoystickManager.h +++ b/src/Joystick/JoystickManager.h @@ -20,7 +20,6 @@ Q_DECLARE_LOGGING_CATEGORY(JoystickManagerLog) -class MultiVehicleManager; class Joystick; /// Joystick Manager @@ -75,7 +74,6 @@ private slots: private: Joystick* _activeJoystick; QMap _name2JoystickMap; - MultiVehicleManager* _multiVehicleManager; int _joystickCheckTimerCounter; QTimer _joystickCheckTimer; diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 92d27b6f2f4..ae19afedb60 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -8,15 +8,14 @@ ****************************************************************************/ #include "JoystickSDL.h" -#include "MultiVehicleManager.h" #include "QGCLoggingCategory.h" #include #include #include -JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager) - : Joystick(name,axisCount,buttonCount,hatCount,multiVehicleManager) +JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController) + : Joystick(name,axisCount,buttonCount,hatCount) , _isGameController(isGameController) , _index(index) { @@ -41,7 +40,7 @@ bool JoystickSDL::init(void) { return true; } -QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { +QMap JoystickSDL::discover() { static QMap ret; QMap newRet; @@ -83,7 +82,7 @@ QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicl name = QString("%1 %2").arg(originalName).arg(duplicateIdx++); } - newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager); + newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController); } else { newRet[name] = ret[name]; JoystickSDL *j = static_cast(newRet[name]); @@ -166,7 +165,7 @@ bool JoystickSDL::_update(void) return true; } -bool JoystickSDL::_getButton(int i) { +bool JoystickSDL::_getButton(int i) const { if (_isGameController) { return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1; } else { @@ -174,7 +173,7 @@ bool JoystickSDL::_getButton(int i) { } } -int JoystickSDL::_getAxis(int i) { +int JoystickSDL::_getAxis(int i) const { if (_isGameController) { return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i)); } else { @@ -182,7 +181,7 @@ int JoystickSDL::_getAxis(int i) { } } -bool JoystickSDL::_getHat(int hat, int i) { +bool JoystickSDL::_getHat(int hat, int i) const { uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT}; if (i < int(sizeof(hatButtons))) { return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0; diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h index bb56b2851f4..527e3ac1871 100644 --- a/src/Joystick/JoystickSDL.h +++ b/src/Joystick/JoystickSDL.h @@ -24,10 +24,10 @@ class MultiVehicleManager; class JoystickSDL : public Joystick { public: - JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager); + JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController); ~JoystickSDL(); - static QMap discover(MultiVehicleManager* _multiVehicleManager); + static QMap discover(); static bool init(void); int index(void) const { return _index; } @@ -43,9 +43,9 @@ class JoystickSDL : public Joystick void _close () final; bool _update () final; - bool _getButton (int i) final; - int _getAxis (int i) final; - bool _getHat (int hat,int i) final; + bool _getButton (int i) const final; + int _getAxis (int i) const final; + bool _getHat (int hat,int i) const final; SDL_Joystick* sdlJoystick; SDL_GameController* sdlController; diff --git a/src/MAVLink/QGCMAVLink.cc b/src/MAVLink/QGCMAVLink.cc index bb61af2b191..7092c7dc709 100644 --- a/src/MAVLink/QGCMAVLink.cc +++ b/src/MAVLink/QGCMAVLink.cc @@ -38,6 +38,8 @@ QGCMAVLink::QGCMAVLink(QObject *parent) : QObject(parent) { // qCDebug(StatusTextHandlerLog) << Q_FUNC_INFO << this; + + (void) qRegisterMetaType("GRIPPER_ACTIONS"); } QGCMAVLink::~QGCMAVLink() diff --git a/src/MAVLink/QGCMAVLink.h b/src/MAVLink/QGCMAVLink.h index ad345a9c1fc..dbe06073b50 100644 --- a/src/MAVLink/QGCMAVLink.h +++ b/src/MAVLink/QGCMAVLink.h @@ -160,3 +160,4 @@ class QGCMAVLink : public QObject static mavlink_status_t* getChannelStatus(mavlink_channel_t channel) { return mavlink_get_channel_status(static_cast(channel)); } }; +Q_DECLARE_METATYPE(GRIPPER_ACTIONS) diff --git a/src/Utilities/QGCLoggingCategory.cc b/src/Utilities/QGCLoggingCategory.cc index fb514ed23d1..74b3efb414d 100644 --- a/src/Utilities/QGCLoggingCategory.cc +++ b/src/Utilities/QGCLoggingCategory.cc @@ -28,8 +28,6 @@ QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") QGC_LOGGING_CATEGORY(GuidedActionsControllerLog, "GuidedActionsControllerLog") QGC_LOGGING_CATEGORY(LocalizationLog, "LocalizationLog") QGC_LOGGING_CATEGORY(VideoAllLog, kVideoAllLogCategory) -QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") - QGCLoggingCategoryRegister* _instance = nullptr; diff --git a/src/Utilities/QGCLoggingCategory.h b/src/Utilities/QGCLoggingCategory.h index d42652c46a3..ec8ace4d3cf 100644 --- a/src/Utilities/QGCLoggingCategory.h +++ b/src/Utilities/QGCLoggingCategory.h @@ -24,7 +24,6 @@ Q_DECLARE_LOGGING_CATEGORY(RTKGPSLog) Q_DECLARE_LOGGING_CATEGORY(GuidedActionsControllerLog) Q_DECLARE_LOGGING_CATEGORY(LocalizationLog) Q_DECLARE_LOGGING_CATEGORY(VideoAllLog) // turns on all individual QGC video logs -Q_DECLARE_LOGGING_CATEGORY(JoystickLog) /// @def QGC_LOGGING_CATEGORY /// This is a QGC specific replacement for Q_LOGGING_CATEGORY. It will register the category name into a