Newer
Older
/*=====================================================================
======================================================================*/
/**
* @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"
oberion
committed
#include <QMutexLocker>
#include <math.h>
Bryant
committed
#include <limits>
using namespace std;
/**
* 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),
isEnabled(false),
done(false),
uas(NULL),
autopilotType(0),
systemType(0),
uasCanReverse(false),
rollAxis(-1),
pitchAxis(-1),
yawAxis(-1),
throttleAxis(-1),
joystickName(""),
joystickID(-1),
joystickNumButtons(0)
calibrationPositive[i] = sdlJoystickMax;
calibrationNegative[i] = sdlJoystickMin;
}
// Make sure we initialize with the correct UAS.
setActiveUAS(UASManager::instance()->getActiveUAS());
// Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected.
start();
oberion
committed
JoystickInput::~JoystickInput()
{
storeGeneralSettings();
storeJoystickSettings();
void JoystickInput::loadGeneralSettings()
{
// Load defaults from settings
QSettings settings;
settings.sync();
// Deal with settings specific to the JoystickInput
settings.beginGroup("JOYSTICK_INPUT");
isEnabled = settings.value("ENABLED", false).toBool();
joystickName = settings.value("JOYSTICK_NAME", "").toString();
settings.endGroup();
}
/**
* @brief Restores settings for the current joystick from saved settings file.
* Assumes that both joystickName & joystickNumAxes are correct.
*/
void JoystickInput::loadJoystickSettings()
// Load defaults from settings
QSettings settings;
settings.sync();
// Now for the current joystick
settings.beginGroup(joystickName);
rollAxis = (settings.value("ROLL_AXIS_MAPPING", -1).toInt());
pitchAxis = (settings.value("PITCH_AXIS_MAPPING", -1).toInt());
yawAxis = (settings.value("YAW_AXIS_MAPPING", -1).toInt());
throttleAxis = (settings.value("THROTTLE_AXIS_MAPPING", -1).toInt());
// Clear out and then restore the (AUTOPILOT, SYSTEM) mapping for joystick settings
joystickSettings.clear();
int autopilots = settings.beginReadArray("AUTOPILOTS");
for (int i = 0; i < autopilots; i++)
{
settings.setArrayIndex(i);
int autopilotType = settings.value("AUTOPILOT_TYPE", 0).toInt();
int systems = settings.beginReadArray("SYSTEMS");
for (int j = 0; j < systems; j++)
{
settings.setArrayIndex(j);
int systemType = settings.value("SYSTEM_TYPE", 0).toInt();
// Now that both the autopilot and system type are available, update some references.
QMap<int, bool>* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted;
QMap<int, bool>* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited;
QMap<int, int>* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions;
// Read back the joystickAxesInverted QList one element at a time.
int axesStored = settings.beginReadArray("AXES_INVERTED");
for (int k = 0; k < axesStored; k++)
{
settings.setArrayIndex(k);
int index = settings.value("INDEX", 0).toInt();
bool inverted = settings.value("INVERTED", false).toBool();
joystickAxesInverted->insert(index, inverted);
}
settings.endArray();
// Read back the joystickAxesLimited QList one element at a time.
axesStored = settings.beginReadArray("AXES_LIMITED");
for (int k = 0; k < axesStored; k++)
{
settings.setArrayIndex(k);
int index = settings.value("INDEX", 0).toInt();
bool limited = settings.value("LIMITED", false).toBool();
joystickAxesLimited->insert(index, limited);
}
settings.endArray();
// Read back the button->action mapping.
int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS");
for (int k = 0; k < buttonsStored; k++)
{
settings.setArrayIndex(k);
int index = settings.value("INDEX", 0).toInt();
int action = settings.value("ACTION", 0).toInt();
joystickButtonActions->insert(index, action);
}
settings.endArray();
}
settings.endArray();
}
settings.endArray();
settings.endGroup();
emit joystickSettingsChanged();
}
void JoystickInput::storeGeneralSettings() const
{
QSettings settings;
settings.beginGroup("JOYSTICK_INPUT");
settings.setValue("ENABLED", isEnabled);
settings.setValue("JOYSTICK_NAME", joystickName);
settings.endGroup();
settings.sync();
void JoystickInput::storeJoystickSettings() const
// Store settings
QSettings settings;
settings.beginGroup(joystickName);
settings.setValue("ROLL_AXIS_MAPPING", rollAxis);
settings.setValue("PITCH_AXIS_MAPPING", pitchAxis);
settings.setValue("YAW_AXIS_MAPPING", yawAxis);
settings.setValue("THROTTLE_AXIS_MAPPING", throttleAxis);
settings.beginWriteArray("AUTOPILOTS");
QMapIterator<int, QMap<int, JoystickSettings> > i(joystickSettings);
int autopilotIndex = 0;
while (i.hasNext())
{
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
i.next();
settings.setArrayIndex(autopilotIndex++);
int autopilotType = i.key();
settings.setValue("AUTOPILOT_TYPE", autopilotType);
settings.beginWriteArray("SYSTEMS");
QMapIterator<int, JoystickSettings> j(i.value());
int systemIndex = 0;
while (j.hasNext())
{
j.next();
settings.setArrayIndex(systemIndex++);
int systemType = j.key();
settings.setValue("SYSTEM_TYPE", systemType);
// Now that both the autopilot and system type are available, update some references.
QMapIterator<int, bool> joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted);
QMapIterator<int, bool> joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited);
QMapIterator<int, int> joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions);
settings.beginWriteArray("AXES_INVERTED");
int k = 0;
while (joystickAxesInverted.hasNext())
{
joystickAxesInverted.next();
int inverted = joystickAxesInverted.value();
// Only save this axes' inversion status if it's not the default
if (inverted)
{
settings.setArrayIndex(k++);
int index = joystickAxesInverted.key();
settings.setValue("INDEX", index);
settings.setValue("INVERTED", inverted);
}
}
settings.endArray();
settings.beginWriteArray("AXES_LIMITED");
k = 0;
while (joystickAxesLimited.hasNext())
{
joystickAxesLimited.next();
int limited = joystickAxesLimited.value();
if (limited)
{
settings.setArrayIndex(k++);
int index = joystickAxesLimited.key();
settings.setValue("INDEX", index);
settings.setValue("LIMITED", limited);
}
}
settings.endArray();
settings.beginWriteArray("BUTTONS_ACTIONS");
k = 0;
while (joystickButtonActions.hasNext())
{
joystickButtonActions.next();
int action = joystickButtonActions.value();
if (action != -1)
{
settings.setArrayIndex(k++);
int index = joystickButtonActions.key();
settings.setValue("INDEX", index);
settings.setValue("ACTION", action);
}
}
settings.endArray();
}
settings.endArray(); // SYSTEMS
}
settings.endArray(); // AUTOPILOTS
settings.endGroup();
settings.sync();
oberion
committed
}
// Do nothing if the UAS hasn't changed.
if (uas == this->uas)
{
return;
}
// Only connect / disconnect is the UAS is of a controllable UAS class
UAS* tmp = 0;
disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
uasCanReverse = false;
// Save any settings for the last UAS
if (joystickID > -1)
{
storeJoystickSettings();
}
if (this->uas && (tmp = dynamic_cast<UAS*>(this->uas)))
connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
uasCanReverse = tmp->systemCanReverse();
// Update the joystick settings for a new UAS.
autopilotType = uas->getAutopilotType();
systemType = uas->getSystemType();
}
// Make sure any UI elements know we've updated the UAS. The UASManager signal is re-emitted here so that UI elements know to
// update their UAS-specific UI.
emit activeUASSet(uas);
// Load any joystick-specific settings now that the UAS has changed.
if (joystickID > -1)
{
loadJoystickSettings();
void JoystickInput::setEnabled(bool enabled)
{
this->isEnabled = enabled;
storeJoystickSettings();
}
// Initialize SDL Joystick support and detect number of joysticks.
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();
// If no joysticks are connected, there's nothing we can do, so just keep
// going back to sleep every second unless the program quits.
while (!numJoysticks && !done)
QGC::SLEEP::sleep(1);
if (done)
{
return;
}
// Now that we've detected a joystick, load in the joystick-agnostic settings.
loadGeneralSettings();
// Enumerate all found devices
qDebug() << QString("%1 Input devices found:").arg(numJoysticks);
int activeJoystick = 0;
for(int i=0; i < numJoysticks; i++ )
QString name = SDL_JoystickName(i);
qDebug() << QString("\t%1").arg(name);
// If we've matched this joystick to what was last opened, note it.
// Note: The way this is implemented the LAST joystick of a given name will be opened.
if (name == joystickName)
{
activeJoystick = i;
}
SDL_Joystick* x = SDL_JoystickOpen(i);
qDebug() << QString("\tNumber of Axes: %1").arg(QString::number(SDL_JoystickNumAxes(x)));
qDebug() << QString("\tNumber of Buttons: %1").arg(QString::number(SDL_JoystickNumButtons(x)));
SDL_JoystickClose(x);
// Set the active joystick based on name, if a joystick was found in the saved settings, otherwise
// default to the first one.
setActiveJoystick(activeJoystick);
// Now make sure we know what the current UAS is and track changes to it.
setActiveUAS(UASManager::instance()->getActiveUAS());
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
void JoystickInput::shutdown()
{
done = true;
}
* 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.
oberion
committed
forever
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.
// Here we map the joystick axis value into the initial range of [0:1].
float axisValue = SDL_JoystickGetAxis(joystick, i);
if (joystickSettings[autopilotType][systemType].axesInverted[i])
{
axisValue = (axisValue - calibrationNegative[i]) / (calibrationPositive[i] - calibrationNegative[i]);
}
else
{
axisValue = (axisValue - calibrationPositive[i]) / (calibrationNegative[i] - calibrationPositive[i]);
}
axisValue = 1.0f - axisValue;
// For non-throttle axes or if the UAS can reverse, go ahead and convert this into the range [-1:1].
if (uasCanReverse || throttleAxis != i)
{
axisValue = axisValue * 2.0f - 1.0f;
}
// Otherwise if this vehicle can only go forward, but the axis is limited to only the positive range,
// scale this so the negative values are ignored for this axis and it's clamped to [0:1].
else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i))
{
axisValue = axisValue * 2.0f - 1.0f;
if (axisValue < 0.0f)
{
axisValue = 0.0f;
}
// Bound rounding errors
if (axisValue > 1.0f) axisValue = 1.0f;
if (axisValue < -1.0f) axisValue = -1.0f;
if (joystickAxes[i] != axisValue)
{
joystickAxes[i] = axisValue;
emit axisValueChanged(i, axisValue);
}
}
int hatPosition = SDL_JoystickGetHat(joystick, 0);
int newYHat = 0;
if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1;
if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1;
int newXHat = 0;
if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1;
if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1;
if (newYHat != yHat || newXHat != xHat)
{
xHat = newXHat;
yHat = newYHat;
emit hatDirectionChanged(newXHat, newYHat);
Bryant
committed
// Emit signals for each button individually
for (int i = 0; i < joystickNumButtons; i++)
Bryant
committed
// If the button was down, but now it's up, trigger a buttonPressed event
quint16 lastButtonState = joystickButtons & (1 << i);
Bryant
committed
if (SDL_JoystickGetButton(joystick, i) && !lastButtonState)
joystickButtons |= 1 << i;
Bryant
committed
}
else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState)
{
emit buttonReleased(i);
if (isEnabled && joystickSettings[autopilotType][systemType].buttonActions.contains(i))
{
emit actionTriggered(joystickSettings[autopilotType][systemType].buttonActions.value(i));
}
joystickButtons &= ~(1 << i);
Bryant
committed
// Now signal an update for all UI together.
if (isEnabled)
{
Bryant
committed
float roll = rollAxis > -1?joystickAxes[rollAxis]:numeric_limits<float>::quiet_NaN();
float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:numeric_limits<float>::quiet_NaN();
float yaw = yawAxis > -1?joystickAxes[yawAxis]:numeric_limits<float>::quiet_NaN();
float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:numeric_limits<float>::quiet_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 we already had a joystick, close that one before opening a new one.
if (joystick && SDL_JoystickOpened(joystickID))
{
storeJoystickSettings();
SDL_JoystickClose(joystick);
joystick = NULL;
joystickID = -1;
}
joystickID = id;
joystick = SDL_JoystickOpen(joystickID);
if (joystick && SDL_JoystickOpened(joystickID))
{
// Update joystick configuration.
joystickName = QString(SDL_JoystickName(joystickID));
joystickNumButtons = SDL_JoystickNumButtons(joystick);
joystickNumAxes = SDL_JoystickNumAxes(joystick);
// Restore saved settings for this joystick.
loadJoystickSettings();
// Update cached joystick axes values.
// Also emit any signals for currently-triggering events
joystickAxes.clear();
for (int i = 0; i < joystickNumAxes; i++)
Bryant
committed
joystickAxes.append(numeric_limits<float>::quiet_NaN());
// Update cached joystick button values.
// Emit signals for any button events.
joystickButtons = 0;
}
else
{
joystickNumButtons = 0;
joystickNumAxes = 0;
}
// Specify that a new joystick has been selected, so that any UI elements can update.
emit newJoystickSelected();
// And then trigger an update of this new UI.
emit joystickSettingsChanged();
}
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping)
{
switch (newMapping)
{
case JOYSTICK_INPUT_MAPPING_ROLL:
rollAxis = axis;
break;
case JOYSTICK_INPUT_MAPPING_PITCH:
pitchAxis = axis;
break;
case JOYSTICK_INPUT_MAPPING_YAW:
yawAxis = axis;
break;
case JOYSTICK_INPUT_MAPPING_THROTTLE:
throttleAxis = axis;
break;
case JOYSTICK_INPUT_MAPPING_NONE:
default:
if (rollAxis == axis)
{
rollAxis = -1;
}
if (pitchAxis == axis)
{
pitchAxis = -1;
}
if (yawAxis == axis)
{
yawAxis = -1;
}
if (throttleAxis == axis)
{
throttleAxis = -1;
joystickSettings[autopilotType][systemType].axesLimited.remove(axis);
}
break;
}
storeJoystickSettings();
}
void JoystickInput::setAxisInversion(int axis, bool inverted)
{
if (axis < joystickNumAxes)
{
joystickSettings[autopilotType][systemType].axesInverted[axis] = inverted;
storeJoystickSettings();
}
}
void JoystickInput::setAxisRangeLimit(int axis, bool limitRange)
{
if (axis < joystickNumAxes)
{
joystickSettings[autopilotType][systemType].axesLimited[axis] = limitRange;
storeJoystickSettings();
}
}
void JoystickInput::setButtonAction(int button, int action)
if (button < joystickNumButtons)
{
joystickSettings[autopilotType][systemType].buttonActions[button] = action;
storeJoystickSettings();
}
}
float JoystickInput::getCurrentValueForAxis(int axis) const
{
if (axis < joystickNumAxes)
{
return joystickAxes.at(axis);
}
return 0.0f;
bool JoystickInput::getInvertedForAxis(int axis) const
{
if (axis < joystickNumAxes)
{
return joystickSettings[autopilotType][systemType].axesInverted.value(axis);
}
return false;
}
bool JoystickInput::getRangeLimitForAxis(int axis) const
{
if (axis < joystickNumAxes)
{
return joystickSettings[autopilotType][systemType].axesLimited.value(axis);
}
return false;
}
int JoystickInput::getActionForButton(int button) const
{
if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button))
{
return joystickSettings[autopilotType][systemType].buttonActions.value(button);
}
return -1;
}