Rework Vehicle Joystick Handling

This fixes a number of issues with the way joysticks were handled
in a multi vehicle context. I reworked the way vehicle determine
to enable and capture the joystick input, as well as seperating
setJoystickEnable from saveJoystick to seperate runtime and saved
state.

src/Joystick/Joystick.cc

@@ -124,6 +124,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
     _loadSettings();
     connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
+    connect(qgcApp()->toolbox()->multiVehicleManager()->vehicles(), &QmlObjectListModel::countChanged, this, &Joystick::_vehicleCountChanged);
 
     _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json");
 }
@@ -222,6 +223,16 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
     }
 }
 
+void Joystick::_vehicleCountChanged(int count)
+{
+    if(count == 0)
+    {
+        // then the last vehicle has been deleted
+        qCDebug(JoystickLog) << "Stopping joystick thread due to last active vehicle deletion";
+        this->stopPolling();
+    }
+}
+
 void Joystick::_loadSettings()
 {
     QSettings settings;

src/Joystick/Joystick.h

@@ -365,4 +365,6 @@ class Joystick : public QThread
 
 private slots:
     void _activeVehicleChanged(Vehicle* activeVehicle);
+    void _vehicleCountChanged(int count);
+
 };

src/Vehicle/Vehicle.cc

@@ -179,8 +179,7 @@ Vehicle::Vehicle(LinkInterface*             link,
 {
     _linkManager = _toolbox->linkManager();
 
-    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
-    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_activeVehicleAvailableChanged);
+    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadJoystickSettings);
     connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleChanged, this, &Vehicle::_activeVehicleChanged);
 
     _mavlink = _toolbox->mavlinkProtocol();
@@ -475,6 +474,9 @@ void Vehicle::_commonInit()
         _settingsManager->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceUDPH264);
         _settingsManager->videoSettings()->lowLatencyMode()->setRawValue(true);
     }
+
+    // enable Joystick if appropriate
+    _loadJoystickSettings();
 }
 
 Vehicle::~Vehicle()
@@ -489,10 +491,6 @@ Vehicle::~Vehicle()
 
     delete _mav;
     _mav = nullptr;
-
-    if (_joystickManager) {
-        _startJoystick(false);
-    }
 }
 
 void Vehicle::prepareDelete()
@@ -2079,41 +2077,43 @@ void Vehicle::resetErrorLevelMessages()
     }
 }
 
-void Vehicle::_loadSettings()
+// this function called in three cases:
+// 1. On constructor of vehicle, to see if we should enable a joystick
+// 2. When there is a new active joystick
+// 3. When the active joystick is disconnected (even if there isnt a new one)
+void Vehicle::_loadJoystickSettings()
 {
     QSettings settings;
     settings.beginGroup(QString(_settingsGroup).arg(_id));
-    // Joystick enabled is a global setting so first make sure there are any joysticks connected
-    if (_toolbox->joystickManager()->joysticks().count()) {
+
+    if (_toolbox->joystickManager()->activeJoystick()) {
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Notified of an active joystick. Loading setting joystickenabled: " << settings.value(_joystickEnabledSettingsKey, false).toBool();
         setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
-        _startJoystick(true);
-    }
-}
 
-void Vehicle::_activeVehicleAvailableChanged(bool isActiveVehicleAvailable)
-{
-    // if there is no longer an active vehicle, disconnect the joystick
-    if(!isActiveVehicleAvailable) {
-        setJoystickEnabled(false);
+        // Always capture the active joystick to the active vehicle
+        // _joystickEnable will determine if it does anything
+        if(_toolbox->multiVehicleManager()->activeVehicle() == this){
+            this->_captureJoystick();
+        }
     }
-}
-
-void Vehicle::_activeVehicleChanged(Vehicle *newActiveVehicle)
-{
-    if(newActiveVehicle == this) {
-        // this vehicle is the newly active vehicle
-        setJoystickEnabled(true);
+    else
+    {
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Notified that there is no active joystick";
+        setJoystickEnabled(false);
     }
 }
 
-void Vehicle::_saveSettings()
+// This is called from the UI when a deliberate action is taken to enable or disable the joystick
+// This save allows the joystick enable state to persist restarts, disconnections of the joystick etc
+void Vehicle::saveJoystickSettings()
 {
     QSettings settings;
     settings.beginGroup(QString(_settingsGroup).arg(_id));
 
     // The joystick enabled setting should only be changed if a joystick is present
     // since the checkbox can only be clicked if one is present
     if (_toolbox->joystickManager()->joysticks().count()) {
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Saving setting joystickenabled: " << _joystickEnabled;
         settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled);
     }
 }
@@ -2125,25 +2125,50 @@ bool Vehicle::joystickEnabled() const
 
 void Vehicle::setJoystickEnabled(bool enabled)
 {
+    if (enabled){
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Joystick Enabled";
+    }
+    else {
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Joystick Disabled";
+    }
+
+    // _joystickEnabled is the runtime state - it determines whether a vehicle is using joystick data when it is active
     _joystickEnabled = enabled;
-    _startJoystick(_joystickEnabled);
-    _saveSettings();
+
+    // if we are the active vehicle, call start polling on the active joystick
+    // This routes the joystick signals to this vehicle
+    // We do this even if we are disabling the joystick
+    // because it will trigger disconnection of the signals
+    if (_toolbox->multiVehicleManager()->activeVehicle() == this){
+        _captureJoystick();
+    }
+
     emit joystickEnabledChanged(_joystickEnabled);
 }
 
-void Vehicle::_startJoystick(bool start)
+void Vehicle::_activeVehicleChanged(Vehicle *newActiveVehicle)
+{
+    // the new active vehicle should always capture the joystick
+    // even if the new active vehicle has joystick disabled
+    // capturing the joystick will stop the joystick data going to the inactive vehicle
+    if (newActiveVehicle == this){
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " is the new active vehicle";
+        _captureJoystick();
+    }
+}
+
+// tells the active joystick where to send data
+void Vehicle::_captureJoystick()
 {
     Joystick* joystick = _joystickManager->activeJoystick();
-    if (joystick) {
-        if (start) {
-            joystick->startPolling(this);
-        } else {
-            joystick->stopPolling();
-            joystick->wait(500);
-        }
+
+    if(joystick){
+        qCDebug(VehicleLog) << "Vehicle " << this->id() << " Capture Joystick";
+        joystick->startPolling(this);
     }
 }
 
+
 QGeoCoordinate Vehicle::homePosition()
 {
     return _homePosition;

src/Vehicle/Vehicle.h

@@ -463,6 +463,9 @@ class Vehicle : public FactGroup
     /// Set home from flight map coordinate
     Q_INVOKABLE void doSetHome(const QGeoCoordinate& coord);
 
+    /// Save the joystick enable setting to the settings group
+    Q_INVOKABLE void saveJoystickSettings(void);
+
     bool    isInitialConnectComplete() const;
     bool    guidedModeSupported     () const;
     bool    pauseVehicleSupported   () const;
@@ -1031,11 +1034,9 @@ private slots:
     void _gotProgressUpdate                 (float progressValue);
 
 private:
-    void _loadSettings                  ();
-    void _activeVehicleAvailableChanged (bool isActiveVehicleAvailable);
+    void _loadJoystickSettings          ();
     void _activeVehicleChanged          (Vehicle* newActiveVehicle);
-    void _saveSettings                  ();
-    void _startJoystick                 (bool start);
+    void _captureJoystick               ();
     void _handlePing                    (LinkInterface* link, mavlink_message_t& message);
     void _handleHomePosition            (mavlink_message_t& message);
     void _handleHeartbeat               (mavlink_message_t& message);

src/VehicleSetup/JoystickConfigController.cc

@@ -538,6 +538,7 @@ void JoystickConfigController::_writeCalibration()
     Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
     if (vehicle) {
         vehicle->setJoystickEnabled(true);
+        vehicle->saveJoystickSettings();
     }
 }
 

src/VehicleSetup/JoystickConfigGeneral.qml

@@ -42,7 +42,10 @@ Item {
             QGCCheckBox {
                 id:             enabledSwitch
                 enabled:        _activeJoystick ? _activeJoystick.calibrated : false
-                onClicked:      globals.activeVehicle.joystickEnabled = checked
+                onClicked:      {
+                    globals.activeVehicle.joystickEnabled = checked
+                    globals.activeVehicle.saveJoystickSettings()
+                }
                 Component.onCompleted: {
                     checked = globals.activeVehicle.joystickEnabled
                 }