diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 76efe1f47d60..4c0fec78fa77 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -24,15 +24,15 @@ #include #include -QGC_LOGGING_CATEGORY(JoystickLog, "Joystick.joystick") -QGC_LOGGING_CATEGORY(JoystickValuesLog, "Joystick.joystickvalues") +QGC_LOGGING_CATEGORY(JoystickLog, "Joystick.Joystick") +QGC_LOGGING_CATEGORY(JoystickValuesLog, "Joystick.JoystickValues") /*===========================================================================*/ AssignedButtonAction::AssignedButtonAction(const QString &name) : action(name) { - // qCDebug(JoystickLog) << Q_FUNC_INFO << this; + qCDebug(JoystickLog) << this; } AssignableButtonAction::AssignableButtonAction(const QString &action_, bool canRepeat_, QObject *parent) @@ -40,7 +40,7 @@ AssignableButtonAction::AssignableButtonAction(const QString &action_, bool canR , _action(action_) , _repeat(canRepeat_) { - // qCDebug(JoystickLog) << Q_FUNC_INFO << this; + qCDebug(JoystickLog) << this; } /*===========================================================================*/ @@ -61,7 +61,7 @@ Joystick::Joystick(const QString &name, int axisCount, int buttonCount, int hatC , _mavlinkActionManager(new MavlinkActionManager(SettingsManager::instance()->mavlinkActionsSettings()->joystickActionsFile(), this)) , _assignableButtonActions(new QmlObjectListModel(this)) { - qCDebug(JoystickLog) << Q_FUNC_INFO << this << name << "axisCount:buttonCount:hatCount" << axisCount << buttonCount << hatCount; + qCDebug(JoystickLog) << this << name << "axisCount:buttonCount:hatCount" << axisCount << buttonCount << hatCount; for (int i = 0; i < _axisCount; i++) { _rgAxisValues[i] = 0; @@ -93,7 +93,7 @@ Joystick::~Joystick() _assignableButtonActions->clearAndDeleteContents(); qDeleteAll(_buttonActionArray); - // qCDebug(JoystickLog) << Q_FUNC_INFO << this; + qCDebug(JoystickLog)<< this; } void Joystick::stop() @@ -135,8 +135,8 @@ void Joystick::_setDefaultCalibration() _exponential = 0; _accumulator = false; _deadband = false; - _axisFrequencyHz = _defaultAxisFrequencyHz; - _buttonFrequencyHz = _defaultButtonFrequencyHz; + _axisFrequencyHz = kDefaultAxisFrequencyHz; + _buttonFrequencyHz = kDefaultButtonFrequencyHz; _throttleMode = ThrottleModeDownZero; _calibrated = true; _circleCorrection = false; @@ -193,7 +193,7 @@ void Joystick::_loadSettings() QSettings settings; settings.beginGroup(_settingsGroup); - Vehicle *const activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + Vehicle *activeVehicle = MultiVehicleManager::instance()->activeVehicle(); if (_txModeSettingsKey && activeVehicle) { _transmitterMode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); } @@ -207,8 +207,8 @@ void Joystick::_loadSettings() _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); + _axisFrequencyHz = settings.value(_axisFrequencySettingsKey, kDefaultAxisFrequencyHz).toFloat(&convertOk); + _buttonFrequencyHz = settings.value(_buttonFrequencySettingsKey, kDefaultButtonFrequencyHz).toFloat(&convertOk); _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); _negativeThrust = settings.value(_negativeThrustSettingsKey, false).toBool(); _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); @@ -220,11 +220,11 @@ void Joystick::_loadSettings() qCDebug(JoystickLog) << " calibrated:txmode:throttlemode:exponential:deadband:badsettings"; qCDebug(JoystickLog) << " " << _calibrated << _transmitterMode << _throttleMode << _exponential << _deadband << badSettings; - 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"); + static const QString minTpl("Axis%1Min"); + static const QString maxTpl("Axis%1Max"); + static const QString trimTpl("Axis%1Trim"); + static const QString revTpl("Axis%1Rev"); + static const QString deadbndTpl("Axis%1Deadbnd"); qCDebug(JoystickLog) << " axis:min:max:trim:reversed:deadband:badsettings"; for (int axis = 0; axis < _axisCount; axis++) { @@ -269,7 +269,7 @@ void Joystick::_loadSettings() // 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++) { const QString buttonAction = settings.value(QString(_buttonActionNameKey).arg(button), QString()).toString(); @@ -282,12 +282,12 @@ void Joystick::_loadSettings() _buttonActionArray[button] = nullptr; } - AssignedButtonAction *const ap = new AssignedButtonAction(buttonAction); + AssignedButtonAction *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; + qCDebug(JoystickLog) << "button:action" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; } if (badSettings) { @@ -306,7 +306,7 @@ void Joystick::_saveButtonSettings() 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; + qCDebug(JoystickLog) << "button:action:repeat" << button << _buttonActionArray[button]->action << _buttonActionArray[button]->repeat; } } } @@ -416,16 +416,15 @@ void Joystick::setTXMode(int mode) float Joystick::_adjustRange(int value, const Calibration_t &calibration, bool withDeadbands) { + float axisBasis; float valueNormalized; float axisLength; - float axisBasis; - if (value > calibration.center) { - axisBasis = 1.0f; + axisBasis = kMaxAxisValue; valueNormalized = value - calibration.center; axisLength = calibration.max - calibration.center; } else { - axisBasis = -1.0f; + axisBasis = kMinAxisValue; valueNormalized = calibration.center - value; axisLength = calibration.center - calibration.min; } @@ -448,7 +447,7 @@ float Joystick::_adjustRange(int value, const Calibration_t &calibration, bool w correctedValue *= -1.0f; } - return std::max(-1.0f, std::min(correctedValue, 1.0f)); + return qBound(-1.0f, correctedValue, 1.0f); } void Joystick::run() @@ -469,8 +468,9 @@ void Joystick::run() if (axisCount() != 0) { _handleAxis(); } + emit dataUpdated(axisValues, buttons); - const int sleep = qMin(static_cast(1000.0f / _maxAxisFrequencyHz), static_cast(1000.0f / _maxButtonFrequencyHz)) / 2; + const int sleep = qMin(static_cast(1000.0f / kMaxAxisFrequencyHz), static_cast(1000.0f / kMaxButtonFrequencyHz)) / 2; QThread::msleep(sleep); } @@ -597,7 +597,7 @@ void Joystick::_handleAxis() _axisTime.start(); for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) { - int newAxisValue = _getAxis(axisIndex); + const int newAxisValue = _getAxis(axisIndex); // Calibration code requires signal to be emitted even if value hasn't changed _rgAxisValues[axisIndex] = newAxisValue; emit rawAxisValueChanged(axisIndex, newAxisValue); @@ -614,84 +614,68 @@ void Joystick::_handleAxis() float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); axis = _rgFunctionAxis[yawFunction]; - float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); axis = _rgFunctionAxis[throttleFunction]; - float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false :_deadband); + float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false : _deadband); float gimbalPitch = NAN; if (_enableManualControlExtensions && _axisCount > 4) { axis = _rgFunctionAxis[gimbalPitchFunction]; - gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); } float gimbalYaw = NAN; if (_enableManualControlExtensions && _axisCount > 5) { axis = _rgFunctionAxis[gimbalYawFunction]; - gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + gimbalYaw = _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; + _throttleAccumulator += (throttle * (_axisFrequencyHz / kAccumulatorMaxSlewRate)); + _throttleAccumulator = qBound(kMinAxisValue, _throttleAccumulator, kMaxAxisValue); + throttle = _throttleAccumulator; } 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))); + const float roll_limited = qBound(-M_PI_4, roll, M_PI_4); + const float pitch_limited = qBound(-M_PI_4, pitch, M_PI_4); + const float yaw_limited = qBound(-M_PI_4, yaw, M_PI_4); + const float throttle_limited = qBound(-M_PI_4, throttle, 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)); + roll = qBound(-1.0f, tanf(asinf(roll_limited)), 1.0f); + pitch = qBound(-1.0f, tanf(asinf(pitch_limited)), 1.0f); + yaw = qBound(-1.0f, tanf(asinf(yaw_limited)), 1.0f); + throttle = qBound(-1.0f, 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); + 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); + throttle = qBound(0.0f, throttle, kMaxAxisValue); } } 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++) { - const quint64 buttonBit = static_cast(1LL << buttonIndex); - if (_rgButtonValues[buttonIndex] != BUTTON_UP) { - buttonPressedBits |= buttonBit; - } - } - - emit axisValues(roll, pitch, yaw, throttle); - - const uint16_t lowButtons = static_cast(buttonPressedBits & 0xFFFF); - const uint16_t highButtons = static_cast((buttonPressedBits >> 16) & 0xFFFF); - _activeVehicle->sendJoystickDataThreadSafe(roll, pitch, yaw, throttle, lowButtons, highButtons, gimbalPitch, gimbalYaw); + const QList axisValues = { roll, pitch, yaw, throttle, gimbalPitch, gimbalYaw }; + qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << axisValues; + emit axisValuesUpdated(axisValues); } void Joystick::startPolling(Vehicle* vehicle) { if (vehicle) { if (_activeVehicle) { + (void) disconnect(this, &Joystick::dataUpdated, _activeVehicle, &Vehicle::sendJoystickData); (void) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); (void) disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); (void) disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); @@ -723,15 +707,16 @@ void Joystick::startPolling(Vehicle* vehicle) if (vehicle->joystickEnabled()) { _pollingStartedForCalibration = false; + (void) connect(this, &Joystick::dataUpdated, _activeVehicle, &Vehicle::sendJoystickData); (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::gimbalPitchStart, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStart); - (void) connect(this, &Joystick::gimbalYawStart, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStart); - (void) connect(this, &Joystick::gimbalPitchStop, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStop); - (void) connect(this, &Joystick::gimbalYawStop, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStop); + (void) connect(this, &Joystick::gimbalPitchStart, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStart); + (void) connect(this, &Joystick::gimbalYawStart, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStart); + (void) connect(this, &Joystick::gimbalPitchStop, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStop); + (void) connect(this, &Joystick::gimbalYawStop, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStop); (void) connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); (void) connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::sendGripperAction); (void) connect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); @@ -755,6 +740,7 @@ void Joystick::stopPolling() } if (_activeVehicle && _activeVehicle->joystickEnabled()) { + (void) disconnect(this, &Joystick::dataUpdated, _activeVehicle, &Vehicle::sendJoystickData); (void) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); (void) disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); (void) disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); @@ -967,8 +953,8 @@ void Joystick::setCircleCorrection(bool circleCorrection) void Joystick::setAxisFrequency(float val) { float result = val; - result = qMax(_minAxisFrequencyHz, result); - result = qMin(_maxAxisFrequencyHz, result); + result = qMax(kMinAxisFrequencyHz, result); + result = qMin(kMaxAxisFrequencyHz, result); if (result != _axisFrequencyHz) { _axisFrequencyHz = result; @@ -980,8 +966,8 @@ void Joystick::setAxisFrequency(float val) void Joystick::setButtonFrequency(float val) { float result = val; - result = qMax(_minButtonFrequencyHz, result); - result = qMin(_maxButtonFrequencyHz, result); + result = qMax(kMnButtonFrequencyHz, result); + result = qMin(kMaxButtonFrequencyHz, result); if (result != _buttonFrequencyHz) { _buttonFrequencyHz = result; @@ -1274,4 +1260,77 @@ void Joystick::setEnableManualControlExtensions(bool enable) _saveSettings(); emit enableManualControlExtensionsChanged(); } -} \ No newline at end of file +} + +QBitArray Joystick::getButtonStates() const +{ + QBitArray buttons(32); + for (int i = 0; i < _totalButtonCount; i++) { + const bool pressed = _rgButtonValues[ + i] != BUTTON_UP; + buttons.setBit(i, pressed); + } + return buttons; +} + +QList Joystick::getAxisValues() const +{ + int axis = _rgFunctionAxis[rollFunction]; + float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[pitchFunction]; + float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[yawFunction]; + float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + + axis = _rgFunctionAxis[throttleFunction]; + float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], (_throttleMode == ThrottleModeDownZero) ? false : _deadband); + + float gimbalPitch = NAN; + if (_axisCount > 4) { + axis = _rgFunctionAxis[gimbalPitchFunction]; + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + } + + float gimbalYaw = NAN; + if (_axisCount > 5) { + axis = _rgFunctionAxis[gimbalYawFunction]; + gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + } + + if (_accumulator) { + _throttleAccumulator += (throttle * (_axisFrequencyHz / kAccumulatorMaxSlewRate)); + _throttleAccumulator = qBound(kMinAxisValue, _throttleAccumulator, kMaxAxisValue); + throttle = _throttleAccumulator; + } + + if (_circleCorrection) { + const float roll_limited = qBound(-M_PI_4, roll, M_PI_4); + const float pitch_limited = qBound(-M_PI_4, pitch, M_PI_4); + const float yaw_limited = qBound(-M_PI_4, yaw, M_PI_4); + const float throttle_limited = qBound(-M_PI_4, throttle, M_PI_4); + + // Map from unit circle to linear range and limit + roll = qBound(-1.0f, tanf(asinf(roll_limited)), 1.0f); + pitch = qBound(-1.0f, tanf(asinf(pitch_limited)), 1.0f); + yaw = qBound(-1.0f, tanf(asinf(yaw_limited)), 1.0f); + throttle = qBound(-1.0f, tanf(asinf(throttle_limited)), 1.0f); + } + + if ( _exponential < -0.01f) { + // Exponential (0% to -50% range like most RC radios) + 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 = qBound(0.0f, throttle, kMaxAxisValue); + } + } else { + throttle = (throttle + 1.0f) / 2.0f; + } +} diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 3c359cee3f2a..3b485cec78f4 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -83,7 +83,6 @@ class Joystick : public QThread Q_PROPERTY(QStringList assignableActionTitles READ assignableActionTitles NOTIFY assignableActionsChanged) Q_PROPERTY(QStringList buttonActions READ buttonActions NOTIFY buttonActionsChanged) Q_PROPERTY(bool enableManualControlExtensions READ enableManualControlExtensions WRITE setEnableManualControlExtensions NOTIFY enableManualControlExtensionsChanged) - enum ButtonEvent_t { BUTTON_UP, BUTTON_DOWN, @@ -188,6 +187,9 @@ class Joystick : public QThread bool enableManualControlExtensions() const { return _enableManualControlExtensions; } void setEnableManualControlExtensions(bool enable); + QBitArray getButtonStates() const; + QList getAxisValues() const; + signals: // The raw signals are only meant for use by calibration void rawAxisValueChanged(int index, int value); @@ -203,6 +205,8 @@ class Joystick : public QThread void circleCorrectionChanged(bool circleCorrection); void enableManualControlExtensionsChanged(); void axisValues(float roll, float pitch, float yaw, float throttle); + void axisValuesUpdated(QList axisValues); + void dataUpdated(QList axisValues, QBitArray buttons); void axisFrequencyHzChanged(); void buttonFrequencyHzChanged(); void startContinuousZoom(int direction); @@ -286,6 +290,7 @@ private slots: QmlObjectListModel *_assignableButtonActions = nullptr; bool _accumulator = false; + float _throttleAccumulator = 0.f; bool _calibrated = false; bool _calibrationMode = false; bool _circleCorrection = true; @@ -307,13 +312,20 @@ private slots: static int _transmitterMode; - static constexpr float _defaultAxisFrequencyHz = 25.0f; - static constexpr float _defaultButtonFrequencyHz = 5.0f; + static constexpr float kMinAxisValue = -1.0f; + static constexpr float kMaxAxisValue = 1.0f; + + static constexpr float kDefaultAxisFrequencyHz = 25.0f; + static constexpr float kDefaultButtonFrequencyHz = 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 float kMinAxisFrequencyHz = 0.25f; + static constexpr float kMaxAxisFrequencyHz = 200.0f; + static constexpr float kMinButtonFrequencyHz = 0.25f; + static constexpr float kMaxButtonFrequencyHz = 50.0f; + + // for throttle to change from min to max it will take 1000ms + static constexpr float kAccumulatorMaxSlewRate = 1000.0f; static constexpr const char *_rgFunctionSettingsKey[maxAxisFunction] = { "RollAxis", diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc index 0f7c128f65f1..08c112b08200 100644 --- a/src/Joystick/JoystickAndroid.cc +++ b/src/Joystick/JoystickAndroid.cc @@ -15,7 +15,7 @@ #include #include -QGC_LOGGING_CATEGORY(JoystickAndroidLog, "Joystick.joystickandroid") +QGC_LOGGING_CATEGORY(JoystickAndroidLog, "Joystick.JoystickAndroid") QList JoystickAndroid::_androidBtnList(_androidBtnListCount); int JoystickAndroid::ACTION_DOWN = 0; diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc index 2bac13c32b53..266706188943 100644 --- a/src/Joystick/JoystickManager.cc +++ b/src/Joystick/JoystickManager.cc @@ -20,7 +20,7 @@ #include #include -QGC_LOGGING_CATEGORY(JoystickManagerLog, "Joystick.joystickmanager") +QGC_LOGGING_CATEGORY(JoystickManagerLog, "Joystick.JoystickManager") Q_APPLICATION_STATIC(JoystickManager, _joystickManager); diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 7b742f411012..eadfcc123a30 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -16,7 +16,7 @@ #include -QGC_LOGGING_CATEGORY(JoystickSDLLog, "Joystick.joysticksdl") +QGC_LOGGING_CATEGORY(JoystickSDLLog, "Joystick.JoystickSDL") JoystickSDL::JoystickSDL(const QString &name, QList gamepadAxes, QList nonGamepadAxes, int buttonCount, int hatCount, int instanceId, bool isGamepad, QObject *parent) : Joystick(name, gamepadAxes.length() + nonGamepadAxes.length(), buttonCount, hatCount, parent) diff --git a/src/MAVLink/MAVLinkMessages.cc b/src/MAVLink/MAVLinkMessages.cc new file mode 100644 index 000000000000..e2a0de62b37a --- /dev/null +++ b/src/MAVLink/MAVLinkMessages.cc @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "MAVLinkMessages.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(MAVLinkMessagesLog, "MAVLink.MAVLinkMessages") + +namespace MAVLinkMessages +{ + +void ManualControl::sendManualControlMsg(int16_t x, int16_t y, int16_t z, int16_t r, + int16_t s, int16_t t, + int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6, + QBitArray buttons) +{ + mavlink_manual_control_t manual_control{}; + manual_control.x = x; + manual_control.y = y; + manual_control.z = z; + manual_control.r = r; + + manual_control.s = s; + manual_control.t = t; + manual_control.aux1 = aux1; + manual_control.aux2 = aux2; + manual_control.aux3 = aux3; + manual_control.aux4 = aux4; + manual_control.aux5 = aux5; + manual_control.aux6 = aux6; + + bool ok; + const quint32 buttonPressed = buttons.toUInt32(QSysInfo::Endian::LittleEndian, &ok); + if (ok) { + manual_control.buttons = static_cast(buttonPressed); + manual_control.buttons2 = static_cast((buttonPressed >> 16) & 0xFFFF); + } +} + +} // namespace MAVLinkMessages diff --git a/src/MAVLink/MAVLinkMessages.h b/src/MAVLink/MAVLinkMessages.h new file mode 100644 index 000000000000..623c70f4296d --- /dev/null +++ b/src/MAVLink/MAVLinkMessages.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * (c) 2009-2024 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "MAVLinkLib.h" + +#include + +Q_DECLARE_LOGGING_CATEGORY(QGCMAVLinkMessagesLog) + +namespace MAVLinkMessages +{ + +class MAVLinkMessage +{ +public: + MAVLinkMessage() = default; + ~MAVLinkMessage() = default; + +protected: + mavlink_message_t message; +} + +class ManualControl : public MAVLinkMessage +{ +public: + ManualControl() = default; + +private: + int16_t x = INT16_MAX; + int16_t y = INT16_MAX; + int16_t z = INT16_MAX; + int16_t r = INT16_MAX; + + QBitArray buttons{32}; + + QBitArray extensions{8}; + + int16_t s = 0; + int16_t t = 0; + + int16_t aux1 = 0; + int16_t aux2 = 0; + int16_t aux3 = 0; + int16_t aux4 = 0; + int16_t aux5 = 0; + int16_t aux6 = 0; + + mavlink_manual_control_t manual_control{}; +}; + +}; // namespace MAVLinkMessages diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b9fad0df7b6a..5f1ae99141d3 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -117,7 +117,6 @@ Vehicle::Vehicle(LinkInterface* link, , _terrainFactGroup (this) , _terrainProtocolHandler (new TerrainProtocolHandler(this, &_terrainFactGroup, this)) { - connect(JoystickManager::instance(), &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadJoystickSettings); connect(MultiVehicleManager::instance(), &MultiVehicleManager::activeVehicleChanged, this, &Vehicle::_activeVehicleChanged); qCDebug(VehicleLog) << "Link started with Mavlink " << (MAVLinkProtocol::instance()->getCurrentVersion() >= 200 ? "V2" : "V1"); @@ -363,8 +362,7 @@ void Vehicle::_commonInit(LinkInterface* link) SettingsManager::instance()->videoSettings()->lowLatencyMode()->setRawValue(true); } - // enable Joystick if appropriate - _loadJoystickSettings(); + _initJoystickHandling(); _gimbalController = new GimbalController(this); @@ -1511,72 +1509,10 @@ bool Vehicle::xConfigMotors() return _firmwarePlugin->multiRotorXConfig(this); } -// this function called in three cases: -// 1. On constructor of vehicle, to see if we should enable a joystick -// 2. When there is a new active joystick -// 3. When the active joystick is disconnected (even if there isnt a new one) -void Vehicle::_loadJoystickSettings() -{ - QSettings settings; - settings.beginGroup(QString(_settingsGroup).arg(_id)); - - if (JoystickManager::instance()->activeJoystick()) { - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Notified of an active joystick. Loading setting joystickenabled: " << settings.value(_joystickEnabledSettingsKey, false).toBool(); - setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool()); - } else { - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Notified that there is no active joystick"; - setJoystickEnabled(false); - } -} - -// This is called from the UI when a deliberate action is taken to enable or disable the joystick -// This save allows the joystick enable state to persist restarts, disconnections of the joystick etc -void Vehicle::saveJoystickSettings() -{ - QSettings settings; - settings.beginGroup(QString(_settingsGroup).arg(_id)); - - // The joystick enabled setting should only be changed if a joystick is present - // since the checkbox can only be clicked if one is present - if (JoystickManager::instance()->joysticks().count()) { - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Saving setting joystickenabled: " << _joystickEnabled; - settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled); - } -} - -bool Vehicle::joystickEnabled() const -{ - return _joystickEnabled; -} - -void Vehicle::setJoystickEnabled(bool enabled) -{ - if (enabled){ - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Joystick Enabled"; - } - else { - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Joystick Disabled"; - } - - // _joystickEnabled is the runtime state - it determines whether a vehicle is using joystick data when it is active - _joystickEnabled = enabled; - - // if we are the active vehicle, call start polling on the active joystick - // This routes the joystick signals to this vehicle - if (enabled && MultiVehicleManager::instance()->activeVehicle() == this){ - _captureJoystick(); - } - - emit joystickEnabledChanged(_joystickEnabled); -} - void Vehicle::_activeVehicleChanged(Vehicle *newActiveVehicle) { - // the new active vehicle should always capture the joystick - // even if the new active vehicle has joystick disabled - // capturing the joystick will stop the joystick data going to the inactive vehicle - if (newActiveVehicle == this){ - qCDebug(JoystickLog) << "Vehicle " << this->id() << " is the new active vehicle"; + if (newActiveVehicle == this) { + qCDebug(VehicleLog) << "Vehicle" << this->id() << "is the new active vehicle"; _captureJoystick(); _isActiveVehicle = true; } else { @@ -1584,18 +1520,6 @@ void Vehicle::_activeVehicleChanged(Vehicle *newActiveVehicle) } } -// tells the active joystick where to send data -void Vehicle::_captureJoystick() -{ - Joystick* joystick = JoystickManager::instance()->activeJoystick(); - - if(joystick){ - qCDebug(JoystickLog) << "Vehicle " << this->id() << " Capture Joystick" << joystick->name(); - joystick->startPolling(this); - } -} - - QGeoCoordinate Vehicle::homePosition() { return _homePosition; @@ -1900,19 +1824,6 @@ void Vehicle::_remoteControlRSSIChanged(uint8_t rssi) } } -void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust) -{ - // The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled - if (!_joystickEnabled) { - sendJoystickDataThreadSafe( - static_cast(roll), - static_cast(pitch), - static_cast(yaw), - static_cast(thrust), - 0, 0, NAN, NAN); - } -} - void Vehicle::_say(const QString& text) { AudioOutput::instance()->say(text.toLower()); @@ -3853,57 +3764,6 @@ void Vehicle::clearAllParamMapRC(void) } } -void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, float thrust, quint16 buttons, quint16 buttons2, float gimbalPitch, float gimbalYaw) -{ - SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); - if (!sharedLink) { - qCDebug(VehicleLog)<< "sendJoystickDataThreadSafe: primary link gone!"; - return; - } - - if (sharedLink->linkConfiguration()->isHighLatency()) { - return; - } - - mavlink_message_t message; - - // Incoming values are in the range -1:1 - float axesScaling = 1.0 * 1000.0; - float newRollCommand = roll * axesScaling; - float newPitchCommand = pitch * axesScaling; // Joystick data is reverse of mavlink values - float newYawCommand = yaw * axesScaling; - float newThrustCommand = thrust * axesScaling; - float newGimbalPitch = gimbalPitch * axesScaling; - float newGimbalYaw = gimbalYaw * axesScaling; - uint8_t extensions = 0; - - if (std::isfinite(gimbalPitch)) { - extensions |= 1; - } - - if (std::isfinite(gimbalYaw)) { - extensions |= 2; - } - - mavlink_msg_manual_control_pack_chan( - static_cast(MAVLinkProtocol::instance()->getSystemId()), - static_cast(MAVLinkProtocol::getComponentId()), - sharedLink->mavlinkChannel(), - &message, - static_cast(_id), - static_cast(newPitchCommand), - static_cast(newRollCommand), - static_cast(newThrustCommand), - static_cast(newYawCommand), - buttons, buttons2, - extensions, - static_cast(newGimbalPitch), - static_cast(newGimbalYaw), - 0, 0, 0, 0, 0, 0 - ); - sendMessageOnLinkThreadSafe(sharedLink.get(), message); -} - void Vehicle::triggerSimpleCamera() { sendMavCommand(_defaultComponentId, @@ -4431,3 +4291,202 @@ const QVariantList &Vehicle::staticCameraList() const } /*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Joystick */ +/*===========================================================================*/ + +void Vehicle::_initJoystickHandling() +{ + (void) connect(JoystickManager::instance(), &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadJoystickSettings); + _loadJoystickSettings(); +} + +void Vehicle::setJoystickEnabled(bool enabled) +{ + if (enabled && (MultiVehicleManager::instance()->activeVehicle() == this)) { + _captureJoystick(); + } + + if (enabled != _joystickEnabled) { + _joystickEnabled = enabled; + emit joystickEnabledChanged(_joystickEnabled); + } +} + +void Vehicle::setManualControlButtonsEnabled(bool enabled) +{ + if (enabled != _manualControlButtonsEnabled) { + _manualControlButtonsEnabled = enabled; + emit manualControlButtonsEnabledChanged(_manualControlButtonsEnabled); + } +} + +void Vehicle::saveJoystickSettings() +{ + if (JoystickManager::instance()->joysticks().isEmpty()) { + return; + } + + QSettings settings; + settings.beginGroup(QString(_settingsGroup).arg(_id)); + settings.setValue(kSettingsKeyJoystickEnabled, _joystickEnabled); + settings.setValue(kSettingsKeyManualControlButtonsEnabled, _manualControlButtonsEnabled); +} + +void Vehicle::_loadJoystickSettings() +{ + if (!JoystickManager::instance()->activeJoystick()) { + setJoystickEnabled(false); + return; + } + + QSettings settings; + settings.beginGroup(QString(_settingsGroup).arg(_id)); + setJoystickEnabled(settings.value(kSettingsKeyJoystickEnabled, false).toBool()); + setManualControlButtonsEnabled(settings.value(kSettingsKeyManualControlButtonsEnabled, false).toBool()); +} + +void Vehicle::_captureJoystick() +{ + Joystick *joystick = JoystickManager::instance()->activeJoystick(); + if (joystick) { + joystick->startPolling(this); + } +} + +void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double throttle, double gimbalPitch, double gimbalYaw) +{ + if (!joystickEnabled()) { + const QList axisValues = { + static_cast(roll), + static_cast(pitch), + static_cast(yaw), + static_cast(throttle), + static_cast(gimbalPitch), + static_cast(gimbalYaw) + }; + const QBitArray buttonsPressed(32); + sendJoystickData(axisValues, buttonsPressed); + } +} + + + +void Vehicle::sendManualControlMsg(int16_t x, int16_t y, int16_t z, int16_t r, + int16_t s, int16_t t, + int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6, + QBitArray buttons) +{ + mavlink_manual_control_t manual_control{}; + manual_control.x = x; + manual_control.y = y; + manual_control.z = z; + manual_control.r = r; + + manual_control.s = s; + manual_control.t = t; + manual_control.aux1 = aux1; + manual_control.aux2 = aux2; + manual_control.aux3 = aux3; + manual_control.aux4 = aux4; + manual_control.aux5 = aux5; + manual_control.aux6 = aux6; + + bool ok; + const quint32 buttonPressed = buttons.toUInt32(QSysInfo::Endian::LittleEndian, &ok); + if (ok) { + manual_control.buttons = static_cast(buttonPressed); + manual_control.buttons2 = static_cast((buttonPressed >> 16) & 0xFFFF); + } + + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + qCWarning(VehicleLog) << "primary link gone!"; + return; + } + + if (sharedLink->linkConfiguration()->isHighLatency()) { + return; + } + + sendMessageOnLinkThreadSafe(sharedLink.get(), message); +} + +// void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, float thrust, float gimbalPitch, float gimbalYaw) + +void Vehicle::sendJoystickData(QList axisValues, QBitArray buttons) +{ + if (axisValues.size() > 12) { + qCWarning(VehicleLog) << "Too many axis values"; + } + + if (buttons.size() > 32) { + qCWarning(VehicleLog) << "Too many buttons"; + } + + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); + if (!sharedLink) { + qCWarning(VehicleLog) << "primary link gone!"; + return; + } + + if (sharedLink->linkConfiguration()->isHighLatency()) { + return; + } + + mavlink_manual_control_t manual_control{}; + + // Incoming values are in the range -1:1 + static constexpr float kAxisScaling = 1000.0; + + manual_control.x = (axisValues.size() > 1) ? (axisValues[1] * -kAxisScaling) : INT16_MAX; // Joystick data is reverse of mavlink values + manual_control.y = (axisValues.size() > 0) ? (axisValues[0] * kAxisScaling) : INT16_MAX; + manual_control.z = (axisValues.size() > 3) ? (axisValues[3] * kAxisScaling) : INT16_MAX; + manual_control.r = (axisValues.size() > 2) ? (axisValues[2] * kAxisScaling) : INT16_MAX; + + if (axisValues.size() > 4) { + manual_control.enabled_extensions |= (1u << 2); + } + + manual_control.s = (axisValues.size() > 4) ? (axisValues[4] * kAxisScaling) : 0; + manual_control.t = (axisValues.size() > 5) ? (axisValues[5] * kAxisScaling) : 0; + + if (!qIsNan(gimbalPitch)) { + newGimbalPitch = gimbalPitch * axisScaling; + + } + + if (!qIsNan(gimbalYaw)) { + newGimbalYaw = gimbalYaw * axisScaling; + manual_control.enabled_extensions |= (1u << 3); + } + + bool ok; + const quint32 buttonPressed = buttons.toUInt32(QSysInfo::Endian::LittleEndian, &ok); + if (ok) { + manual_control.buttons = static_cast(buttonPressed); + manual_control.buttons2 = static_cast((buttonPressed >> 16) & 0xFFFF); + } + + mavlink_message_t message{}; + mavlink_msg_manual_control_pack_chan( + static_cast(MAVLinkProtocol::instance()->getSystemId()), + static_cast(MAVLinkProtocol::getComponentId()), + sharedLink->mavlinkChannel(), + &message, + static_cast(_id), + static_cast(newPitchCommand), + static_cast(newRollCommand), + static_cast(newThrustCommand), + static_cast(newYawCommand), + lowButtons, highButtons, + extensions, + 0, 0, + static_cast(newGimbalPitch), + static_cast(newGimbalYaw), + 0, 0, 0, 0 + ); + sendMessageOnLinkThreadSafe(sharedLink.get(), message); +} + +/*---------------------------------------------------------------------------*/ diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ef46799c727f..80f160f46df2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -59,7 +59,6 @@ class GeoFenceManager; class ImageProtocolManager; class StatusTextHandler; class InitialConnectStateMachine; -class Joystick; class LinkInterface; class MAVLinkLogManager; class MissionManager; @@ -150,7 +149,6 @@ class Vehicle : public VehicleFactGroup Q_PROPERTY(QmlObjectListModel* cameraTriggerPoints READ cameraTriggerPoints CONSTANT) Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged) Q_PROPERTY(float longitude READ longitude NOTIFY coordinateChanged) - Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged) Q_PROPERTY(bool px4Firmware READ px4Firmware NOTIFY firmwareTypeChanged) Q_PROPERTY(bool apmFirmware READ apmFirmware NOTIFY firmwareTypeChanged) @@ -291,8 +289,6 @@ class Vehicle : public VehicleFactGroup /// Resets link status counters Q_INVOKABLE void resetCounters (); - Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust); - /// Command vehicle to return to launch Q_INVOKABLE void guidedModeRTL(bool smartRTL); @@ -411,9 +407,6 @@ class Vehicle : public VehicleFactGroup /// Set home from flight map coordinate Q_INVOKABLE void doSetHome(const QGeoCoordinate& coord); - /// Save the joystick enable setting to the settings group - Q_INVOKABLE void saveJoystickSettings(void); - Q_INVOKABLE void sendSetupSigning(); Q_INVOKABLE QVariant expandedToolbarIndicatorSource(const QString& indicatorName); @@ -443,10 +436,6 @@ class Vehicle : public VehicleFactGroup void updateFlightDistance(double distance); - bool joystickEnabled () const; - void setJoystickEnabled (bool enabled); - void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons, quint16 buttons2, float gimbalPitch, float gimbalYaw); - // Property accesors int id() const{ return _id; } int compId() const{ return _compID; } @@ -820,7 +809,6 @@ public slots: signals: void coordinateChanged (QGeoCoordinate coordinate); - void joystickEnabledChanged (bool enabled); void mavlinkMessageReceived (const mavlink_message_t& message); void homePositionChanged (const QGeoCoordinate& homePosition); void armedPositionChanged(); @@ -943,9 +931,7 @@ private slots: void _altitudeAboveTerrainReceived (bool sucess, QList heights); private: - void _loadJoystickSettings (); void _activeVehicleChanged (Vehicle* newActiveVehicle); - void _captureJoystick (); void _handlePing (LinkInterface* link, mavlink_message_t& message); void _handleHomePosition (mavlink_message_t& message); void _handleHeartbeat (mavlink_message_t& message); @@ -1012,7 +998,6 @@ private slots: QTimer _csvLogTimer; QFile _csvLogFile; - bool _joystickEnabled = false; bool _isActiveVehicle = false; QGeoCoordinate _coordinate; @@ -1109,7 +1094,6 @@ private slots: bool _heardFrom = false; bool _isROIEnabled = false; - Joystick* _activeJoystick = nullptr; bool _checkLatestStableFWDone = false; int _firmwareMajorVersion = versionNotSetValue; @@ -1227,7 +1211,6 @@ private slots: // FactGroup facts const QString _settingsGroup = QStringLiteral("Vehicle%1"); // %1 replaced with mavlink system id - const QString _joystickEnabledSettingsKey = QStringLiteral("JoystickEnabled"); const QString _vehicleFactGroupName = QStringLiteral("vehicle"); const QString _gpsFactGroupName = QStringLiteral("gps"); @@ -1472,6 +1455,44 @@ private slots: QGCCameraManager *_cameraManager = nullptr; +/*---------------------------------------------------------------------------*/ +/*===========================================================================*/ +/* Joystick */ +/*===========================================================================*/ +private: + Q_PROPERTY(bool joystickEnabled READ joystickEnabled WRITE setJoystickEnabled NOTIFY joystickEnabledChanged) + Q_PROPERTY(bool manualControlButtonsEnabled READ manualControlButtonsEnabled WRITE setManualControlButtonsEnabled NOTIFY manualControlButtonsEnabledChanged) + +public: + Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double throttle, double gimbalPitch = NAN, double gimbalYaw = NAN); + + /// Save the joystick enable setting to the settings group + Q_INVOKABLE void saveJoystickSettings(); + + bool joystickEnabled() const { return _joystickEnabled; }; + void setJoystickEnabled(bool enabled); + + bool manualControlButtonsEnabled() const { return _manualControlButtonsEnabled; } + void setManualControlButtonsEnabled(bool enabled); + +signals: + void joystickEnabledChanged(bool enabled); + void manualControlButtonsEnabledChanged(bool enabled); + +public slots: + void sendJoystickData(QList axisValues, QBitArray buttons); + +private: + void _initJoystickHandling(); + void _captureJoystick(); + void _loadJoystickSettings(); + + bool _joystickEnabled = false; + bool _manualControlButtonsEnabled = false; + + static constexpr const char *kSettingsKeyJoystickEnabled = "JoystickEnabled"; + static constexpr const char *kSettingsKeyManualControlButtonsEnabled = "ManualControlButtonsEnabled"; + /*---------------------------------------------------------------------------*/ }; Q_DECLARE_METATYPE(Vehicle::MavCmdResultFailureCode_t) diff --git a/src/Vehicle/VehicleSetup/JoystickConfigGeneral.qml b/src/Vehicle/VehicleSetup/JoystickConfigGeneral.qml index 14b9f2f84a45..edfa323233f6 100644 --- a/src/Vehicle/VehicleSetup/JoystickConfigGeneral.qml +++ b/src/Vehicle/VehicleSetup/JoystickConfigGeneral.qml @@ -13,11 +13,7 @@ import QtQuick.Dialogs import QtQuick.Layouts import QGroundControl - import QGroundControl.Controls - - - import QGroundControl.FactControls Item { @@ -26,8 +22,9 @@ Item { readonly property real axisMonitorWidth: ScreenTools.defaultFontPixelWidth * 32 - property bool _buttonsOnly: _activeJoystick.axisCount == 0 + property bool _buttonsOnly: _activeJoystick.axisCount == 0 property bool _requiresCalibration: !_activeJoystick.calibrated && !_buttonsOnly + property var _activeVehicle: globals.activeVehicle Column { id: mainCol @@ -48,23 +45,23 @@ Item { id: enabledSwitch enabled: !_requiresCalibration onClicked: { - globals.activeVehicle.joystickEnabled = checked - globals.activeVehicle.saveJoystickSettings() + _activeVehicle.joystickEnabled = checked + _activeVehicle.saveJoystickSettings() } Component.onCompleted: { - checked = globals.activeVehicle.joystickEnabled + checked = _activeVehicle.joystickEnabled } Connections { - target: globals.activeVehicle + target: _activeVehicle onJoystickEnabledChanged: { - enabledSwitch.checked = globals.activeVehicle.joystickEnabled + enabledSwitch.checked = _activeVehicle.joystickEnabled } } Connections { target: joystickManager onActiveJoystickChanged: { if(_activeJoystick) { - enabledSwitch.checked = Qt.binding(function() { return _activeJoystick.calibrated && globals.activeVehicle.joystickEnabled }) + enabledSwitch.checked = Qt.binding(function() { return _activeJoystick.calibrated && _activeVehicle.joystickEnabled }) } } } @@ -158,8 +155,9 @@ Item { columnSpacing: ScreenTools.defaultFontPixelWidth rowSpacing: ScreenTools.defaultFontPixelHeight anchors.centerIn: parent + QGCLabel { - text: globals.activeVehicle.sub ? qsTr("Lateral") : qsTr("Roll") + text: _activeVehicle.sub ? qsTr("Lateral") : qsTr("Roll") Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 12 } AxisMonitor { @@ -173,7 +171,7 @@ Item { QGCLabel { id: pitchLabel width: _attitudeLabelWidth - text: globals.activeVehicle.sub ? qsTr("Forward") : qsTr("Pitch") + text: _activeVehicle.sub ? qsTr("Forward") : qsTr("Pitch") } AxisMonitor { id: pitchAxis @@ -209,17 +207,50 @@ Item { reversed: controller.throttleAxisReversed } + QGCLabel { + id: gimbalPitchLabel + enabled: gimbalPitchAxis.enabled + width: _attitudeLabelWidth + text: qsTr("Gimbal Pitch") + } + AxisMonitor { + id: gimbalPitchAxis + enabled: _activeJoystick.axisCount >= 5 + height: ScreenTools.defaultFontPixelHeight + width: axisMonitorWidth + mapped: controller.gimbalPitchAxisMapped + reversed: controller.gimbalPitchAxisReversed + } + + QGCLabel { + id: gimbalYawLabel + enabled: gimbalYawAxis.enabled + width: _attitudeLabelWidth + text: qsTr("Gimbal Yaw") + } + AxisMonitor { + id: gimbalYawAxis + enabled: _activeJoystick.axisCount >= 6 + height: ScreenTools.defaultFontPixelHeight + width: axisMonitorWidth + mapped: controller.gimbalYawAxisMapped + reversed: controller.gimbalYawAxisReversed + } + Connections { - target: _activeJoystick - onAxisValues: (roll, pitch, yaw, throttle) => { - rollAxis.axisValue = roll * 32768.0 - pitchAxis.axisValue = pitch * 32768.0 - yawAxis.axisValue = yaw * 32768.0 - throttleAxis.axisValue = _activeJoystick.negativeThrust ? throttle * -32768.0 : (-2 * throttle + 1) * 32768.0 + target: _activeJoystick + onAxisValuesUpdated: (roll, pitch, yaw, throttle, gimbalPitch, gimbalYaw) => { + rollAxis.axisValue = roll * 32768.0 + pitchAxis.axisValue = pitch * 32768.0 + yawAxis.axisValue = yaw * 32768.0 + throttleAxis.axisValue = _activeJoystick.negativeThrust ? (throttle * -32768.0) : ((-2 * throttle + 1) * 32768.0) + gimbalPitchAxis.axisValue = gimbalPitch * 32768.0 + gimbalYawAxis.axisValue = gimbalYaw * 32768.0 } } } } + Rectangle { color: Qt.rgba(0,0,0,0) border.color: qgcPal.text