diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 6a44f79ff0c130321a2908ee324698a3346f4060..5c8f48135e30385eec82377f7f89f9c927d55399 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -19,6 +19,7 @@ #include "QGC.h" #include #include +#include /** * The coordinate frame of the joystick axis is the aeronautical frame like shown on this image: @@ -34,7 +35,7 @@ JoystickInput::JoystickInput() : yawAxis(-1), throttleAxis(-1), joystickName(""), - joystickButtons(0), + joystickNumButtons(0), joystickID(-1) { loadSettings(); @@ -124,10 +125,10 @@ void JoystickInput::init() } // Enumerate joysticks and select one - joysticksFound = SDL_NumJoysticks(); + numJoysticks = SDL_NumJoysticks(); // Wait for joysticks if none are connected - while (joysticksFound == 0 && !done) + while (numJoysticks == 0 && !done) { QGC::SLEEP::msleep(400); // INITIALIZE SDL Joystick support @@ -135,15 +136,15 @@ void JoystickInput::init() { printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError()); } - joysticksFound = SDL_NumJoysticks(); + numJoysticks = SDL_NumJoysticks(); } if (done) { return; } - qDebug() << QString("%1 Input devices found:").arg(joysticksFound); - for(int i=0; i < joysticksFound; i++ ) + qDebug() << QString("%1 Input devices found:").arg(numJoysticks); + for(int i=0; i < numJoysticks; i++ ) { qDebug() << QString("\t- %1").arg(SDL_JoystickName(i)); SDL_Joystick* x = SDL_JoystickOpen(i); @@ -187,8 +188,7 @@ void JoystickInput::run() SDL_JoystickUpdate(); // Emit all necessary signals for all axes. - float axesValues[joystickAxes]; - for (int i = 0; i < joystickAxes; i++) + for (int i = 0; i < joystickNumAxes; 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. @@ -199,22 +199,12 @@ void JoystickInput::run() // Bound rounding errors if (axisValue > 1.0f) axisValue = 1.0f; if (axisValue < -1.0f) axisValue = -1.0f; - axesValues[i] = axisValue; + joystickAxes[i] = axisValue; emit axisValueChanged(i, axisValue); } // Build up vectors describing the hat position - // - // Coordinate frame for joystick hat: - // - // y - // ^ - // | - // | - // 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; @@ -222,28 +212,28 @@ void JoystickInput::run() emit hatDirectionChanged(xHat, yHat); // Emit signals for each button individually - for (int i = 0; i < joystickButtons; i++) + for (int i = 0; i < joystickNumButtons; i++) { // If the button was down, but now it's up, trigger a buttonPressed event - quint16 lastButtonState = buttonState & (1 << i); + quint16 lastButtonState = joystickButtons & (1 << i); if (SDL_JoystickGetButton(joystick, i) && !lastButtonState) { emit buttonPressed(i); - buttonState |= 1 << i; + joystickButtons |= 1 << i; } else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState) { emit buttonReleased(i); - buttonState &= ~(1 << i); + joystickButtons &= ~(1 << i); } } // Now signal an update for all UI together. - 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); + float roll = rollAxis > -1?joystickAxes[rollAxis]:NAN; + float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:NAN; + float yaw = yawAxis > -1?joystickAxes[yawAxis]:NAN; + float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:NAN; + emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons); // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) QGC::SLEEP::msleep(20); @@ -263,17 +253,30 @@ void JoystickInput::setActiveJoystick(int id) joystick = SDL_JoystickOpen(joystickID); if (joystick) { + // Update joystick configuration. joystickName = QString(SDL_JoystickName(joystickID)); - joystickButtons = SDL_JoystickNumButtons(joystick); - joystickAxes = SDL_JoystickNumAxes(joystick); - qDebug() << QString("Switching to joystick '%1' with %2 buttons/%3 axes").arg(joystickName, QString::number(joystickButtons), QString::number(joystickAxes)); + joystickNumButtons = SDL_JoystickNumButtons(joystick); + joystickNumAxes = SDL_JoystickNumAxes(joystick); + + // Reset cached joystick values + joystickAxes.clear(); + while (joystickAxes.size() < joystickNumAxes) + { + joystickAxes.append(0); + } + joystickButtons = 0; + qDebug() << QString("Switching to joystick '%1' with %2 buttons/%3 axes").arg(joystickName, QString::number(joystickNumButtons), QString::number(joystickNumAxes)); + } + else + { + joystickNumButtons = 0; + joystickNumAxes = 0; } - buttonState = 0; } float JoystickInput::getCurrentValueForAxis(int axisID) { - if (joystick && axisID < joystickAxes) + if (joystick && axisID < joystickNumAxes) { return SDL_JoystickGetAxis(joystick, axisID); } diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 4ac0b899af5bf90fbeeb638acbf8c97dfb010f23..6e923ad2d6f6bf81c8e8d2db6aa9f35aef870427 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -89,12 +89,12 @@ public: int getJoystickNumButtons() const { - return joystickButtons; + return joystickNumButtons; } int getJoystickNumAxes() const { - return joystickAxes; + return joystickNumAxes; } int getJoystickID() const @@ -109,7 +109,7 @@ public: int getNumJoysticks() const { - return joysticksFound; + return numJoysticks; } QString getJoystickNameById(int id) const @@ -130,19 +130,22 @@ protected: bool done; // Store the mapping between axis numbers and the roll/pitch/yaw/throttle configuration. + // Value is one of JoystickAxis::JOYSTICK_AXIS_MAPPING. int rollAxis; int pitchAxis; int yawAxis; int throttleAxis; // Cache information on the joystick instead of polling the SDL everytime. + int numJoysticks; ///< Total number of joysticks detected by the SDL. QString joystickName; - int joystickAxes; - int joystickButtons; int joystickID; - int joysticksFound; + int joystickNumAxes; + int joystickNumButtons; - quint16 buttonState; ///< Track the state of the buttons so we can trigger on Up and Down events + QList joystickAxes; ///< The values of every axes during the last sample + quint16 joystickButtons; ///< The state of every button. Bitfield supporting 16 buttons with 1s indicating that the button is down. + int xHat, yHat; ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left. void init(); @@ -151,14 +154,14 @@ signals: /** * @brief Signal containing all joystick raw positions * - * @param roll forward / pitch / x axis, front: 1, center: 0, back: -1 - * @param pitch left / roll / y axis, left: -1, middle: 0, right: 1 - * @param yaw turn axis, left-turn: -1, centered: 0, right-turn: 1 - * @param thrust Thrust, 0%: 0, 100%: 1 + * @param roll forward / pitch / x axis, front: 1, center: 0, back: -1. If the roll axis isn't defined, NaN is transmit instead. + * @param pitch left / roll / y axis, left: -1, middle: 0, right: 1. If the roll axis isn't defined, NaN is transmit instead. + * @param yaw turn axis, left-turn: -1, centered: 0, right-turn: 1. If the roll axis isn't defined, NaN is transmit instead. + * @param throttle Throttle, -100%:-1.0, 0%: 0.0, 100%: 1.0. If the roll axis isn't defined, NaN is transmit instead. * @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, int buttons); + void joystickChanged(double roll, double pitch, double yaw, double throttle, int xHat, int yHat, int buttons); /** * @brief Emit a new value for an axis