Skip to content
GitLab
Commit 400d6d2c authored by Jacob Walser's avatar Jacob Walser
Browse files

Joystick Hotplugging

parent a8240d22
Hide whitespace changes
Inline Side-by-side
src/Joystick/Joystick.cc
View file @ 400d6d2c
......@@ -71,9 +71,9 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
Joystick::~Joystick()
{
delete _rgAxisValues;
delete _rgCalibration;
delete _rgButtonValues;
delete[] _rgAxisValues;
delete[] _rgCalibration;
delete[] _rgButtonValues;
}
void Joystick::_loadSettings(void)
......@@ -286,7 +286,7 @@ void Joystick::run(void)
}
}
if (_calibrationMode != CalibrationModeCalibrating) {
if (_calibrationMode != CalibrationModeCalibrating && _calibrated) {
int axis = _rgFunctionAxis[rollFunction];
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
......@@ -406,6 +406,9 @@ void Joystick::startPolling(Vehicle* vehicle)
vehicle->setJoystickEnabled(false);
}
// Update qml in case of joystick transition
emit calibratedChanged(_calibrated);
// Only connect the new vehicle if it wants joystick data
if (vehicle->joystickEnabled()) {
_pollingStartedForCalibration = false;
......@@ -430,14 +433,15 @@ void Joystick::stopPolling(void)
if (_activeVehicle && _activeVehicle->joystickEnabled()) {
UAS* uas = _activeVehicle->uas();
// Neutral attitude controls
// emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
}
// FIXME: ****
//disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
_exitThread = true;
}
}
}
void Joystick::setCalibration(int axis, Calibration_t& calibration)
......
src/Joystick/Joystick.h
View file @ 400d6d2c
......@@ -88,6 +88,11 @@ public:
QVariantList buttonActions(void);
QString name(void) { return _name; }
// Joystick index used by sdl library
// Settable because sdl library remaps indicies after certain events
virtual int index(void) = 0;
virtual void setIndex(int index) = 0;
int throttleMode(void);
void setThrottleMode(int mode);
......
src/Joystick/JoystickManager.cc
View file @ 400d6d2c
......@@ -55,10 +55,22 @@ void JoystickManager::setToolbox(QGCToolbox *toolbox)
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
}
void JoystickManager::discoverJoysticks()
void JoystickManager::init()
{
if (JoystickSDL::init()) {
_setActiveJoystickFromSettings();
connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
_joystickCheckTimer.start(250);
}
}
void JoystickManager::_setActiveJoystickFromSettings(void)
{
QMap<QString,Joystick*> newMap;
#ifdef __sdljoystick__
_name2JoystickMap = JoystickSDL::discover(_multiVehicleManager);
// Get the latest joystick mapping
newMap = JoystickSDL::discover(_multiVehicleManager);
#elif defined(__android__)
/*
* Android Joystick not yet supported
......@@ -66,16 +78,30 @@ void JoystickManager::discoverJoysticks()
*/
#endif
if (!_name2JoystickMap.count()) {
qCDebug(JoystickManagerLog) << "\tnone found";
return;
// Check to see if our current mapping contains any joysticks that are not in the new mapping
// If so, those joysticks have been unplugged, and need to be cleaned up
QMap<QString, Joystick*>::iterator i;
for (i = _name2JoystickMap.begin(); i != _name2JoystickMap.end(); ++i) {
if (!newMap.contains(i.key())) {
qCDebug(JoystickManagerLog) << "Releasing joystick:" << i.key();
if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
qCDebug(JoystickManagerLog) << "\twas active";
_activeJoystick->stopPolling();
_activeJoystick = NULL;
}
// FIXME: Don't leave this hanging! We need to free the object.
//i.value()->deleteLater();
}
}
_setActiveJoystickFromSettings();
}
_name2JoystickMap = newMap;
emit availableJoysticksChanged();
void JoystickManager::_setActiveJoystickFromSettings(void)
{
if (!_name2JoystickMap.count()) {
setActiveJoystick(NULL);
return;
}
QSettings settings;
settings.beginGroup(_settingsGroup);
......@@ -97,23 +123,30 @@ Joystick* JoystickManager::activeJoystick(void)
void JoystickManager::setActiveJoystick(Joystick* joystick)
{
QSettings settings;
if (!_name2JoystickMap.contains(joystick->name())) {
if (joystick != NULL && !_name2JoystickMap.contains(joystick->name())) {
qCWarning(JoystickManagerLog) << "Set active not in map" << joystick->name();
return;
}
if (_activeJoystick == joystick) {
return;
}
if (_activeJoystick) {
_activeJoystick->stopPolling();
}
_activeJoystick = joystick;
settings.beginGroup(_settingsGroup);
settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
emit activeJoystickChanged(_activeJoystick);
emit activeJoystickNameChanged(_activeJoystick->name());
if (_activeJoystick != NULL) {
qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
settings.beginGroup(_settingsGroup);
settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
emit activeJoystickChanged(_activeJoystick);
emit activeJoystickNameChanged(_activeJoystick->name());
}
}
QVariantList JoystickManager::joysticks(void)
......@@ -146,3 +179,25 @@ void JoystickManager::setActiveJoystickName(const QString& name)
setActiveJoystick(_name2JoystickMap[name]);
}
void JoystickManager::_updateAvailableJoysticks(void)
{
SDL_Event event;
while (SDL_PollEvent(&event)) {
switch(event.type) {
case SDL_QUIT:
qCDebug(JoystickManagerLog) << "SDL ERROR:" << SDL_GetError();
break;
case SDL_JOYDEVICEADDED:
qCDebug(JoystickManagerLog) << "Joystick added:" << event.jdevice.which;
_setActiveJoystickFromSettings();
break;
case SDL_JOYDEVICEREMOVED:
qCDebug(JoystickManagerLog) << "Joystick removed:" << event.jdevice.which;
_setActiveJoystickFromSettings();
break;
default:
break;
}
}
}
src/Joystick/JoystickManager.h
View file @ 400d6d2c
......@@ -29,8 +29,8 @@ public:
~JoystickManager();
/// List of available joysticks
Q_PROPERTY(QVariantList joysticks READ joysticks CONSTANT)
Q_PROPERTY(QStringList joystickNames READ joystickNames CONSTANT)
Q_PROPERTY(QVariantList joysticks READ joysticks NOTIFY availableJoysticksChanged)
Q_PROPERTY(QStringList joystickNames READ joystickNames NOTIFY availableJoysticksChanged)
/// Active joystick
Q_PROPERTY(Joystick* activeJoystick READ activeJoystick WRITE setActiveJoystick NOTIFY activeJoystickChanged)
......@@ -49,13 +49,15 @@ public:
virtual void setToolbox(QGCToolbox *toolbox);
public slots:
void discoverJoysticks();
void init();
signals:
void activeJoystickChanged(Joystick* joystick);
void activeJoystickNameChanged(const QString& name);
void availableJoysticksChanged(void);
private slots:
void _updateAvailableJoysticks(void);
private:
void _setActiveJoystickFromSettings(void);
......@@ -67,6 +69,8 @@ private:
static const char * _settingsGroup;
static const char * _settingsKeyActiveJoystick;
QTimer _joystickCheckTimer;
};
#endif
src/Joystick/JoystickSDL.cc
View file @ 400d6d2c
......@@ -12,15 +12,21 @@ JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, in
{
}
QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
static QMap<QString, Joystick*> ret;
bool JoystickSDL::init(void) {
if (SDL_InitSubSystem(SDL_INIT_GAMECONTROLLER | SDL_INIT_JOYSTICK | SDL_INIT_NOPARACHUTE) < 0) {
SDL_JoystickEventState(SDL_ENABLE);
qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
return ret;
return false;
}
_loadGameControllerMappings();
return true;
}
QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) {
static QMap<QString, Joystick*> ret;
QMap<QString,Joystick*> newRet;
// Load available joysticks
......@@ -29,6 +35,7 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
for (int i=0; i<SDL_NumJoysticks(); i++) {
QString name = SDL_JoystickNameForIndex(i);
// TODO use GUID instead of name in case of two joysticks with same name
if (!ret.contains(name)) {
int axisCount, buttonCount, hatCount;
bool isGameController;
......@@ -57,11 +64,24 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
}
qCDebug(JoystickLog) << "\t" << name << "axes:" << axisCount << "buttons:" << buttonCount << "hats:" << hatCount << "isGC:" << isGameController;
ret[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
} else {
newRet[name] = ret[name];
if (newRet[name]->index() != i) {
newRet[name]->setIndex(i); // This joystick index has been remapped by SDL
}
// Anything left in ret after we exit the loop has been removed (unplugged) and needs to be cleaned up.
// We will handle that in JoystickManager in case the removed joystick was in use.
ret.remove(name);
qCDebug(JoystickLog) << "\tSkipping duplicate" << name;
}
}
if (!newRet.count()) {
qCDebug(JoystickLog) << "\tnone found";
}
ret = newRet;
return ret;
}
......@@ -93,11 +113,32 @@ bool JoystickSDL::_open(void) {
return false;
}
qCDebug(JoystickLog) << "Opened joystick at" << sdlJoystick;
return true;
}
void JoystickSDL::_close(void) {
SDL_JoystickClose(sdlJoystick);
if (sdlJoystick == NULL) {
qCDebug(JoystickLog) << "Attempt to close null joystick!";
return;
}
qCDebug(JoystickLog) << "Closing" << SDL_JoystickName(sdlJoystick) << "at" << sdlJoystick;
// We get a segfault if we try to close a joystick that has been detached
if (SDL_JoystickGetAttached(sdlJoystick) == SDL_FALSE) {
qCDebug(JoystickLog) << "\tJoystick is not attached!";
} else {
if (SDL_JoystickInstanceID(sdlJoystick) != -1) {
qCDebug(JoystickLog) << "\tID:" << SDL_JoystickInstanceID(sdlJoystick);
SDL_JoystickClose(sdlJoystick);
}
}
sdlJoystick = NULL;
sdlController = NULL;
}
bool JoystickSDL::_update(void)
......@@ -131,4 +172,3 @@ uint8_t JoystickSDL::_getHat(int hat,int i) {
}
return 0;
}
src/Joystick/JoystickSDL.h
View file @ 400d6d2c
......@@ -13,6 +13,10 @@ public:
JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager);
static QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager);
static bool init(void);
int index(void) { return _index; }
void setIndex(int index) { _index = index; }
private:
static void _loadGameControllerMappings();
......
src/QGCApplication.cc
View file @ 400d6d2c
......@@ -429,8 +429,8 @@ bool QGCApplication::_initForNormalAppBoot(void)
// Load known link configurations
toolbox()->linkManager()->loadLinkConfigurationList();
// Probe for joysticks - TODO: manage on a timer or use events to deal with hotplug
toolbox()->joystickManager()->discoverJoysticks();
// Probe for joysticks
toolbox()->joystickManager()->init();
if (_settingsUpgraded) {
settings.clear();
......
src/Vehicle/Vehicle.cc
View file @ 400d6d2c
......@@ -141,6 +141,8 @@ Vehicle::Vehicle(LinkInterface* link,
{
_addLink(link);
connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_activeJoystickChanged);
_mavlink = qgcApp()->toolbox()->mavlinkProtocol();
connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived);
......@@ -1295,6 +1297,12 @@ QStringList Vehicle::joystickModes(void)
return list;
}
void Vehicle::_activeJoystickChanged(void)
{
_loadSettings();
_startJoystick(true);
}
bool Vehicle::joystickEnabled(void)
{
return _joystickEnabled;
......
src/Vehicle/Vehicle.h
View file @ 400d6d2c
......@@ -690,6 +690,8 @@ private slots:
void _newGeoFenceAvailable(void);
void _sendMavCommandAgain(void);
void _activeJoystickChanged(void);
private:
bool _containsLink(LinkInterface* link);
void _addLink(LinkInterface* link);
......
src/VehicleSetup/JoystickConfig.qml
View file @ 400d6d2c
......@@ -27,6 +27,16 @@ SetupPage {
pageName: qsTr("Joystick")
pageDescription: qsTr("Joystick Setup is used to configure a calibrate joysticks.")
Connections {
target: joystickManager
onAvailableJoysticksChanged: {
if( joystickManager.joysticks.length == 0 ) {
summaryButton.checked = true
setupView.showSummaryPanel()
}
}
}
Component {
id: pageComponent
......@@ -358,11 +368,20 @@ SetupPage {
QGCCheckBox {
enabled: checked || _activeJoystick.calibrated
id: enabledCheckBox
enabled: _activeJoystick.calibrated
text: _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)")
checked: _activeVehicle.joystickEnabled
onClicked: _activeVehicle.joystickEnabled = checked
Connections {
target: joystickManager
onActiveJoystickChanged: {
enabledCheckBox.checked = Qt.binding(function() { return _activeJoystick.calibrated && _activeVehicle.joystickEnabled })
}
}
}
Row {
......@@ -390,6 +409,18 @@ SetupPage {
joystickCombo.currentIndex = index
}
}
Connections {
target: joystickManager
onAvailableJoysticksChanged: {
var index = joystickCombo.find(joystickManager.activeJoystickName)
if (index == -1) {
console.warn(qsTr("Active joystick name not in combo"), joystickManager.activeJoystickName)
} else {
joystickCombo.currentIndex = index
}
}
}
}
}
......
src/VehicleSetup/JoystickConfigController.cc
View file @ 400d6d2c
......@@ -69,7 +69,9 @@ void JoystickConfigController::start(void)
JoystickConfigController::~JoystickConfigController()
{
_activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
if(_activeJoystick) {
_activeJoystick->stopCalibrationMode(Joystick::CalibrationModeMonitor);
}
}
/// @brief Returns the state machine entry for the specified state.
......@@ -491,9 +493,10 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
int paramAxis;
paramAxis = joystick->getFunctionAxis((Joystick::AxisFunction_t)function);
_rgFunctionAxisMapping[function] = paramAxis;
_rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
if(paramAxis >= 0) {
_rgFunctionAxisMapping[function] = paramAxis;
_rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
}
}
_signalAllAttiudeValueChanges();
......@@ -809,9 +812,11 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
if (_activeJoystick) {
joystickTransition = true;
disconnect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
delete _rgAxisInfo;
delete _axisValueSave;
delete _axisRawValue;
// This will reset _rgFunctionAxis values to -1 to prevent out-of-bounds accesses
_resetInternalCalibrationValues();
delete[] _rgAxisInfo;
delete[] _axisValueSave;
delete[] _axisRawValue;
_axisCount = 0;
_activeJoystick = NULL;
}
......@@ -826,6 +831,7 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
_rgAxisInfo = new struct AxisInfo[_axisCount];
_axisValueSave = new int[_axisCount];
_axisRawValue = new int[_axisCount];
_setInternalCalibrationValuesFromSettings();
connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment