Allow changing/saving joystick TX mode

src/Joystick/Joystick.cc

@@ -25,6 +25,7 @@ const char* Joystick::_throttleModeSettingsKey =    "ThrottleMode";
 const char* Joystick::_exponentialSettingsKey =     "Exponential";
 const char* Joystick::_accumulatorSettingsKey =     "Accumulator";
 const char* Joystick::_deadbandSettingsKey =        "Deadband";
+const char* Joystick::_txModeSettingsKey =          "TXMode";
 
 const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
     "RollAxis",
@@ -33,6 +34,8 @@ const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
     "ThrottleAxis"
 };
 
+int Joystick::_transmitterMode = 2;
+
 Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager)
     : _exitThread(false)
     , _name(name)
@@ -81,6 +84,9 @@ void Joystick::_loadSettings(void)
     QSettings   settings;
 
     settings.beginGroup(_settingsGroup);
+
+    _transmitterMode = settings.value(_txModeSettingsKey, 2).toInt();
+
     settings.beginGroup(_name);
 
     bool badSettings = false;
@@ -136,6 +142,10 @@ void Joystick::_loadSettings(void)
         qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings;
     }
 
+    // FunctionAxis mappings are always stored in TX mode 2
+    // Remap to stored TX mode in settings
+    _remapAxes(2, _transmitterMode, _rgFunctionAxis);
+
     for (int button=0; button<_totalButtonCount; button++) {
         _rgButtonActions << settings.value(QString(_buttonActionSettingsKey).arg(button), QString()).toString();
         qCDebug(JoystickLog) << "_loadSettings button:action" << button << _rgButtonActions[button];
@@ -152,6 +162,11 @@ void Joystick::_saveSettings(void)
     QSettings settings;
 
     settings.beginGroup(_settingsGroup);
+
+    // Transmitter mode is static
+    // Save the mode we are using
+    settings.setValue(_txModeSettingsKey, _transmitterMode);
+
     settings.beginGroup(_name);
 
     settings.setValue(_calibratedSettingsKey, _calibrated);
@@ -160,7 +175,7 @@ void Joystick::_saveSettings(void)
     settings.setValue(_deadbandSettingsKey, _deadband);
     settings.setValue(_throttleModeSettingsKey, _throttleMode);
 
-    qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband" << _calibrated << _throttleMode << _deadband;
+    qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _transmitterMode;
 
     QString minTpl  ("Axis%1Min");
     QString maxTpl  ("Axis%1Max");
@@ -187,8 +202,13 @@ void Joystick::_saveSettings(void)
                                 << calibration->deadband;
     }
 
+    // Always save function Axis mappings in TX Mode 2
+    // Write mode 2 mappings without changing mapping currently in use
+    int temp[maxFunction];
+    _remapAxes(_transmitterMode, 2, temp);
+
     for (int function=0; function<maxFunction; function++) {
-        settings.setValue(_rgFunctionSettingsKey[function], _rgFunctionAxis[function]);
+        settings.setValue(_rgFunctionSettingsKey[function], temp[function]);
         qCDebug(JoystickLog) << "_saveSettings name:function:axis" << _name << function << _rgFunctionSettingsKey[function];
     }
 
@@ -198,6 +218,42 @@ void Joystick::_saveSettings(void)
     }
 }
 
+// Relative mappings of axis functions between different TX modes
+int Joystick::_mapFunctionMode(int mode, int function) {
+
+    static const int mapping[][4] = {
+        { 2, 1, 0, 3 },
+        { 2, 3, 0, 1 },
+        { 0, 1, 2, 3 },
+        { 0, 3, 2, 1 }};
+
+    return mapping[mode-1][function];
+}
+
+// Remap current axis functions from current TX mode to new TX mode
+void Joystick::_remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]) {
+    int temp[maxFunction];
+
+    for(int function = 0; function < maxFunction; function++) {
+        temp[_mapFunctionMode(newMode, function)] = _rgFunctionAxis[_mapFunctionMode(currentMode, function)];
+    }
+
+    for(int function = 0; function < maxFunction; function++) {
+        newMapping[function] = temp[function];
+    }
+
+}
+
+void Joystick::setTXMode(int mode) {
+    if(mode > 0 && mode <= 4) {
+        _remapAxes(_transmitterMode, mode, _rgFunctionAxis);
+        _transmitterMode = mode;
+        _saveSettings();
+    } else {
+        qCWarning(JoystickLog) << "Invalid mode:" << mode;
+    }
+}
+
 /// Adjust the raw axis value to the -1:1 range given calibration information
 float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDeadbands)
 {
@@ -471,6 +527,7 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
 
     _calibrated = true;
     _rgFunctionAxis[function] = axis;
+
     _saveSettings();
     emit calibratedChanged(_calibrated);
 }

src/Joystick/Joystick.h

@@ -101,6 +101,9 @@ public:
     bool deadband(void);
     void setDeadband(bool accu);
 
+    void setTXMode(int mode);
+    int getTXMode(void) { return _transmitterMode; }
+
     typedef enum {
         CalibrationModeOff,         // Not calibrating
         CalibrationModeMonitor,     // Monitors are active, continue to send to vehicle if already polling
@@ -157,6 +160,9 @@ private:
     virtual int _getAxis(int i) = 0;
     virtual uint8_t _getHat(int hat,int i) = 0;
 
+    int _mapFunctionMode(int mode, int function);
+    void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]);
+
     // Override from QThread
     virtual void run(void);
 
@@ -172,6 +178,7 @@ protected:
     int     _hatButtonCount;
     int     _totalButtonCount;
 
+    static int          _transmitterMode;
     CalibrationMode_t   _calibrationMode;
 
     int*                _rgAxisValues;
@@ -203,6 +210,7 @@ private:
     static const char* _exponentialSettingsKey;
     static const char* _accumulatorSettingsKey;
     static const char* _deadbandSettingsKey;
+    static const char* _txModeSettingsKey;
 };
 
 #endif

src/VehicleSetup/JoystickConfigController.cc

@@ -499,6 +499,8 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
         _rgFunctionAxisMapping[function] = paramAxis;
         _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function;
     }
+
+    _transmitterMode = joystick->getTXMode();
     
     _signalAllAttiudeValueChanges();
 }
@@ -806,11 +808,14 @@ bool JoystickConfigController::throttleAxisReversed(void)
 
 void JoystickConfigController::setTransmitterMode(int mode)
 {
-    if (mode == 1 || mode == 2 || mode == 3 || mode == 4) {
+    if (mode > 0 && mode <= 4) {
         _transmitterMode = mode;
         if (_currentStep != -1) {
             const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
             _setHelpImage(state->image);
+        } else {
+            _activeJoystick->setTXMode(mode);
+            _setInternalCalibrationValuesFromSettings();
         }
     }
 }
@@ -831,6 +836,8 @@ void JoystickConfigController::_signalAllAttiudeValueChanges(void)
     emit pitchAxisDeadbandChanged(pitchAxisDeadband());
     emit yawAxisDeadbandChanged(yawAxisDeadband());
     emit throttleAxisDeadbandChanged(throttleAxisDeadband());
+
+    emit transmitterModeChanged(_transmitterMode);
 }
 
 void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)