From e93d3d1de4e3f542489a21075d714ba88fed8112 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 22 Nov 2015 10:23:16 +0100
Subject: [PATCH] Joystick: Fix calibration and default use: Do not send
 anything to vehicle until enabled. Enable after calibration by default.

---
 src/Joystick/Joystick.cc                     | 54 ++++++++++++--------
 src/VehicleSetup/JoystickConfigController.cc |  7 ++-
 2 files changed, 38 insertions(+), 23 deletions(-)

diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc
index 9c10ca70b..7c417a996 100644
--- a/src/Joystick/Joystick.cc
+++ b/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") {
diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc
index f6003e6f5..96beec806 100644
--- a/src/VehicleSetup/JoystickConfigController.cc
+++ b/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
-- 
2.22.0