diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 17ce67b478e49a66e087359aeef9597b0df59c3f..f86dbab980599262fad1fc9136b13b5ca2d48b9f 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -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) diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index c9915eae20d1c7d6d29600948e7e4493cbc7bbc7..8b8b52490e71d17652a3c62c389802f05649bedd 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -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); diff --git a/src/Joystick/JoystickManager.cc b/src/Joystick/JoystickManager.cc index a8d3f6f936e54d1522d736b9b01b9f9e5386c1a7..0e3fa6a2d23dfd62d8bc45355b4a4220d59d2236 100644 --- a/src/Joystick/JoystickManager.cc +++ b/src/Joystick/JoystickManager.cc @@ -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 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::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; + } + } +} diff --git a/src/Joystick/JoystickManager.h b/src/Joystick/JoystickManager.h index 8460b752f8b6346e4c45d2e63000c5ed8ce2d596..98a3d09ee2c0f58d3101e5dadd55a77291296b3e 100644 --- a/src/Joystick/JoystickManager.h +++ b/src/Joystick/JoystickManager.h @@ -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 diff --git a/src/Joystick/JoystickSDL.cc b/src/Joystick/JoystickSDL.cc index 303c3005c00c6ff659cecff0852e897b72141e4b..4e074e923c3ae3e889b97de6fd099404d5a7affd 100644 --- a/src/Joystick/JoystickSDL.cc +++ b/src/Joystick/JoystickSDL.cc @@ -12,15 +12,21 @@ JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, in { } -QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { - static QMap 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 JoystickSDL::discover(MultiVehicleManager* _multiVehicleManager) { + static QMap ret; + + QMap newRet; // Load available joysticks @@ -29,6 +35,7 @@ QMap JoystickSDL::discover(MultiVehicleManager* _multiVehicl for (int i=0; i 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; } - diff --git a/src/Joystick/JoystickSDL.h b/src/Joystick/JoystickSDL.h index 2c4a9ca6aeb93fae1880f4f8b3e266604c181eb3..e8111a79b9a0ed0d908f574c09d461a4678ebe34 100644 --- a/src/Joystick/JoystickSDL.h +++ b/src/Joystick/JoystickSDL.h @@ -13,6 +13,10 @@ public: JoystickSDL(const QString& name, int axisCount, int buttonCount, int hatCount, int index, bool isGameController, MultiVehicleManager* multiVehicleManager); static QMap discover(MultiVehicleManager* _multiVehicleManager); + static bool init(void); + + int index(void) { return _index; } + void setIndex(int index) { _index = index; } private: static void _loadGameControllerMappings(); diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index d436f683248a7199b67b1fb290b69720287ca6d9..6a7de14ab036040e38c36c4afb74cc5b4a58b0b9 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -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(); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 10c7deee44222ad43d2d4f07d893497c3f735bfe..6c7df9c29faa478a909f94c1350fff87f89533e3 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -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; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c9d2c01140dc14871dfec21d9502cc1b375f12d2..c291ea210ba11db3f44ea9eef0e544c8c2c9edab 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -690,6 +690,8 @@ private slots: void _newGeoFenceAvailable(void); void _sendMavCommandAgain(void); + void _activeJoystickChanged(void); + private: bool _containsLink(LinkInterface* link); void _addLink(LinkInterface* link); diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index 503c6be53598527acd296f3b319db50ffa7afe3c..58069ffa76dd9ad3c218a670127b83364a848d46 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -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 + } + } + } } } diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc index 505478bd73107da3be0fe2dba60b4ea689a059d8..8111896d96dcef5f0f1b3985b9debf92e680fd45 100644 --- a/src/VehicleSetup/JoystickConfigController.cc +++ b/src/VehicleSetup/JoystickConfigController.cc @@ -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); } }