Merge pull request #4477 from bluerobotics/hotplugging

Joystick Hotplugging

src/Joystick/Joystick.cc

@@ -81,9 +81,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::_setDefaultCalibration(void) {
@@ -410,7 +410,7 @@ void Joystick::run(void)
             }
         }
 
-        if (_calibrationMode != CalibrationModeCalibrating) {
+        if (_calibrationMode != CalibrationModeCalibrating && _calibrated) {
             int     axis = _rgFunctionAxis[rollFunction];
             float   roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
 
@@ -530,6 +530,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;
@@ -554,14 +557,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

@@ -95,6 +95,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;
 
 	virtual bool requiresCalibration(void) { return true; }
 

src/Joystick/JoystickManager.cc

@@ -48,17 +48,35 @@ JoystickManager::~JoystickManager() {
 
 void JoystickManager::setToolbox(QGCToolbox *toolbox)
 {
-   QGCTool::setToolbox(toolbox);
+    QGCTool::setToolbox(toolbox);
 
     _multiVehicleManager = _toolbox->multiVehicleManager();
 
     QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
 }
 
-void JoystickManager::discoverJoysticks()
+void JoystickManager::init()
 {
 #ifdef __sdljoystick__
-    _name2JoystickMap = JoystickSDL::discover(_multiVehicleManager);
+    if (JoystickSDL::init()) {
+        _setActiveJoystickFromSettings();
+        connect(&_joystickCheckTimer, &QTimer::timeout, this, &JoystickManager::_updateAvailableJoysticks);
+        _joystickCheckTimer.start(250);
+    }
+#elif defined(__android__)
+    /*
+     * Android Joystick not yet supported
+     */
+#endif
+}
+
+void JoystickManager::_setActiveJoystickFromSettings(void)
+{
+    QMap<QString,Joystick*> newMap;
+
+#ifdef __sdljoystick__
+    // Get the latest joystick mapping
+    newMap = JoystickSDL::discover(_multiVehicleManager);
 #elif defined(__android__)
     /*
      * Android Joystick not yet supported
@@ -66,16 +84,31 @@ void JoystickManager::discoverJoysticks()
      */
 #endif
 
+    if (_activeJoystick && !newMap.contains(_activeJoystick->name())) {
+        qCDebug(JoystickManagerLog) << "Active joystick removed";
+        setActiveJoystick(NULL);
+    }
+
+    // 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();
+            i.value()->stopPolling();
+            i.value()->wait(1000);
+            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);
@@ -97,23 +130,31 @@ 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());
-    
+    if (_activeJoystick != NULL) {
+        qCDebug(JoystickManagerLog) << "Set active:" << _activeJoystick->name();
+
+        settings.beginGroup(_settingsGroup);
+        settings.setValue(_settingsKeyActiveJoystick, _activeJoystick->name());
+    }
+
     emit activeJoystickChanged(_activeJoystick);
-    emit activeJoystickNameChanged(_activeJoystick->name());
+    emit activeJoystickNameChanged(_activeJoystick?_activeJoystick->name():"");
 }
 
 QVariantList JoystickManager::joysticks(void)
@@ -146,3 +187,31 @@ void JoystickManager::setActiveJoystickName(const QString& name)
     
     setActiveJoystick(_name2JoystickMap[name]);
 }
+
+void JoystickManager::_updateAvailableJoysticks(void)
+{
+#ifdef __sdljoystick__
+    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;
+        }
+    }
+#elif defined(__android__)
+    /*
+     * Android Joystick not yet supported
+     */
+#endif
+}

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

@@ -13,15 +13,21 @@ JoystickSDL::JoystickSDL(const QString& name, int axisCount, int buttonCount, in
     if(_isGameController) _setDefaultCalibration();
 }
 
-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
 
@@ -30,6 +36,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;
@@ -58,11 +65,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;
 }
 
@@ -94,11 +114,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)
@@ -132,4 +173,3 @@ uint8_t JoystickSDL::_getHat(int hat,int i) {
     }
     return 0;
 }
-

src/Joystick/JoystickSDL.h

@@ -12,7 +12,11 @@ class JoystickSDL : public Joystick
 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 QMap<QString, Joystick*> discover(MultiVehicleManager* _multiVehicleManager); 
+    static bool init(void);
+
+    int index(void) { return _index; }
+    void setIndex(int index) { _index = index; }
 
     // This can be uncommented to hide the calibration buttons for gamecontrollers in the future
     // bool requiresCalibration(void) final { return !_isGameController; }

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

@@ -143,6 +143,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);
@@ -1298,6 +1300,12 @@ QStringList Vehicle::joystickModes(void)
     return list;
 }
 
+void Vehicle::_activeJoystickChanged(void)
+{
+	_loadSettings();
+	_startJoystick(true);
+}
+
 bool Vehicle::joystickEnabled(void)
 {
     return _joystickEnabled;

src/Vehicle/Vehicle.h

@@ -701,6 +701,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
 
@@ -359,11 +369,22 @@ SetupPage {
 
 
                             QGCCheckBox {
-                                enabled:    checked || _activeJoystick.calibrated
-                                text:       _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)")
+                                id:         enabledCheckBox
+                                enabled:    _activeJoystick ? _activeJoystick.calibrated : false
+                                text:       _activeJoystick ? _activeJoystick.calibrated ? qsTr("Enable joystick input") : qsTr("Enable not allowed (Calibrate First)") : ""
                                 checked:    _activeVehicle.joystickEnabled
 
                                 onClicked:  _activeVehicle.joystickEnabled = checked
+
+                                Connections {
+                                    target: joystickManager
+
+                                    onActiveJoystickChanged: {
+                                        if(_activeJoystick) {
+                                            enabledCheckBox.checked = Qt.binding(function() { return _activeJoystick.calibrated && _activeVehicle.joystickEnabled })
+                                        }
+                                    }
+                                }
                             }
 
                             Row {
@@ -391,6 +412,16 @@ SetupPage {
                                             joystickCombo.currentIndex = index
                                         }
                                     }
+
+                                    Connections {
+                                        target: joystickManager
+                                        onAvailableJoysticksChanged: {
+                                            var index = joystickCombo.find(joystickManager.activeJoystickName)
+                                            if (index >= 0) {
+                                                joystickCombo.currentIndex = index
+                                            }
+                                        }
+                                    }
                                 }
                             }
 
@@ -403,7 +434,7 @@ SetupPage {
                                 QGCRadioButton {
                                     exclusiveGroup: throttleModeExclusiveGroup
                                     text:           qsTr("Center stick is zero throttle")
-                                    checked:        _activeJoystick.throttleMode == 0
+                                    checked:        _activeJoystick ? _activeJoystick.throttleMode == 0 : false
 
                                     onClicked: _activeJoystick.throttleMode = 0
                                 }
@@ -412,11 +443,11 @@ SetupPage {
                                     x:          20
                                     width:      parent.width
                                     spacing:    ScreenTools.defaultFontPixelWidth
-                                    visible:    _activeJoystick.throttleMode == 0
+                                    visible:    _activeJoystick ? _activeJoystick.throttleMode == 0 : false
 
                                     QGCCheckBox {
                                         id:         accumulator
-                                        checked:    _activeJoystick.accumulator
+                                        checked:    _activeJoystick ? _activeJoystick.accumulator : false
                                         text:       qsTr("Spring loaded throttle smoothing")
 
                                         onClicked:  _activeJoystick.accumulator = checked
@@ -426,7 +457,7 @@ SetupPage {
                                 QGCRadioButton {
                                     exclusiveGroup: throttleModeExclusiveGroup
                                     text:           qsTr("Full down stick is zero throttle")
-                                    checked:        _activeJoystick.throttleMode == 1
+                                    checked:        _activeJoystick ? _activeJoystick.throttleMode == 1 : false
 
                                     onClicked: _activeJoystick.throttleMode = 1
                                 }
@@ -437,7 +468,7 @@ SetupPage {
 
                                 QGCCheckBox {
                                     id:         exponential
-                                    checked:    _activeJoystick.exponential
+                                    checked:    _activeJoystick ? _activeJoystick.exponential : false
                                     text:       qsTr("Use exponential curve on roll, pitch, yaw")
 
                                     onClicked:  _activeJoystick.exponential = checked
@@ -526,7 +557,7 @@ SetupPage {
 
                             Repeater {
                                 id:     buttonActionRepeater
-                                model:  _activeJoystick.totalButtonCount
+                                model:  _activeJoystick ? _activeJoystick.totalButtonCount : 0
 
                                 Row {
                                     spacing: ScreenTools.defaultFontPixelWidth
@@ -536,7 +567,7 @@ SetupPage {
 
                                     QGCCheckBox {
                                         anchors.verticalCenter:     parent.verticalCenter
-                                        checked:                    _activeJoystick.buttonActions[modelData] != ""
+                                        checked:                    _activeJoystick ? _activeJoystick.buttonActions[modelData] != "" : false
 
                                         onClicked: _activeJoystick.setButtonAction(modelData, checked ? buttonActionCombo.textAt(buttonActionCombo.currentIndex) : "")
                                     }
@@ -562,7 +593,7 @@ SetupPage {
                                     QGCComboBox {
                                         id:             buttonActionCombo
                                         width:          ScreenTools.defaultFontPixelWidth * 20
-                                        model:          _activeJoystick.actions
+                                        model:          _activeJoystick ? _activeJoystick.actions : 0
 
                                         onActivated:            _activeJoystick.setButtonAction(modelData, textAt(index))
                                         Component.onCompleted:  currentIndex = find(_activeJoystick.buttonActions[modelData])
@@ -593,7 +624,7 @@ SetupPage {
 
                             Repeater {
                                 id:     jsButtonActionRepeater
-                                model:  _activeJoystick.totalButtonCount
+                                model:  _activeJoystick ? _activeJoystick.totalButtonCount : 0
 
                                 Row {
                                     spacing: ScreenTools.defaultFontPixelWidth
@@ -719,7 +750,7 @@ SetupPage {
 
                     Repeater {
                         id:     axisMonitorRepeater
-                        model:  _activeJoystick.axisCount
+                        model:  _activeJoystick ? _activeJoystick.axisCount : 0
                         width:  parent.width
 
                         Row {
@@ -771,7 +802,7 @@ SetupPage {
 
                         Repeater {
                             id:     buttonMonitorRepeater
-                            model:  _activeJoystick.totalButtonCount
+                            model:  _activeJoystick ? _activeJoystick.totalButtonCount : 0
 
                             Rectangle {
                                 width:          ScreenTools.defaultFontPixelHeight * 1.2

src/VehicleSetup/JoystickConfigController.cc

@@ -73,7 +73,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.
@@ -495,9 +497,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;
+        }
     }
 
     _transmitterMode = joystick->getTXMode();
@@ -849,9 +852,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;
     }
@@ -866,6 +871,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);
     }
 }