From ff2c011d17c2a3d091d4aee188e72bb7ddf7b27c Mon Sep 17 00:00:00 2001 From: Bryant Date: Wed, 12 Jun 2013 18:02:17 -0700 Subject: [PATCH] Refactored much of the joystick interface with the largest changes being the removal of dead/unused code including broken event-based SDL reading code. Additionally the mapping code should now work, though it hasn't been tested with controller switching or a real UAS. Finally the existing settings code has been commented-out as it is not compatible with the current state of this code and needs to be able to support different joysticks based on name. --- src/input/JoystickInput.cc | 209 +++++++++++++------------------------ src/input/JoystickInput.h | 107 +++++-------------- src/ui/JoystickAxis.cc | 11 +- src/ui/JoystickAxis.h | 27 ++++- src/ui/JoystickAxis.ui | 23 ++++ src/ui/JoystickButton.h | 2 + src/ui/JoystickButton.ui | 20 ---- src/ui/JoystickWidget.cc | 66 +++++------- src/ui/JoystickWidget.h | 24 +---- src/ui/JoystickWidget.ui | 157 +++++++++++++++------------- 10 files changed, 274 insertions(+), 372 deletions(-) diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 3c0aae4e5..6a44f79ff 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -27,20 +27,15 @@ JoystickInput::JoystickInput() : sdlJoystickMin(-32768.0f), sdlJoystickMax(32767.0f), - defaultIndex(0), uas(NULL), - uasButtonList(QList()), done(false), - thrustAxis(2), - xAxis(0), - yAxis(1), - yawAxis(3), - autoButtonMapping(-1), - manualButtonMapping(-1), - stabilizeButtonMapping(-1), - joystickName(tr("Unitinialized")), + rollAxis(-1), + pitchAxis(-1), + yawAxis(-1), + throttleAxis(-1), + joystickName(""), joystickButtons(0), - joystickID(0) + joystickID(-1) { loadSettings(); @@ -64,34 +59,34 @@ JoystickInput::~JoystickInput() void JoystickInput::loadSettings() { - // Load defaults from settings - QSettings settings; - settings.sync(); - settings.beginGroup("QGC_JOYSTICK_INPUT"); - xAxis = (settings.value("X_AXIS_MAPPING", xAxis).toInt()); - yAxis = (settings.value("Y_AXIS_MAPPING", yAxis).toInt()); - thrustAxis = (settings.value("THRUST_AXIS_MAPPING", thrustAxis).toInt()); - yawAxis = (settings.value("YAW_AXIS_MAPPING", yawAxis).toInt()); - autoButtonMapping = (settings.value("AUTO_BUTTON_MAPPING", autoButtonMapping).toInt()); - stabilizeButtonMapping = (settings.value("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping).toInt()); - manualButtonMapping = (settings.value("MANUAL_BUTTON_MAPPING", manualButtonMapping).toInt()); - settings.endGroup(); +// // Load defaults from settings +// QSettings settings; +// settings.sync(); +// settings.beginGroup("QGC_JOYSTICK_INPUT"); +// xAxis = (settings.value("X_AXIS_MAPPING", xAxis).toInt()); +// yAxis = (settings.value("Y_AXIS_MAPPING", yAxis).toInt()); +// thrustAxis = (settings.value("THRUST_AXIS_MAPPING", thrustAxis).toInt()); +// yawAxis = (settings.value("YAW_AXIS_MAPPING", yawAxis).toInt()); +// autoButtonMapping = (settings.value("AUTO_BUTTON_MAPPING", autoButtonMapping).toInt()); +// stabilizeButtonMapping = (settings.value("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping).toInt()); +// manualButtonMapping = (settings.value("MANUAL_BUTTON_MAPPING", manualButtonMapping).toInt()); +// settings.endGroup(); } void JoystickInput::storeSettings() { - // Store settings - QSettings settings; - settings.beginGroup("QGC_JOYSTICK_INPUT"); - settings.setValue("X_AXIS_MAPPING", xAxis); - settings.setValue("Y_AXIS_MAPPING", yAxis); - settings.setValue("THRUST_AXIS_MAPPING", thrustAxis); - settings.setValue("YAW_AXIS_MAPPING", yawAxis); - settings.setValue("AUTO_BUTTON_MAPPING", autoButtonMapping); - settings.setValue("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping); - settings.setValue("MANUAL_BUTTON_MAPPING", manualButtonMapping); - settings.endGroup(); - settings.sync(); +// // Store settings +// QSettings settings; +// settings.beginGroup("QGC_JOYSTICK_INPUT"); +// settings.setValue("X_AXIS_MAPPING", xAxis); +// settings.setValue("Y_AXIS_MAPPING", yAxis); +// settings.setValue("THRUST_AXIS_MAPPING", thrustAxis); +// settings.setValue("YAW_AXIS_MAPPING", yawAxis); +// settings.setValue("AUTO_BUTTON_MAPPING", autoButtonMapping); +// settings.setValue("STABILIZE_BUTTON_MAPPING", stabilizeButtonMapping); +// settings.setValue("MANUAL_BUTTON_MAPPING", manualButtonMapping); +// settings.endGroup(); +// settings.sync(); } @@ -158,10 +153,8 @@ void JoystickInput::init() SDL_JoystickClose(x); } - SDL_JoystickEventState(SDL_ENABLE); - - // And attach to the default joystick. - setActiveJoystick(defaultIndex); + // And attach to the first joystick found to start. + setActiveJoystick(0); // Make sure active UAS is set setActiveUAS(UASManager::instance()->getActiveUAS()); @@ -174,6 +167,8 @@ void JoystickInput::shutdown() /** * @brief Execute the Joystick process + * Note that the SDL procedure is polled. This is because connecting and disconnecting while the event checker is running + * fails as of SDL 1.2. It is therefore much easier to just poll for the joystick we want to sample. */ void JoystickInput::run() { @@ -187,97 +182,26 @@ void JoystickInput::run() exit(); return; } - while(SDL_PollEvent(&event)) - { - SDL_JoystickUpdate(); - - // Todo check if it would be more beneficial to use the event structure - switch(event.type) { - case SDL_KEYDOWN: - /* handle keyboard stuff here */ - //qDebug() << "KEY PRESSED!"; - break; - - case SDL_QUIT: - /* Set whatever flags are necessary to */ - /* end the main loop here */ - break; - - case SDL_JOYBUTTONDOWN: /* Handle Joystick Button Presses */ - if ( event.jbutton.button == 0 ) { - //qDebug() << "BUTTON PRESSED!"; - } - break; - - case SDL_JOYAXISMOTION: /* Handle Joystick Motion */ - if ( ( event.jaxis.value < -3200 ) || (event.jaxis.value > 3200 ) ) { - if( event.jaxis.axis == 0) { - /* Left-right movement code goes here */ - } - - if( event.jaxis.axis == 1) { - /* Up-Down movement code goes here */ - } - } - break; - - default: - //qDebug() << "SDL event occured"; - break; - } - } + // Poll the joystick for new values. + SDL_JoystickUpdate(); -// // Display all axes -// for(int i = 0; i < SDL_JoystickNumAxes(joystick); i++) -// { -// qDebug() << "\rAXIS" << i << "is: " << SDL_JoystickGetAxis(joystick, i); -// } - - // THRUST - double thrust = ((double)SDL_JoystickGetAxis(joystick, thrustAxis) - calibrationNegative[thrustAxis]) / (calibrationPositive[thrustAxis] - calibrationNegative[thrustAxis]); - // Has to be inverted for Logitech Wingman - thrust = 1.0f - thrust; - thrust = thrust * 2.0f - 1.0f; - // Bound rounding errors - if (thrust > 1.0f) thrust = 1.0f; - if (thrust < -1.0f) thrust = -1.0f; - emit thrustChanged((float)thrust); - - // X Axis - double x = ((double)SDL_JoystickGetAxis(joystick, xAxis) - calibrationNegative[xAxis]) / (calibrationPositive[xAxis] - calibrationNegative[xAxis]); - x = 1.0f - x; - x = x * 2.0f - 1.0f; - // Bound rounding errors - if (x > 1.0f) x = 1.0f; - if (x < -1.0f) x = -1.0f; - emit xChanged((float)x); - - // Y Axis - double y = ((double)SDL_JoystickGetAxis(joystick, yAxis) - calibrationNegative[yAxis]) / (calibrationPositive[yAxis] - calibrationNegative[yAxis]); - y = 1.0f - y; - y = y * 2.0f - 1.0f; - // Bound rounding errors - if (y > 1.0f) y = 1.0f; - if (y < -1.0f) y = -1.0f; - emit yChanged((float)y); - - // Yaw Axis - - double yaw = ((double)SDL_JoystickGetAxis(joystick, yawAxis) - calibrationNegative[yawAxis]) / (calibrationPositive[yawAxis] - calibrationNegative[yawAxis]); - yaw = 1.0f - yaw; - yaw = yaw * 2.0f - 1.0f; - // Bound rounding errors - if (yaw > 1.0f) yaw = 1.0f; - if (yaw < -1.0f) yaw = -1.0f; - emit yawChanged((float)yaw); - - // Get joystick hat position, convert it to vector - int hatPosition = SDL_JoystickGetHat(joystick, 0); - - int xHat,yHat; - xHat = 0; - yHat = 0; + // Emit all necessary signals for all axes. + float axesValues[joystickAxes]; + for (int i = 0; i < joystickAxes; i++) + { + // First emit the uncalibrated values for each axis based on their ID. + // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine. + float axisValue = (SDL_JoystickGetAxis(joystick, i) - calibrationNegative[i]) / (calibrationPositive[i] - calibrationNegative[i]); + axisValue = 1.0f - axisValue; + axisValue = axisValue * 2.0f - 1.0f; + + // Bound rounding errors + if (axisValue > 1.0f) axisValue = 1.0f; + if (axisValue < -1.0f) axisValue = -1.0f; + axesValues[i] = axisValue; + emit axisValueChanged(i, axisValue); + } // Build up vectors describing the hat position // @@ -289,18 +213,16 @@ void JoystickInput::run() // | // 0 ----> x // - + int hatPosition = SDL_JoystickGetHat(joystick, 0); + int xHat = 0, yHat = 0; if ((SDL_HAT_UP & hatPosition) > 0) yHat = 1; if ((SDL_HAT_DOWN & hatPosition) > 0) yHat = -1; - if ((SDL_HAT_LEFT & hatPosition) > 0) xHat = -1; if ((SDL_HAT_RIGHT & hatPosition) > 0) xHat = 1; - - // Send new values to rest of groundstation emit hatDirectionChanged(xHat, yHat); // Emit signals for each button individually - for (int i = 0; i < SDL_JoystickNumButtons(joystick); i++) + for (int i = 0; i < joystickButtons; i++) { // If the button was down, but now it's up, trigger a buttonPressed event quint16 lastButtonState = buttonState & (1 << i); @@ -317,7 +239,11 @@ void JoystickInput::run() } // Now signal an update for all UI together. - emit joystickChanged(y, x, yaw, thrust, xHat, yHat, buttonState); + float roll = rollAxis > -1?axesValues[rollAxis]:0.0f; + float pitch = pitchAxis > -1?axesValues[pitchAxis]:0.0f; + float yaw = yawAxis > -1?axesValues[yawAxis]:0.0f; + float throttle = throttleAxis > -1?axesValues[throttleAxis]:0.0f; + emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, buttonState); // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) QGC::SLEEP::msleep(20); @@ -326,6 +252,13 @@ void JoystickInput::run() void JoystickInput::setActiveJoystick(int id) { + if (joystick && SDL_JoystickOpened(joystickID)) + { + SDL_JoystickClose(joystick); + joystick = NULL; + joystickID = -1; + } + joystickID = id; joystick = SDL_JoystickOpen(joystickID); if (joystick) @@ -338,7 +271,11 @@ void JoystickInput::setActiveJoystick(int id) buttonState = 0; } -const QString& JoystickInput::getName() +float JoystickInput::getCurrentValueForAxis(int axisID) { - return joystickName; + if (joystick && axisID < joystickAxes) + { + return SDL_JoystickGetAxis(joystick, axisID); + } + return 0.0f; } diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 96896083e..4ac0b899a 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -57,8 +57,6 @@ public: void run(); void shutdown(); - const QString& getName(); - /** * @brief Load joystick settings */ @@ -69,19 +67,19 @@ public: */ void storeSettings(); - int getMappingThrustAxis() const + int getMappingThrottleAxis() const { - return thrustAxis; + return throttleAxis; } - int getMappingXAxis() const + int getMappingRollAxis() const { - return xAxis; + return rollAxis; } - int getMappingYAxis() const + int getMappingPitchAxis() const { - return yAxis; + return pitchAxis; } int getMappingYawAxis() const @@ -89,21 +87,6 @@ public: return yawAxis; } - int getMappingAutoButton() const - { - return autoButtonMapping; - } - - int getMappingManualButton() const - { - return manualButtonMapping; - } - - int getMappingStabilizeButton() const - { - return stabilizeButtonMapping; - } - int getJoystickNumButtons() const { return joystickButtons; @@ -119,6 +102,11 @@ public: return joystickID; } + const QString& getName() const + { + return joystickName; + } + int getNumJoysticks() const { return joysticksFound; @@ -129,33 +117,31 @@ public: return QString(SDL_JoystickName(id)); } + float getCurrentValueForAxis(int axisID); + const double sdlJoystickMin; const double sdlJoystickMax; protected: - int defaultIndex; double calibrationPositive[10]; double calibrationNegative[10]; SDL_Joystick* joystick; UASInterface* uas; - QList uasButtonList; bool done; - QMutex m_doneMutex; - // Axis 3 is thrust (CALIBRATION!) - int thrustAxis; - int xAxis; - int yAxis; + // Store the mapping between axis numbers and the roll/pitch/yaw/throttle configuration. + int rollAxis; + int pitchAxis; int yawAxis; - int autoButtonMapping; - int manualButtonMapping; - int stabilizeButtonMapping; - SDL_Event event; + int throttleAxis; + + // Cache information on the joystick instead of polling the SDL everytime. QString joystickName; int joystickAxes; int joystickButtons; int joystickID; int joysticksFound; + quint16 buttonState; ///< Track the state of the buttons so we can trigger on Up and Down events void init(); @@ -175,32 +161,11 @@ signals: void joystickChanged(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons); /** - * @brief Thrust lever of the joystick has changed - * - * @param thrust Thrust, 0%: 0, 100%: 1.0 - */ - void thrustChanged(int thrust); - - /** - * @brief X-Axis / forward-backward axis has changed - * - * @param x forward / pitch / x axis, front: +1.0, center: 0.0, back: -1.0 - */ - void xChanged(int x); - - /** - * @brief Y-Axis / left-right axis has changed - * - * @param y left / roll / y axis, left: -1.0, middle: 0.0, right: +1.0 - */ - void yChanged(int y); - - /** - * @brief Yaw / left-right turn has changed + * @brief Emit a new value for an axis * - * @param yaw turn axis, left-turn: -1.0, middle: 0.0, right-turn: +1.0 + * @param value Value of the axis, between -1.0 and 1.0. */ - void yawChanged(int yaw); + void axisValueChanged(int axis, float value); /** * @brief Joystick button has changed state from unpressed to pressed. @@ -236,19 +201,15 @@ public slots: void setActiveUAS(UASInterface* uas); /** @brief Switch to a new joystick by ID number. */ void setActiveJoystick(int id); - void setMappingThrustAxis(int mapping) - { - thrustAxis = mapping; - } - void setMappingXAxis(int mapping) + void setMappingRollAxis(int mapping) { - xAxis = mapping; + rollAxis = mapping; } - void setMappingYAxis(int mapping) + void setMappingPitchAxis(int mapping) { - yAxis = mapping; + pitchAxis = mapping; } void setMappingYawAxis(int mapping) @@ -256,19 +217,9 @@ public slots: yawAxis = mapping; } - void setMappingAutoButton(int mapping) - { - autoButtonMapping = mapping; - } - - void setMappingManualButton(int mapping) - { - manualButtonMapping = mapping; - } - - void setMappingStabilizeButton(int mapping) + void setMappingThrottleAxis(int mapping) { - stabilizeButtonMapping = mapping; + throttleAxis = mapping; } }; diff --git a/src/ui/JoystickAxis.cc b/src/ui/JoystickAxis.cc index 4a65de5d2..5f16f4964 100644 --- a/src/ui/JoystickAxis.cc +++ b/src/ui/JoystickAxis.cc @@ -4,10 +4,12 @@ JoystickAxis::JoystickAxis(int id, QWidget *parent) : QWidget(parent), + id(id), ui(new Ui::JoystickAxis) { ui->setupUi(this); ui->label->setText(QString::number(id)); + connect(ui->comboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(mappingComboBoxChanged(int))); } JoystickAxis::~JoystickAxis() @@ -15,7 +17,12 @@ JoystickAxis::~JoystickAxis() delete ui; } -void JoystickAxis::setValue(int value) +void JoystickAxis::setValue(float value) { - ui->progressBar->setValue(value); + ui->progressBar->setValue(100.0f * value); +} + +void JoystickAxis::mappingComboBoxChanged(int newMapping) +{ + emit mappingChanged(id, newMapping); } diff --git a/src/ui/JoystickAxis.h b/src/ui/JoystickAxis.h index 343936550..ee948d256 100644 --- a/src/ui/JoystickAxis.h +++ b/src/ui/JoystickAxis.h @@ -15,11 +15,36 @@ public: explicit JoystickAxis(int id, QWidget *parent = 0); ~JoystickAxis(); + /** + * @brief The JOYSTICK_MAPPING enum storing the values for each item in the mapping combobox. + * This should match the order of items in the mapping combobox in JoystickAxis.ui. + */ + enum JOYSTICK_AXIS_MAPPING + { + JOYSTICK_AXIS_MAPPING_NONE = 0, + JOYSTICK_AXIS_MAPPING_YAW = 1, + JOYSTICK_AXIS_MAPPING_PITCH = 2, + JOYSTICK_AXIS_MAPPING_ROLL = 3, + JOYSTICK_AXIS_MAPPING_THROTTLE = 4 + }; + +signals: + /** @brief Signal a change in this axis' yaw/pitch/roll mapping */ + void mappingChanged(int id, int newMapping); + public slots: - void setValue(int value); + /** @brief Update the displayed value of the included progressbar. + * @param value A value between -1.0 and 1.0. + */ + void setValue(float value); private: + int id; ///< The ID for this axis. Corresponds to the IDs used by JoystickInput. Ui::JoystickAxis *ui; + +private slots: + /** @brief Handle changes to the mapping dropdown bar. */ + void mappingComboBoxChanged(int newMapping); }; #endif // JOYSTICKAXIS_H diff --git a/src/ui/JoystickAxis.ui b/src/ui/JoystickAxis.ui index 925727a3b..ac09895a0 100644 --- a/src/ui/JoystickAxis.ui +++ b/src/ui/JoystickAxis.ui @@ -61,6 +61,26 @@ -- + + + Yaw + + + + + Pitch + + + + + Roll + + + + + Throttle + + @@ -80,6 +100,9 @@ 100 + + -100 + 0 diff --git a/src/ui/JoystickButton.h b/src/ui/JoystickButton.h index 4c1bbaebf..eb5bc3101 100644 --- a/src/ui/JoystickButton.h +++ b/src/ui/JoystickButton.h @@ -11,9 +11,11 @@ class JoystickButton; class JoystickButton : public QWidget { Q_OBJECT + public: explicit JoystickButton(int id, QWidget *parent = 0); virtual ~JoystickButton(); + private: int id; Ui::JoystickButton *m_ui; diff --git a/src/ui/JoystickButton.ui b/src/ui/JoystickButton.ui index de82b9b5b..8e5191a70 100644 --- a/src/ui/JoystickButton.ui +++ b/src/ui/JoystickButton.ui @@ -48,26 +48,6 @@ -- - - - Yaw - - - - - Pitch - - - - - Roll - - - - - Throttle - - diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index b8c408ec6..73e8d2478 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -21,17 +21,10 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : // Initialize the UI based on the current joystick initUI(); - // Watch for input events from the joystick - connect(this->joystick, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), this, SLOT(updateJoystick(double,double,double,double,int,int))); + // Watch for button and hat input events from the joystick. connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(joystickButtonPressed(int))); connect(this->joystick, SIGNAL(buttonReleased(int)), this, SLOT(joystickButtonReleased(int))); - - // Watch for changes to the button/axis mappings -// connect(m_ui->rollMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingXAxis(int))); -// connect(m_ui->pitchMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingYAxis(int))); -// connect(m_ui->yawMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingYawAxis(int))); -// connect(m_ui->throttleMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingThrustAxis(int))); -// connect(m_ui->autoMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingAutoButton(int))); + connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float))); // Update the UI if the joystick changes. connect(m_ui->joystickNameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateUIForJoystick(int))); @@ -92,15 +85,6 @@ JoystickWidget::~JoystickWidget() delete m_ui; } -void JoystickWidget::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat) -{ - setX(roll); - setY(pitch); - setZ(yaw); - setThrottle(thrust); - setHat(xHat, yHat); -} - void JoystickWidget::changeEvent(QEvent *e) { switch (e->type()) { @@ -143,49 +127,46 @@ void JoystickWidget::updateUIForJoystick(int id) for (int i = 0; i < joystick->getJoystickNumAxes(); i++) { JoystickAxis* axis = new JoystickAxis(i, m_ui->axesBox); + axis->setValue(joystick->getCurrentValueForAxis(i)); + connect(axis, SIGNAL(mappingChanged(int,int)), this, SLOT(setMappingAxis(int,int))); // And make sure we insert BEFORE the vertical spacer. m_ui->axesLayout->insertWidget(i, axis); axes.append(axis); } } -void JoystickWidget::setX(float x) -{ - if (axes.size() > 0) - { - axes.at(0)->setValue(x * 100); - } -} - -void JoystickWidget::setY(float y) +void JoystickWidget::updateAxisValue(int axis, float value) { - if (axes.size() > 1) + if (axis < axes.size()) { - axes.at(1)->setValue(y * 100); + axes.at(axis)->setValue(value); } } -void JoystickWidget::setZ(float z) +void JoystickWidget::setHat(float x, float y) { - if (axes.size() > 2) - { - axes.at(2)->setValue(z * 100); - } + updateStatus(tr("Hat position: x: %1, y: %2").arg(x).arg(y)); } -void JoystickWidget::setThrottle(float t) +void JoystickWidget::setMappingAxis(int axisID, int newMapping) { - if (axes.size() > 3) + switch (newMapping) { - axes.at(3)->setValue(t * 100); + case JoystickAxis::JOYSTICK_AXIS_MAPPING_ROLL: + joystick->setMappingRollAxis(axisID); + break; + case JoystickAxis::JOYSTICK_AXIS_MAPPING_PITCH: + joystick->setMappingPitchAxis(axisID); + break; + case JoystickAxis::JOYSTICK_AXIS_MAPPING_YAW: + joystick->setMappingYawAxis(axisID); + break; + case JoystickAxis::JOYSTICK_AXIS_MAPPING_THROTTLE: + joystick->setMappingThrottleAxis(axisID); + break; } } -void JoystickWidget::setHat(float x, float y) -{ - updateStatus(tr("Hat position: x: %1, y: %2").arg(x).arg(y)); -} - void JoystickWidget::joystickButtonPressed(int key) { QString colorStyle = QString("QLabel { background-color: %1;}").arg(buttonLabelColor.name()); @@ -199,4 +180,5 @@ void JoystickWidget::joystickButtonReleased(int key) void JoystickWidget::updateStatus(const QString& status) { + m_ui->statusLabel->setText(status); } diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index 63d050ced..5b8dd532e 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -33,6 +33,7 @@ This file is part of the PIXHAWK project #include #include +#include #include "JoystickInput.h" #include "MainWindow.h" #include "JoystickAxis.h" @@ -52,26 +53,7 @@ public: virtual ~JoystickWidget(); public slots: - /** - * @brief Receive raw joystick values - * - * @param roll forward / pitch / x axis, front: 32'767, center: 0, back: -32'768 - * @param pitch left / roll / y axis, left: -32'768, middle: 0, right: 32'767 - * @param yaw turn axis, left-turn: -32'768, centered: 0, right-turn: 32'767 - * @param thrust Thrust, 0%: 0, 100%: 65535 - * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward - * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right - */ - void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat); - /** @brief Back/forth movement */ - void setX(float x); - /** @brief Left/right movement */ - void setY(float y); - /** @brief Wrist rotation */ - void setZ(float z); - /** @brief Throttle lever */ - void setThrottle(float thrust); - /** @brief Hat switch position */ + void updateAxisValue(int axis, float value); void setHat(float x, float y); /** @brief Trigger a UI change based on a button being pressed */ void joystickButtonPressed(int key); @@ -98,6 +80,8 @@ protected: protected slots: /** @brief Update the UI for a new joystick based on SDL ID. */ void updateUIForJoystick(int id); + /** @brief Change the stored mapping for a given axis. */ + void setMappingAxis(int axisID, int newMapping); private: Ui::JoystickWidget *m_ui; diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui index 2d74aa4d0..f33134eb6 100644 --- a/src/ui/JoystickWidget.ui +++ b/src/ui/JoystickWidget.ui @@ -78,86 +78,97 @@ QFrame::Sunken - + QLayout::SetMinimumSize - - - - 0 - 0 - - - - - 100 - 0 - - - - - 16777215 - 16777215 - - - - Buttons - - - Qt::AlignCenter - - - false - - - - 1 - - - QLayout::SetMinimumSize - - - 3 - - - 3 - - - 3 - - - 3 - - - + + + + + + 0 + 0 + + + + + 100 + 0 + + + + + 16777215 + 16777215 + + + + Buttons + + + Qt::AlignCenter + + + false + + + + 1 + + + QLayout::SetMinimumSize + + + 3 + + + 3 + + + 3 + + + 3 + + + + + + + + + 0 + 0 + + + + + 100 + 0 + + + + Axes + + + Qt::AlignCenter + + + + QLayout::SetMinimumSize + + + + + - - - - 0 - 0 - - - - - 100 - 0 - - - - Axes - - - Qt::AlignCenter + + + - - - QLayout::SetMinimumSize - - -- 2.22.0