Merge pull request #2257 from mavlink/joystick_fix

Joystick fix: Enable joystick right after calibration & do not send anything to vehicle if not configured

src/Joystick/Joystick.cc

@@ -367,27 +367,30 @@ void Joystick::run(void)
 
 void Joystick::startPolling(Vehicle* vehicle)
 {
-    if (isRunning()) {
-        if (vehicle != _activeVehicle) {
-            // Joystick was previously disabled, but now enabled from config screen
-            
-            if (_calibrationMode == CalibrationModeOff) {
-                qWarning() << "Incorrect usage pattern";
-                return;
-            }
-            
-            _activeVehicle = vehicle;
-            _pollingStartedForCalibration = false;
+    if (vehicle) {
+
+        // If a vehicle is connected, disconnect it
+        if (_activeVehicle) {
+            UAS* uas = _activeVehicle->uas();
+            disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
         }
-    } else {
+
+        // Always set up the new vehicle
         _activeVehicle = vehicle;
-        
-        UAS* uas = _activeVehicle->uas();
-        
-        connect(this, &Joystick::manualControl,         uas, &UAS::setExternalControlSetpoint);
-        // FIXME: ****
-        //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
-        
+
+        // Only connect the new vehicle if it wants joystick data
+        if (vehicle->joystickEnabled()) {
+            _pollingStartedForCalibration = false;
+
+            UAS* uas = _activeVehicle->uas();
+            connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
+            // FIXME: ****
+            //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
+        }
+    }
+
+
+    if (!isRunning()) {
         _exitThread = false;
         start();
     }
@@ -396,9 +399,12 @@ void Joystick::startPolling(Vehicle* vehicle)
 void Joystick::stopPolling(void)
 {
     if (isRunning()) {
-        UAS* uas = _activeVehicle->uas();
-        
-        disconnect(this, &Joystick::manualControl,          uas, &UAS::setExternalControlSetpoint);
+
+        if (_activeVehicle && _activeVehicle->joystickEnabled()) {
+            UAS* uas = _activeVehicle->uas();
+
+            disconnect(this, &Joystick::manualControl,          uas, &UAS::setExternalControlSetpoint);
+        }
         // FIXME: ****
         //disconnect(this, &Joystick::buttonActionTriggered,  uas, &UAS::triggerAction);
         
@@ -544,6 +550,10 @@ void Joystick::stopCalibrationMode(CalibrationMode_t mode)
 
 void Joystick::_buttonAction(const QString& action)
 {
+    if (!_activeVehicle || !_activeVehicle->joystickEnabled()) {
+        return;
+    }
+
     if (action == "Arm") {
         _activeVehicle->setArmed(true);
     } else if (action == "Disarm") {

src/Vehicle/Vehicle.cc

@@ -838,6 +838,7 @@ void Vehicle::setJoystickEnabled(bool enabled)
     _joystickEnabled = enabled;
     _startJoystick(_joystickEnabled);
     _saveSettings();
+    emit joystickEnabledChanged(_joystickEnabled);
 }
 
 void Vehicle::_startJoystick(bool start)

src/VehicleSetup/JoystickConfig.qml

@@ -355,12 +355,10 @@ QGCView {
 
 
                         QGCCheckBox {
-                            enabled:    allowEnableDisable
+                            enabled:    _activeJoystick.calibrated
                             text:       _activeJoystick.calibrated ? "Enable joystick input" : "Enable/Disable not allowed (Calibrate First)"
                             checked:    _activeVehicle.joystickEnabled
 
-                            property bool allowEnableDisable:   _activeJoystick.calibrated
-
                             onClicked:  _activeVehicle.joystickEnabled = checked
                         }
 

src/VehicleSetup/JoystickConfigController.cc

@@ -99,7 +99,7 @@ const JoystickConfigController::stateMachineEntry* JoystickConfigController::_ge
     static const char* msgPitchDown =       "Move the Pitch stick all the way down and hold it there...";
     static const char* msgPitchUp =         "Move the Pitch stick all the way up and hold it there...";
     static const char* msgPitchCenter =     "Allow the Pitch stick to move back to center...";
-    static const char* msgComplete =        "All settings have been captured. Click Next to Save.";
+    static const char* msgComplete =        "All settings have been captured. Click Next to enable the joystick.";
     
     static const stateMachineEntry rgStateMachine[] = {
         //Function
@@ -564,6 +564,11 @@ void JoystickConfigController::_writeCalibration(void)
     
     _stopCalibration();
     _setInternalCalibrationValuesFromSettings();
+
+    Vehicle* vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
+    if (vehicle) {
+        vehicle->setJoystickEnabled(true);
+    }
 }
 
 /// @brief Starts the calibration process