diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 16a34d5d8adb1ae6965a95c8b0a99224355fee1e..0aacd6ac08abc5b75f924dc7f2c6e9e34e75c3dc 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -23,7 +23,7 @@ QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") const char* Joystick::_settingsGroup = "Joysticks"; -const char* Joystick::_calibratedSettingsKey = "Calibrated2"; // Increment number to force recalibration +const char* Joystick::_calibratedSettingsKey = "Calibrated3"; // Increment number to force recalibration const char* Joystick::_buttonActionSettingsKey = "ButtonActionName%1"; const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_exponentialSettingsKey = "Exponential"; @@ -37,6 +37,7 @@ const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL"; const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; +const char* Joystick::_gimbalSettingsKey = "GimbalEnabled"; const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); @@ -81,13 +82,13 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC { _rgAxisValues = new int[_axisCount]; _rgCalibration = new Calibration_t[_axisCount]; - _rgButtonValues = new bool[_totalButtonCount]; + _rgButtonValues = new uint8_t[_totalButtonCount]; for (int i = 0; i < _axisCount; i++) { _rgAxisValues[i] = 0; } for (int i = 0; i < _totalButtonCount; i++) { - _rgButtonValues[i] = false; + _rgButtonValues[i] = BUTTON_UP; } _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); @@ -169,12 +170,10 @@ void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) { _updateTXModeSettingsKey(activeVehicle); - if(activeVehicle) { QSettings settings; settings.beginGroup(_settingsGroup); int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); - setTXMode(mode); } } @@ -201,6 +200,7 @@ void Joystick::_loadSettings() _deadband = settings.value(_deadbandSettingsKey, false).toBool(); _frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat(); _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); + _gimbalEnabled = settings.value(_gimbalSettingsKey, false).toBool(); _throttleMode = static_cast(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); badSettings |= !convertOk; @@ -277,6 +277,7 @@ void Joystick::_saveSettings() settings.setValue(_deadbandSettingsKey, _deadband); settings.setValue(_frequencySettingsKey, _frequency); settings.setValue(_throttleModeSettingsKey, _throttleMode); + settings.setValue(_gimbalSettingsKey, _gimbalEnabled); settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; @@ -410,7 +411,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDead } -void Joystick::run(void) +void Joystick::run() { _open(); @@ -428,8 +429,11 @@ void Joystick::run(void) // Update buttons for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { bool newButtonValue = _getButton(buttonIndex); - if (newButtonValue != _rgButtonValues[buttonIndex]) { - _rgButtonValues[buttonIndex] = newButtonValue; + if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_DOWN; + emit rawButtonPressedChanged(buttonIndex, newButtonValue); + } else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) { + _rgButtonValues[buttonIndex] = BUTTON_UP; emit rawButtonPressedChanged(buttonIndex, newButtonValue); } } @@ -441,9 +445,12 @@ void Joystick::run(void) // Create new index value that includes the normal button list int rgButtonValueIndex = hatIndex*numHatButtons + hatButtonIndex + _buttonCount; // Get hat value from joystick - bool newButtonValue = _getHat(hatIndex,hatButtonIndex); - if (newButtonValue != _rgButtonValues[rgButtonValueIndex]) { - _rgButtonValues[rgButtonValueIndex] = newButtonValue; + bool newButtonValue = _getHat(hatIndex, hatButtonIndex); + if (newButtonValue && _rgButtonValues[rgButtonValueIndex] == BUTTON_UP) { + _rgButtonValues[rgButtonValueIndex] = BUTTON_DOWN; + emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue); + } else if (!newButtonValue && _rgButtonValues[rgButtonValueIndex] != BUTTON_UP) { + _rgButtonValues[rgButtonValueIndex] = BUTTON_UP; emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue); } } @@ -451,31 +458,38 @@ void Joystick::run(void) if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { int axis = _rgFunctionAxis[rollFunction]; - float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); axis = _rgFunctionAxis[pitchFunction]; - float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); + 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); - axis = _rgFunctionAxis[gimbalPitchFunction]; - float gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + float gimbalPitch = 0.0f; + float gimbalYaw = 0.0f; - axis = _rgFunctionAxis[gimbalYawFunction]; - float gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + if(_axisCount > 4) { + axis = _rgFunctionAxis[gimbalPitchFunction]; + gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } + + if(_axisCount > 5) { + axis = _rgFunctionAxis[gimbalYawFunction]; + gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); + } - if ( _accumulator ) { + 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 += 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 ) { + 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))); @@ -490,8 +504,7 @@ void Joystick::run(void) if ( _exponential < -0.01f) { // Exponential (0% to -50% range like most RC radios) - //_exponential is set by a slider in joystickConfigAdvanced.qml - + // _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; @@ -507,36 +520,40 @@ void Joystick::run(void) throttle = (throttle + 1.0f) / 2.0f; } - // Set up button pressed information - - quint16 newButtonBits = 0; // New set of button which are down - quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal - + //-- Process button press for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { - quint16 buttonBit = static_cast(1 << buttonIndex); - - if (!_rgButtonValues[buttonIndex]) { - // Button up, just record it - newButtonBits |= buttonBit; - } else { - if (_lastButtonBits & buttonBit) { - // Button was up last time through, but is now down which indicates a button press - qCDebug(JoystickLog) << "button triggered" << buttonIndex; - QString buttonAction =_rgButtonActions[buttonIndex]; - if (!buttonAction.isEmpty()) { - _buttonAction(buttonAction); - } + //-- This button just went down + if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) { + //-- Flag it as processed + _rgButtonValues[buttonIndex] = BUTTON_REPEAT; + //-- Process button + QString buttonAction =_rgButtonActions[buttonIndex]; + qCDebug(JoystickLog) << "button triggered" << buttonIndex << buttonAction; + if (!buttonAction.isEmpty()) { + _buttonAction(buttonAction); } + } + } + // Set up button bitmap + quint64 buttonPressedBits = 0; // Buttons pressed for manualControl signal + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { + quint64 buttonBit = static_cast(1 << buttonIndex); + if (_rgButtonValues[buttonIndex] != BUTTON_UP) { // Mark the button as pressed as long as its pressed buttonPressedBits |= buttonBit; } } - _lastButtonBits = newButtonBits; 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. - emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); + // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits) + uint16_t shortButtons = static_cast(buttonPressedBits & 0xFFFF); + emit manualControl(roll, -pitch, yaw, throttle, shortButtons, _activeVehicle->joystickMode()); + if(_activeVehicle && _axisCount > 4 && _gimbalEnabled) { + //-- TODO: There is nothing consuming this as there are no messages to handle gimbal + // the way MANUAL_CONTROL handles the other channels. + emit manualControlGimbal((gimbalPitch + 1.0f) / 2.0f * 90.0f, gimbalYaw * 180.0f); + } } // Sleep. Update rate of joystick is by default 25 Hz @@ -550,36 +567,40 @@ void Joystick::run(void) void Joystick::startPolling(Vehicle* vehicle) { if (vehicle) { - // If a vehicle is connected, disconnect it if (_activeVehicle) { UAS* uas = _activeVehicle->uas(); disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); } - // Always set up the new vehicle _activeVehicle = vehicle; - // If joystick is not calibrated, disable it if ( !_calibrated ) { vehicle->setJoystickEnabled(false); } - // Update qml in case of joystick transition emit calibratedChanged(_calibrated); - // Only connect the new vehicle if it wants joystick data if (vehicle->joystickEnabled()) { _pollingStartedForCalibration = false; - UAS* uas = _activeVehicle->uas(); connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); + connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); + connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + connect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + connect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + connect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); // FIXME: **** //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); } } - - if (!isRunning()) { _exitThread = false; start(); @@ -589,16 +610,20 @@ void Joystick::startPolling(Vehicle* vehicle) void Joystick::stopPolling(void) { if (isRunning()) { - if (_activeVehicle && _activeVehicle->joystickEnabled()) { UAS* uas = _activeVehicle->uas(); // Neutral attitude controls // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode()); - disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); + disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); + disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed); + disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); + disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); + disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); } // FIXME: **** //disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); - _exitThread = true; } } @@ -609,7 +634,6 @@ void Joystick::setCalibration(int axis, Calibration_t& calibration) qCWarning(JoystickLog) << "Invalid axis index" << axis; return; } - _calibrated = true; _rgCalibration[axis] = calibration; _saveSettings(); @@ -631,7 +655,6 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis) qCWarning(JoystickLog) << "Invalid axis index" << axis; return; } - _calibrated = true; _rgFunctionAxis[function] = axis; _saveSettings(); @@ -646,7 +669,7 @@ int Joystick::getFunctionAxis(AxisFunction_t function) return _rgFunctionAxis[function]; } -QStringList Joystick::actions(void) +QStringList Joystick::actions() { QStringList list; list << _buttonActionArm << _buttonActionDisarm << _buttonActionToggleArm; @@ -692,7 +715,7 @@ QString Joystick::getButtonAction(int button) return _rgButtonActions[button]; } -QVariantList Joystick::buttonActions(void) +QVariantList Joystick::buttonActions() { QVariantList list; for (int button=0; button<_totalButtonCount; button++) { @@ -787,6 +810,13 @@ float Joystick::frequency() return _frequency; } +void Joystick::setGimbalEnabled(bool set) +{ + _gimbalEnabled = set; + _saveSettings(); + emit gimbalEnabledChanged(); +} + void Joystick::setFrequency(float val) { //-- Arbitrary limits @@ -816,17 +846,17 @@ void Joystick::_buttonAction(const QString& action) return; } if (action == _buttonActionArm) { - _activeVehicle->setArmed(true); + emit setArmed(true); } else if (action == _buttonActionDisarm) { - _activeVehicle->setArmed(false); + emit setArmed(false); } else if (action == _buttonActionToggleArm) { - _activeVehicle->setArmed(!_activeVehicle->armed()); + emit setArmed(!_activeVehicle->armed()); } else if (action == _buttonActionVTOLFixedWing) { - _activeVehicle->setVtolInFwdFlight(true); + emit setVtolInFwdFlight(true); } else if (action == _buttonActionVTOLMultiRotor) { - _activeVehicle->setVtolInFwdFlight(false); + emit setVtolInFwdFlight(false); } else if (_activeVehicle->flightModes().contains(action)) { - _activeVehicle->setFlightMode(action); + emit setFlightMode(action); } else if(action == _buttonActionZoomIn || action == _buttonActionZoomOut) { emit stepZoom(action == _buttonActionZoomIn ? 1 : -1); } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { @@ -842,15 +872,15 @@ void Joystick::_buttonAction(const QString& action) } else if(action == _buttonActionToggleVideoRecord) { emit toggleVideoRecord(); } else if(action == _buttonActionGimbalUp) { - _activeVehicle->gimbalPitchStep(1); + emit gimbalPitchStep(1); } else if(action == _buttonActionGimbalDown) { - _activeVehicle->gimbalPitchStep(-1); + emit gimbalPitchStep(-1); } else if(action == _buttonActionGimbalLeft) { - _activeVehicle->gimbalYawStep(-1); + emit gimbalYawStep(-1); } else if(action == _buttonActionGimbalRight) { - _activeVehicle->gimbalYawStep(1); + emit gimbalYawStep(1); } else if(action == _buttonActionGimbalCenter) { - _activeVehicle->centerGimbal(); + emit centerGimbal(); } else { qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index f0b36a3df6cad3436eb9dc8580f09b7a2edd10aa..be35c87f433bf98e65204d4ee9c697bff4bec461 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -60,31 +60,35 @@ public: 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(QStringList actions READ actions CONSTANT) - - Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged) - Q_INVOKABLE void setButtonAction(int button, const QString& action); - Q_INVOKABLE QString getButtonAction(int button); - - Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) - 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 requiresCalibration READ requiresCalibration CONSTANT) - Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged) - Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged) + 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) + Q_PROPERTY(QStringList actions READ actions CONSTANT) + Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged) + + Q_PROPERTY(bool gimbalEnabled READ gimbalEnabled WRITE setGimbalEnabled NOTIFY gimbalEnabledChanged) + Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) + Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged) + 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 setButtonAction (int button, const QString& action); + Q_INVOKABLE QString getButtonAction (int button); // Property accessors - int axisCount(void) { return _axisCount; } - int totalButtonCount(void) { return _totalButtonCount; } + QString name () { return _name; } + int totalButtonCount () { return _totalButtonCount; } + int axisCount () { return _axisCount; } + bool gimbalEnabled () { return _gimbalEnabled; } + QStringList actions (); + QVariantList buttonActions (); + + void setGimbalEnabled (bool set); /// Start the polling thread which will in turn emit joystick signals void startPolling(Vehicle* vehicle); @@ -96,10 +100,7 @@ public: void setFunctionAxis(AxisFunction_t function, int axis); int getFunctionAxis(AxisFunction_t function); - QStringList actions(void); - QVariantList buttonActions(void); - QString name(void) { return _name; } /* // Joystick index used by sdl library // Settable because sdl library remaps indices after certain events @@ -162,18 +163,26 @@ signals: /// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle /// @param mode See Vehicle::JoystickMode_t enum - void manualControl (float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode); + void manualControl (float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode); + void manualControlGimbal (float gimbalPitch, float gimbalYaw); void buttonActionTriggered(int action); - void frequencyChanged (); - void stepZoom (int direction); - void stepCamera (int direction); - void stepStream (int direction); - void triggerCamera (); - void startVideoRecord (); - void stopVideoRecord (); - void toggleVideoRecord (); + void gimbalEnabledChanged (); + void frequencyChanged (); + 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 setArmed (bool arm); + void setVtolInFwdFlight (bool set); + void setFlightMode (const QString& flightMode); protected: void _setDefaultCalibration (); @@ -185,29 +194,35 @@ protected: bool _validButton (int button); private: - virtual bool _open() = 0; - virtual void _close() = 0; - virtual bool _update() = 0; + 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 uint8_t _getHat(int hat,int i) = 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(void); + virtual void run(); protected: + enum { + BUTTON_UP, + BUTTON_DOWN, + BUTTON_REPEAT + }; + + uint8_t*_rgButtonValues = nullptr; + bool _exitThread = false; ///< true: signal thread to exit bool _calibrationMode = false; int* _rgAxisValues = nullptr; Calibration_t* _rgCalibration = nullptr; - bool* _rgButtonValues = nullptr; - quint16 _lastButtonBits = 0; ThrottleMode_t _throttleMode = ThrottleModeDownZero; bool _negativeThrust = false; float _exponential = 0; @@ -216,7 +231,7 @@ protected: bool _circleCorrection = true; float _frequency = 25.0f; Vehicle* _activeVehicle = nullptr; - + bool _gimbalEnabled = false; bool _pollingStartedForCalibration = false; @@ -234,7 +249,7 @@ protected: QStringList _rgButtonActions; - MultiVehicleManager* _multiVehicleManager; + MultiVehicleManager* _multiVehicleManager = nullptr; private: static const char* _rgFunctionSettingsKey[maxFunction]; @@ -254,6 +269,7 @@ private: static const char* _roverTXModeSettingsKey; static const char* _vtolTXModeSettingsKey; static const char* _submarineTXModeSettingsKey; + static const char* _gimbalSettingsKey; static const char* _buttonActionArm; static const char* _buttonActionDisarm; diff --git a/src/Joystick/JoystickAndroid.cc b/src/Joystick/JoystickAndroid.cc index 7f77b7983c5f53bf2d846115690ed7da24dda35e..3994864e0b3bce07898acf7b0ef0444f277022b5 100644 --- a/src/Joystick/JoystickAndroid.cc +++ b/src/Joystick/JoystickAndroid.cc @@ -200,11 +200,10 @@ int JoystickAndroid::_getAxis(int i) { return axisValue[ i ]; } -uint8_t JoystickAndroid::_getHat(int hat,int i) { +bool JoystickAndroid::_getHat(int hat,int i) { Q_UNUSED(hat); Q_UNUSED(i); - - return 0; + return false; } static JoystickManager *_manager = nullptr; diff --git a/src/Joystick/JoystickAndroid.h b/src/Joystick/JoystickAndroid.h index 1a0afbdf12ac9b4f4b6e32b3b15f757d998803e5..2ff473c15a12dc9597c20d0914bafa51ecca8b23 100644 --- a/src/Joystick/JoystickAndroid.h +++ b/src/Joystick/JoystickAndroid.h @@ -29,13 +29,13 @@ private: bool handleKeyEvent(jobject event); bool handleGenericMotionEvent(jobject event); - virtual bool _open(); - virtual void _close(); - virtual bool _update(); + virtual bool _open (); + virtual void _close (); + virtual bool _update (); - virtual bool _getButton(int i); - virtual int _getAxis(int i); - virtual uint8_t _getHat(int hat,int i); + virtual bool _getButton (int i); + virtual int _getAxis (int i); + virtual bool _getHat (int hat,int i); int *btnCode; int *axisCode; diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index d61238be5924a103c9c27ce303e773f677941442..4f1876d7f978fb7f4fa18986125eb516e05b27dc 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -19,7 +19,6 @@ bool JoystickSDL::init(void) { qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError(); return false; } - _loadGameControllerMappings(); return true; } @@ -78,7 +77,7 @@ QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicl newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager); } else { newRet[name] = ret[name]; - JoystickSDL *j = (JoystickSDL*)newRet[name]; + JoystickSDL *j = static_cast(newRet[name]); if (j->index() != i) { j->setIndex(i); // This joystick index has been remapped by SDL } @@ -131,7 +130,7 @@ bool JoystickSDL::_open(void) { } void JoystickSDL::_close(void) { - if (sdlJoystick == NULL) { + if (sdlJoystick == nullptr) { qCDebug(JoystickLog) << "Attempt to close null joystick!"; return; } @@ -152,8 +151,8 @@ void JoystickSDL::_close(void) { } } - sdlJoystick = NULL; - sdlController = NULL; + sdlJoystick = nullptr; + sdlController = nullptr; } bool JoystickSDL::_update(void) @@ -164,26 +163,25 @@ bool JoystickSDL::_update(void) } bool JoystickSDL::_getButton(int i) { - if ( _isGameController ) { - return !!SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)); + if (_isGameController) { + return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1; } else { - return !!SDL_JoystickGetButton(sdlJoystick, i); + return SDL_JoystickGetButton(sdlJoystick, i) == 1; } } int JoystickSDL::_getAxis(int i) { - if ( _isGameController ) { + if (_isGameController) { return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i)); } else { return SDL_JoystickGetAxis(sdlJoystick, i); } } -uint8_t JoystickSDL::_getHat(int hat,int i) { +bool JoystickSDL::_getHat(int hat, int i) { 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]); + if (i < int(sizeof(hatButtons))) { + return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0; } - return 0; + return false; } diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h index 905b01da4c4cb3de7375cfcb22c707b44254b684..df565525e4410d3f1164969f593dc544f1d0c6c6 100644 --- a/src/Joystick/JoystickSDL.h +++ b/src/Joystick/JoystickSDL.h @@ -24,16 +24,17 @@ public: private: static void _loadGameControllerMappings(); - bool _open() final; - void _close() final; - bool _update() final; + bool _open () final; + void _close () final; + bool _update () final; - bool _getButton(int i) final; - int _getAxis(int i) final; - uint8_t _getHat(int hat,int i) final; + bool _getButton (int i) final; + int _getAxis (int i) final; + bool _getHat (int hat,int i) final; + + SDL_Joystick* sdlJoystick; + SDL_GameController* sdlController; - SDL_Joystick *sdlJoystick; - SDL_GameController *sdlController; bool _isGameController; int _index; ///< Index for SDL_JoystickOpen diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index b1aac0981fdc18fb941dba0355991325dcc6276f..d0d9c76105e6cbfa377ac9859e781618135f7120 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3064,26 +3064,29 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle.")); return; } - if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { - sendMavCommandInt(defaultComponentId(), - MAV_CMD_DO_ORBIT, - MAV_FRAME_GLOBAL, - true, // show error if fails - radius, - qQNaN(), // Use default velocity - 0, // Vehicle points to center - qQNaN(), // reserved - centerCoord.latitude(), centerCoord.longitude(), amslAltitude); + sendMavCommandInt( + defaultComponentId(), + MAV_CMD_DO_ORBIT, + MAV_FRAME_GLOBAL, + true, // show error if fails + static_cast(radius), + static_cast(qQNaN()), // Use default velocity + 0, // Vehicle points to center + static_cast(qQNaN()), // reserved + centerCoord.latitude(), centerCoord.longitude(), static_cast(amslAltitude)); } else { - sendMavCommand(defaultComponentId(), - MAV_CMD_DO_ORBIT, - true, // show error if fails - radius, - qQNaN(), // Use default velocity - 0, // Vehicle points to center - qQNaN(), // reserved - centerCoord.latitude(), centerCoord.longitude(), amslAltitude); + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_ORBIT, + true, // show error if fails + static_cast(radius), + static_cast(qQNaN()), // Use default velocity + 0, // Vehicle points to center + static_cast(qQNaN()), // reserved + static_cast(centerCoord.latitude()), + static_cast(centerCoord.longitude()), + static_cast(amslAltitude)); } } @@ -3098,10 +3101,11 @@ void Vehicle::pauseVehicle(void) void Vehicle::abortLanding(double climbOutAltitude) { - sendMavCommand(defaultComponentId(), - MAV_CMD_DO_GO_AROUND, - true, // show error if fails - climbOutAltitude); + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_GO_AROUND, + true, // show error if fails + static_cast(climbOutAltitude)); } bool Vehicle::guidedMode(void) const @@ -3116,11 +3120,12 @@ void Vehicle::setGuidedMode(bool guidedMode) void Vehicle::emergencyStop(void) { - sendMavCommand(_defaultComponentId, - MAV_CMD_COMPONENT_ARM_DISARM, - true, // show error if fails - 0.0f, - 21196.0f); // Magic number for emergency stop + sendMavCommand( + _defaultComponentId, + MAV_CMD_COMPONENT_ARM_DISARM, + true, // show error if fails + 0.0f, + 21196.0f); // Magic number for emergency stop } void Vehicle::setCurrentMissionSequence(int seq) @@ -3129,13 +3134,14 @@ void Vehicle::setCurrentMissionSequence(int seq) seq--; } mavlink_message_t msg; - mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(), - _mavlink->getComponentId(), - priorityLink()->mavlinkChannel(), - &msg, - id(), - _compID, - seq); + mavlink_msg_mission_set_current_pack_chan( + static_cast(_mavlink->getSystemId()), + static_cast(_mavlink->getComponentId()), + priorityLink()->mavlinkChannel(), + &msg, + static_cast(id()), + _compID, + static_cast(seq)); sendMessageOnLink(priorityLink(), msg); } @@ -3147,13 +3153,13 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo entry.component = component; entry.command = command; entry.showError = showError; - entry.rgParam[0] = param1; - entry.rgParam[1] = param2; - entry.rgParam[2] = param3; - entry.rgParam[3] = param4; - entry.rgParam[4] = param5; - entry.rgParam[5] = param6; - entry.rgParam[6] = param7; + entry.rgParam[0] = static_cast(param1); + entry.rgParam[1] = static_cast(param2); + entry.rgParam[2] = static_cast(param3); + entry.rgParam[3] = static_cast(param4); + entry.rgParam[4] = static_cast(param5); + entry.rgParam[5] = static_cast(param6); + entry.rgParam[6] = static_cast(param7); _mavCommandQueue.append(entry); @@ -3172,13 +3178,13 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, entry.command = command; entry.frame = frame; entry.showError = showError; - entry.rgParam[0] = param1; - entry.rgParam[1] = param2; - entry.rgParam[2] = param3; - entry.rgParam[3] = param4; + entry.rgParam[0] = static_cast(param1); + entry.rgParam[1] = static_cast(param2); + entry.rgParam[2] = static_cast(param3); + entry.rgParam[3] = static_cast(param4); entry.rgParam[4] = param5; entry.rgParam[5] = param6; - entry.rgParam[6] = param7; + entry.rgParam[6] = static_cast(param7); _mavCommandQueue.append(entry); @@ -3367,8 +3373,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */); if (showError) { - QString commandName = _toolbox->missionCommandTree()->friendlyName((MAV_CMD)ack.command); - + QString commandName = _toolbox->missionCommandTree()->friendlyName(static_cast(ack.command)); switch (ack.result) { case MAV_RESULT_TEMPORARILY_REJECTED: qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName)); @@ -4016,6 +4021,7 @@ void Vehicle::gimbalPitchStep(int direction) void Vehicle::gimbalYawStep(int direction) { if(!_haveGimbalData) { + qDebug() << "Yaw:" << _curGinmbalYaw << direction << (_curGinmbalYaw + direction); double y = static_cast(_curGinmbalYaw + direction); gimbalControlValue(static_cast(_curGinmbalYaw), y); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index d1044254128c850f2f17fec9d4ba59787e73c465..231e82f4acdde729470f9584f3bd8dc4e450305a 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -840,8 +840,8 @@ public: QGeoCoordinate homePosition(void); - bool armed(void) { return _armed; } - void setArmed(bool armed); + bool armed () { return _armed; } + void setArmed (bool armed); bool flightModeSetAvailable(void); QStringList flightModes(void); @@ -1084,7 +1084,6 @@ public: void _setFlying(bool flying); void _setLanding(bool landing); - void setVtolInFwdFlight(bool vtolInFwdFlight); void _setHomePosition(QGeoCoordinate& homeCoord); void _setMaxProtoVersion (unsigned version); @@ -1101,6 +1100,9 @@ public: qreal gimbalYaw () { return static_cast(_curGinmbalYaw); } bool gimbalData () { return _haveGimbalData; } +public slots: + void setVtolInFwdFlight (bool vtolInFwdFlight); + signals: void allLinksInactive(Vehicle* vehicle); void coordinateChanged(QGeoCoordinate coordinate); diff --git a/src/VehicleSetup/JoystickConfigAdvanced.qml b/src/VehicleSetup/JoystickConfigAdvanced.qml index 1e4a4e869c6291c22abf8b4156cf82b38d793add..7f3279e79e926cae23877159cd91204444f6c1b0 100644 --- a/src/VehicleSetup/JoystickConfigAdvanced.qml +++ b/src/VehicleSetup/JoystickConfigAdvanced.qml @@ -103,6 +103,30 @@ Item { } } } + //--------------------------------------------------------------------- + //-- Enable Gimbal + QGCLabel { + text: qsTr("Enable gimbal control (Experimental)") + visible: advancedSettings.checked + Layout.alignment: Qt.AlignVCenter + } + QGCCheckBox { + id: enabledGimbal + visible: advancedSettings.checked + enabled: _activeJoystick + onClicked: _activeJoystick.gimbalEnabled = checked + Component.onCompleted: { + checked = _activeJoystick.gimbalEnabled + } + Connections { + target: joystickManager + onActiveJoystickChanged: { + if(_activeJoystick) { + enabledGimbal.checked = Qt.binding(function() { return _activeJoystick.gimbalEnabled }) + } + } + } + } //----------------------------------------------------------------- //-- Mode QGCLabel { diff --git a/src/VehicleSetup/JoystickConfigCalibration.qml b/src/VehicleSetup/JoystickConfigCalibration.qml index edb6a79b2f87d68d92002990c2fd501e58ca3bd9..74b2bc9247079f604f260dd8f3976335bfb0f6ee 100644 --- a/src/VehicleSetup/JoystickConfigCalibration.qml +++ b/src/VehicleSetup/JoystickConfigCalibration.qml @@ -96,7 +96,7 @@ Item { width: ScreenTools.defaultFontPixelWidth * 0.125 height: parent.height * 0.2 color: qgcPal.text - visible: controller.hasGimbal + visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled x: (parent.width * 0.5) - (width * 0.5) y: (parent.height * 0.5) - (height * 0.5) } @@ -105,7 +105,7 @@ Item { width: parent.width * 0.035 height: width radius: width * 0.5 - visible: controller.hasGimbal + visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled x: (parent.width * controller.gimbalPositions[0]) - (width * 0.5) y: (parent.height * controller.gimbalPositions[1]) - (height * 0.5) } @@ -115,7 +115,7 @@ Item { width: parent.width * 0.2 height: ScreenTools.defaultFontPixelWidth * 0.125 color: qgcPal.text - visible: controller.hasGimbal + visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled x: (parent.width * 0.5) - (width * 0.5) y: (parent.height * 0.3) - (height * 0.5) } @@ -124,7 +124,7 @@ Item { width: parent.width * 0.035 height: width radius: width * 0.5 - visible: controller.hasGimbal + visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled x: (parent.width * controller.gimbalPositions[2]) - (width * 0.5) y: (parent.height * controller.gimbalPositions[3]) - (height * 0.5) } diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc index 6025a86061a818272266f6a02f380d0a6140b947..10e404ce003356c6cb581011b0e793080389c754 100644 --- a/src/VehicleSetup/JoystickConfigController.cc +++ b/src/VehicleSetup/JoystickConfigController.cc @@ -12,8 +12,6 @@ #include "JoystickManager.h" #include "QGCApplication.h" -#include - QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog") const int JoystickConfigController::_calCenterPoint = 0; @@ -167,9 +165,14 @@ void JoystickConfigController::_advanceState() { _currentStep++; const stateMachineEntry* state = _getStateMachineEntry(_currentStep); - while (state->channelID > _axisCount) { - _currentStep++; - state = _getStateMachineEntry(_currentStep); + //-- Handle Gimbal + if (state->channelID > _axisCount) { + //-- No channels for gimbal + _advanceState(); + } + if((state->channelID == 4 || state->channelID == 5) && !_activeJoystick->gimbalEnabled()) { + //-- Gimbal disabled. Skip it. + _advanceState(); } _setupCurrentState(); } @@ -220,7 +223,10 @@ void JoystickConfigController::_axisValueChanged(int axis, int value) // Track the axis count by keeping track of how many axes we see if (axis + 1 > static_cast(_axisCount)) { _axisCount = axis + 1; - emit hasGimbalChanged(); + if(_axisCount > 4) + emit hasGimbalPitchChanged(); + if(_axisCount > 5) + emit hasGimbalYawChanged(); } } if (_currentStep != -1) { @@ -631,8 +637,8 @@ void JoystickConfigController::_setStickPositions() _sticksYawRight = stLeftStickRight; _sticksRollLeft = stRightStickLeft; _sticksRollRight = stRightStickRight; - _sticksPitchUp = stLeftStickDown; - _sticksPitchDown = stLeftStickUp; + _sticksPitchUp = stLeftStickUp; + _sticksPitchDown = stLeftStickDown; break; case 2: _sticksThrottleUp = stLeftStickUp; @@ -641,8 +647,8 @@ void JoystickConfigController::_setStickPositions() _sticksYawRight = stLeftStickRight; _sticksRollLeft = stRightStickLeft; _sticksRollRight = stRightStickRight; - _sticksPitchUp = stRightStickDown; - _sticksPitchDown = stRightStickUp; + _sticksPitchUp = stRightStickUp; + _sticksPitchDown = stRightStickDown; break; case 3: _sticksThrottleUp = stRightStickUp; @@ -651,8 +657,8 @@ void JoystickConfigController::_setStickPositions() _sticksYawRight = stRightStickRight; _sticksRollLeft = stLeftStickLeft; _sticksRollRight = stLeftStickRight; - _sticksPitchUp = stLeftStickDown; - _sticksPitchDown = stLeftStickUp; + _sticksPitchUp = stLeftStickUp; + _sticksPitchDown = stLeftStickDown; break; case 4: _sticksThrottleUp = stLeftStickUp; @@ -661,8 +667,8 @@ void JoystickConfigController::_setStickPositions() _sticksYawRight = stRightStickRight; _sticksRollLeft = stLeftStickLeft; _sticksRollRight = stLeftStickRight; - _sticksPitchUp = stRightStickDown; - _sticksPitchDown = stRightStickUp; + _sticksPitchUp = stRightStickUp; + _sticksPitchDown = stRightStickDown; break; default: Q_ASSERT(false); @@ -766,7 +772,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) delete[] _axisRawValue; _axisCount = 0; _activeJoystick = nullptr; - emit hasGimbalChanged(); + emit hasGimbalPitchChanged(); + emit hasGimbalYawChanged(); } if (joystick) { @@ -781,7 +788,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) _axisRawValue = new int[_axisCount]; _setInternalCalibrationValuesFromSettings(); connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); - emit hasGimbalChanged(); + emit hasGimbalPitchChanged(); + emit hasGimbalYawChanged(); } } diff --git a/src/VehicleSetup/JoystickConfigController.h b/src/VehicleSetup/JoystickConfigController.h index f79e0d57b0a9c0d8389226da323610cb235016b1..685059889e497affe355b5ffb4212df394661092 100644 --- a/src/VehicleSetup/JoystickConfigController.h +++ b/src/VehicleSetup/JoystickConfigController.h @@ -46,7 +46,9 @@ public: Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged) Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged) - Q_PROPERTY(bool hasGimbal READ hasGimbal NOTIFY hasGimbalChanged) + Q_PROPERTY(bool hasGimbalPitch READ hasGimbalPitch NOTIFY hasGimbalPitchChanged) + Q_PROPERTY(bool hasGimbalYaw READ hasGimbalYaw NOTIFY hasGimbalYawChanged) + Q_PROPERTY(bool gimbalPitchAxisMapped READ gimbalPitchAxisMapped NOTIFY gimbalPitchAxisMappedChanged) Q_PROPERTY(bool gimbalYawAxisMapped READ gimbalYawAxisMapped NOTIFY gimbalYawAxisMappedChanged) @@ -90,7 +92,8 @@ public: bool gimbalPitchAxisReversed (); bool gimbalYawAxisReversed (); - bool hasGimbal () { return _axisCount > 5; } + bool hasGimbalPitch () { return _axisCount > 4; } + bool hasGimbalYaw () { return _axisCount > 5; } bool getDeadbandToggle (); void setDeadbandToggle (bool); @@ -136,7 +139,8 @@ signals: void skipEnabledChanged (); void stickPositionsChanged (); void gimbalPositionsChanged (); - void hasGimbalChanged (); + void hasGimbalPitchChanged (); + void hasGimbalYawChanged (); void statusTextChanged (); // @brief Signalled when in unit test mode and a message box should be displayed by the next button diff --git a/src/VehicleSetup/JoystickConfigGeneral.qml b/src/VehicleSetup/JoystickConfigGeneral.qml index dbca886c2e556d6f8ad71272e695bf4883356249..4d7940523df9ed15c54475192a27220fdfb39682 100644 --- a/src/VehicleSetup/JoystickConfigGeneral.qml +++ b/src/VehicleSetup/JoystickConfigGeneral.qml @@ -149,6 +149,7 @@ Item { anchors.centerIn: parent QGCLabel { text: activeVehicle.sub ? qsTr("Lateral") : qsTr("Roll") + Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 12 } AxisMonitor { id: rollAxis @@ -156,10 +157,6 @@ Item { width: axisMonitorWidth mapped: controller.rollAxisMapped reversed: controller.rollAxisReversed - Connections { - target: _activeJoystick - onManualControl: rollAxis.axisValue = roll * 32768.0 - } } QGCLabel { @@ -173,10 +170,6 @@ Item { width: axisMonitorWidth mapped: controller.pitchAxisMapped reversed: controller.pitchAxisReversed - Connections { - target: _activeJoystick - onManualControl: pitchAxis.axisValue = pitch * 32768.0 - } } QGCLabel { @@ -190,10 +183,6 @@ Item { width: axisMonitorWidth mapped: controller.yawAxisMapped reversed: controller.yawAxisReversed - Connections { - target: _activeJoystick - onManualControl: yawAxis.axisValue = yaw * 32768.0 - } } QGCLabel { @@ -207,9 +196,15 @@ Item { width: axisMonitorWidth mapped: controller.throttleAxisMapped reversed: controller.throttleAxisReversed - Connections { - target: _activeJoystick - onManualControl: throttleAxis.axisValue = _activeJoystick.negativeThrust ? throttle * -32768.0 : (-2 * throttle + 1) * 32768.0 + } + + Connections { + target: _activeJoystick + onManualControl: { + 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 } } @@ -217,7 +212,7 @@ Item { id: gimbalPitchLabel width: _attitudeLabelWidth text: qsTr("Gimbal Pitch") - visible: controller.hasGimbal + visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled } AxisMonitor { id: gimbalPitchAxis @@ -225,18 +220,14 @@ Item { width: axisMonitorWidth mapped: controller.gimbalPitchAxisMapped reversed: controller.gimbalPitchAxisReversed - visible: controller.hasGimbal - Connections { - target: _activeJoystick - onManualControl: gimbalPitchAxis.axisValue = gimbalPitch * 32768.0 - } + visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled } QGCLabel { id: gimbalYawLabel width: _attitudeLabelWidth text: qsTr("Gimbal Yaw") - visible: controller.hasGimbal + visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled } AxisMonitor { id: gimbalYawAxis @@ -244,9 +235,14 @@ Item { width: axisMonitorWidth mapped: controller.gimbalYawAxisMapped reversed: controller.gimbalYawAxisReversed - Connections { - target: _activeJoystick - onManualControl: gimbalYawAxis.axisValue = gimbalYaw * 32768.0 + visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled + } + + Connections { + target: _activeJoystick + onManualControlGimbal: { + gimbalPitchAxis.axisValue = gimbalPitch * 32768.0 + gimbalYawAxis.axisValue = gimbalYaw * 32768.0 } } }