diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index c07134db78dc631fa8c97f317ddd69c2234bf9c4..fbfe0faa12fcfd9ee0ea4ddeb51cb040d11332fa 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -101,7 +101,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas) tmp = dynamic_cast(this->uas); if(tmp) { - disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double))); + disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); disconnect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int))); } } @@ -110,7 +110,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas) tmp = dynamic_cast(this->uas); if(tmp) { - connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double))); + connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int))); } if (!isRunning()) @@ -163,7 +163,6 @@ void JoystickInput::init() */ void JoystickInput::run() { - init(); forever @@ -284,16 +283,16 @@ void JoystickInput::run() // Send new values to rest of groundstation emit hatDirectionChanged(xHat, yHat); - emit joystickChanged(y, x, yaw, thrust, xHat, yHat); - // Display all buttons + int buttons = 0; for(int i = 0; i < SDL_JoystickNumButtons(joystick); i++) { //qDebug() << "BUTTON" << i << "is: " << SDL_JoystickGetAxis(joystick, i); if(SDL_JoystickGetButton(joystick, i)) { emit buttonPressed(i); + buttons |= 1 << i; // Check if button is a UAS select button if (uasButtonList.contains(i)) @@ -307,6 +306,7 @@ void JoystickInput::run() } } + emit joystickChanged(y, x, yaw, thrust, xHat, yHat, buttons); // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) QGC::SLEEP::msleep(20); diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index fc1a096409312487932b7090dbfe35676499038e..2bcba7dfb42706bee5fefb03192c90b287af47d0 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -141,7 +141,7 @@ signals: * @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 joystickChanged(double roll, double pitch, double yaw, double thrust, int xHat, int yHat); + void joystickChanged(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons); /** * @brief Thrust lever of the joystick has changed diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cfe62c783cb87c26f8e28de7e73e3062a0658b4a..e3511bbf17a112a59963e3c3d50f82ad6e7afeeb 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2435,7 +2435,7 @@ void UAS::disarmSystem() * Set the manual control commands. * This can only be done if the system has manual inputs enabled and is armed. */ -void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust) +void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons) { // Scale values double rollPitchScaling = 1.0f * 1000.0f; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e56a58a6a1794d76cc90d7c837eba40d63cbdadf..7eb1e78ce18f1ccf85cb04c86f11dcfdc51d0305 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -566,7 +566,7 @@ public slots: void disarmSystem(); /** @brief Set the values for the manual control of the vehicle */ - void setManualControlCommands(double roll, double pitch, double yaw, double thrust); + void setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons); /** @brief Receive a button pressed event from an input device, e.g. joystick */ void receiveButton(int buttonIndex);