/*=====================================================================
======================================================================*/
/**
* @file
* @brief Joystick interface
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
* @author Andreas Romer <mavteam@student.ethz.ch>
*
*/
#include "JoystickInput.h"
#include <QDebug>
#include <limits.h>
#include "UAS.h"
#include "UASManager.h"
#include "QGC.h"
#include <QMutexLocker>
#include <QSettings>
#include <math.h>
/**
* The coordinate frame of the joystick axis is the aeronautical frame like shown on this image:
* @image html http://pixhawk.ethz.ch/wiki/_media/standards/body-frame.png Aeronautical frame
*/
JoystickInput::JoystickInput() :
sdlJoystickMin(-32768.0f),
sdlJoystickMax(32767.0f),
uas(NULL),
done(false),
rollAxis(-1),
pitchAxis(-1),
yawAxis(-1),
throttleAxis(-1),
joystickName(""),
joystickNumButtons(0),
joystickID(-1)
{
loadSettings();
for (int i = 0; i < 10; i++) {
calibrationPositive[i] = sdlJoystickMax;
calibrationNegative[i] = sdlJoystickMin;
}
// Listen for when the active UAS changes so we can change who we're sending data to.
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
// Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected.
start();
}
JoystickInput::~JoystickInput()
{
storeSettings();
done = true;
}
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();
}
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();
}
void JoystickInput::setActiveUAS(UASInterface* uas)
{
// Only connect / disconnect is the UAS is of a controllable UAS class
UAS* tmp = 0;
if (this->uas)
{
tmp = dynamic_cast<UAS*>(this->uas);
if(tmp)
{
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)));
}
}
this->uas = uas;
if (this->uas)
{
tmp = dynamic_cast<UAS*>(this->uas);
if(tmp) {
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)));
}
}
}
void JoystickInput::init()
{
// INITIALIZE SDL Joystick support
if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
}
// Enumerate joysticks and select one
numJoysticks = SDL_NumJoysticks();
// Wait for joysticks if none are connected
while (numJoysticks == 0 && !done)
{
QGC::SLEEP::msleep(400);
// INITIALIZE SDL Joystick support
if (SDL_InitSubSystem(SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0)
{
printf("Couldn't initialize SimpleDirectMediaLayer: %s\n", SDL_GetError());
}
numJoysticks = SDL_NumJoysticks();
}
if (done)
{
return;
}
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);
qDebug() << QString("Number of Axes: %1").arg(QString::number(SDL_JoystickNumAxes(x)));
qDebug() << QString("Number of Buttons: %1").arg(QString::number(SDL_JoystickNumButtons(x)));
qDebug() << QString("Number of Balls: %1").arg(QString::number(SDL_JoystickNumBalls(x)));
SDL_JoystickClose(x);
}
// And attach to the first joystick found to start.
setActiveJoystick(0);
// Make sure active UAS is set
setActiveUAS(UASManager::instance()->getActiveUAS());
}
void JoystickInput::shutdown()
{
done = true;
}
/**
* @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()
{
init();
forever
{
if (done)
{
done = false;
exit();
return;
}
// Poll the joystick for new values.
SDL_JoystickUpdate();
// Emit all necessary signals for all axes.
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.
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;
joystickAxes[i] = axisValue;
emit axisValueChanged(i, axisValue);
}
// Build up vectors describing the hat position
int hatPosition = SDL_JoystickGetHat(joystick, 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;
emit hatDirectionChanged(xHat, yHat);
// Emit signals for each button individually
for (int i = 0; i < joystickNumButtons; i++)
{
// If the button was down, but now it's up, trigger a buttonPressed event
quint16 lastButtonState = joystickButtons & (1 << i);
if (SDL_JoystickGetButton(joystick, i) && !lastButtonState)
{
emit buttonPressed(i);
joystickButtons |= 1 << i;
}
else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState)
{
emit buttonReleased(i);
joystickButtons &= ~(1 << i);
}
}
// Now signal an update for all UI together.
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);
}
}
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)
{
// Update joystick configuration.
joystickName = QString(SDL_JoystickName(joystickID));
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;
}
}
float JoystickInput::getCurrentValueForAxis(int axisID)
{
if (joystick && axisID < joystickNumAxes)
{
return SDL_JoystickGetAxis(joystick, axisID);
}
return 0.0f;
}