Joystick Hotplugging

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)

src/Joystick/Joystick.h

@@ -89,6 +89,11 @@ public:
 
     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

@@ -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
 
+    // 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();
+        }
+    }
+
+    _name2JoystickMap = newMap;
+    emit availableJoysticksChanged();
+
     if (!_name2JoystickMap.count()) {
-        qCDebug(JoystickManagerLog) << "\tnone found";
+        setActiveJoystick(NULL);
         return;
     }
 	
-    _setActiveJoystickFromSettings();
-}
-
-void JoystickManager::_setActiveJoystickFromSettings(void)
-{
     QSettings settings;
     
     settings.beginGroup(_settingsGroup);
@@ -98,22 +124,29 @@ 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());
+    if (_activeJoystick != NULL) {
+        qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
 
-    emit activeJoystickChanged(_activeJoystick);
-    emit activeJoystickNameChanged(_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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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);
     }
 }