Implementing throttle axis accumulator with deadband support

src/Joystick/Joystick.cc

@@ -23,6 +23,7 @@ const char* Joystick::_calibratedSettingsKey =      "Calibrated1"; // Increment
 const char* Joystick::_buttonActionSettingsKey =    "ButtonActionName%1";
 const char* Joystick::_throttleModeSettingsKey =    "ThrottleMode";
 const char* Joystick::_exponentialSettingsKey =     "Exponential";
+const char* Joystick::_accumulatorSettingsKey =     "Accumulator";
 
 const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = {
     "RollAxis",
@@ -46,6 +47,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     , _lastButtonBits(0)
     , _throttleMode(ThrottleModeCenterZero)
     , _exponential(false)
+    , _accumulator(false)
     , _activeVehicle(NULL)
     , _pollingStartedForCalibration(false)
     , _multiVehicleManager(multiVehicleManager)
@@ -86,6 +88,7 @@ void Joystick::_loadSettings(void)
 
     _calibrated = settings.value(_calibratedSettingsKey, false).toBool();
     _exponential = settings.value(_exponentialSettingsKey, false).toBool();
+    _accumulator = settings.value(_accumulatorSettingsKey, false).toBool();
 
     _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
     badSettings |= !convertOk;
@@ -96,6 +99,7 @@ void Joystick::_loadSettings(void)
     QString maxTpl  ("Axis%1Max");
     QString trimTpl ("Axis%1Trim");
     QString revTpl  ("Axis%1Rev");
+    QString deadbndTpl  ("Axis%1Deadbnd");
 
     for (int axis=0; axis<_axisCount; axis++) {
         Calibration_t* calibration = &_rgCalibration[axis];
@@ -109,9 +113,13 @@ void Joystick::_loadSettings(void)
         calibration->max = settings.value(maxTpl.arg(axis), 32767).toInt(&convertOk);
         badSettings |= !convertOk;
 
+        calibration->deadband = settings.value(deadbndTpl.arg(axis), 0).toInt(&convertOk);
+        badSettings |= !convertOk;
+
         calibration->reversed = settings.value(revTpl.arg(axis), false).toBool();
 
-        qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << badSettings;
+
+        qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings;
     }
 
     for (int function=0; function<maxFunction; function++) {
@@ -145,6 +153,7 @@ void Joystick::_saveSettings(void)
 
     settings.setValue(_calibratedSettingsKey, _calibrated);
     settings.setValue(_exponentialSettingsKey, _exponential);
+    settings.setValue(_accumulatorSettingsKey, _accumulator);
     settings.setValue(_throttleModeSettingsKey, _throttleMode);
 
     qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode" << _calibrated << _throttleMode;
@@ -153,6 +162,7 @@ void Joystick::_saveSettings(void)
     QString maxTpl  ("Axis%1Max");
     QString trimTpl ("Axis%1Trim");
     QString revTpl  ("Axis%1Rev");
+    QString deadbndTpl  ("Axis%1Deadbnd");
 
     for (int axis=0; axis<_axisCount; axis++) {
         Calibration_t* calibration = &_rgCalibration[axis];
@@ -161,6 +171,7 @@ void Joystick::_saveSettings(void)
         settings.setValue(minTpl.arg(axis), calibration->min);
         settings.setValue(maxTpl.arg(axis), calibration->max);
         settings.setValue(revTpl.arg(axis), calibration->reversed);
+        settings.setValue(deadbndTpl.arg(axis), calibration->deadband);
 
         qCDebug(JoystickLog) << "_saveSettings name:axis:min:max:trim:reversed"
                                 << _name
@@ -168,7 +179,8 @@ void Joystick::_saveSettings(void)
                                 << calibration->min
                                 << calibration->max
                                 << calibration->center
-                                << calibration->reversed;
+                                << calibration->reversed
+                                << calibration->deadband;
     }
 
     for (int function=0; function<maxFunction; function++) {
@@ -189,6 +201,8 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
     float axisLength;
     float axisBasis;
 
+
+
     if (value > calibration.center) {
         axisBasis = 1.0f;
         valueNormalized = value - calibration.center;
@@ -199,6 +213,13 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
         axisLength =  calibration.center - calibration.min;
     }
 
+    if (_accumulator) {//deadband will be applied only in accumulator mode
+        int deadband = calibration.deadband*1.5;    //we are increasing deadband to accommodate slight variations
+        if (valueNormalized>deadband) valueNormalized-=deadband;
+        else if (valueNormalized<-deadband) valueNormalized+=deadband;
+        else valueNormalized = 0.f;
+    }
+
     float axisPercent = valueNormalized / axisLength;
 
     float correctedValue = axisBasis * axisPercent;
@@ -214,7 +235,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration)
                             << calibration.min
                             << calibration.max
                             << calibration.center
-                            << calibration.center
+                            << calibration.deadband
                             << axisBasis
                             << valueNormalized
                             << axisLength;
@@ -276,6 +297,15 @@ void Joystick::run(void)
                     axis = _rgFunctionAxis[throttleFunction];
             float   throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis]);
 
+            if ( _accumulator ) {
+                static float throttle_accu = 0.f;
+
+                throttle_accu += throttle*(40/1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time)
+
+                throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
+                throttle = throttle_accu;
+            }
+
             float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
             float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
             float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
@@ -529,6 +559,19 @@ void Joystick::setExponential(bool expo)
     emit exponentialChanged(_exponential);
 }
 
+bool Joystick::accumulator(void)
+{
+    return _accumulator;
+}
+
+void Joystick::setAccumulator(bool accu)
+{
+    _accumulator = accu;
+
+    _saveSettings();
+    emit accumulatorChanged(_accumulator);
+}
+
 void Joystick::startCalibrationMode(CalibrationMode_t mode)
 {
     if (mode == CalibrationModeOff) {

src/Joystick/Joystick.h

@@ -34,6 +34,7 @@ public:
         int     min;
         int     max;
         int     center;
+        int     deadband;
         bool    reversed;
     } Calibration_t;
 
@@ -66,6 +67,7 @@ public:
 
     Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
     Q_PROPERTY(int exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
+    Q_PROPERTY(int accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
 
     // Property accessors
 
@@ -93,6 +95,9 @@ public:
     bool exponential(void);
     void setExponential(bool expo);
 
+    bool accumulator(void);
+    void setAccumulator(bool accu);
+
     typedef enum {
         CalibrationModeOff,         // Not calibrating
         CalibrationModeMonitor,     // Monitors are active, continue to send to vehicle if already polling
@@ -118,6 +123,8 @@ signals:
 
     void exponentialChanged(bool exponential);
 
+    void accumulatorChanged(bool accumulator);
+
     void enabledChanged(bool enabled);
 
     /// Signal containing new joystick information
@@ -175,6 +182,7 @@ protected:
     ThrottleMode_t      _throttleMode;
 
     bool                _exponential;
+    bool                _accumulator;
 
     Vehicle*            _activeVehicle;
     bool                _pollingStartedForCalibration;
@@ -189,6 +197,7 @@ private:
     static const char* _buttonActionSettingsKey;
     static const char* _throttleModeSettingsKey;
     static const char* _exponentialSettingsKey;
+    static const char* _accumulatorSettingsKey;
 };
 
 #endif

src/VehicleSetup/JoystickConfig.qml

@@ -382,6 +382,20 @@ SetupPage {
                                     onClicked: _activeJoystick.throttleMode = 0
                                 }
 
+                                Row {
+                                    width:      parent.width
+                                    spacing:    ScreenTools.defaultFontPixelWidth
+                                    visible:    _activeJoystick.throttleMode == 0
+
+                                    QGCCheckBox {
+                                        id:         accumulator
+                                        checked:    _activeJoystick.accumulator
+                                        text:       qsTr("Use accumulator on throttle")
+
+                                        onClicked:  _activeJoystick.accumulator = checked
+                                    }
+                                }
+
                                 QGCRadioButton {
                                     exclusiveGroup: throttleModeExclusiveGroup
                                     text:           qsTr("Full down stick is zero throttle")

src/VehicleSetup/JoystickConfigController.cc

@@ -228,12 +228,13 @@ void JoystickConfigController::_saveAllTrims(void)
 void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value)
 {
     Q_UNUSED(function);
-    Q_UNUSED(axis);
+    _rgAxisInfo[axis].deadband = std::max(abs(value),_rgAxisInfo[axis].deadband);
-    Q_UNUSED(value);
-    
-    // FIXME: Doesn't wait for center
-    
     _nextButton->setEnabled(true);
+
+    // FIXME: Doesn't wait for center
+    // FIXME: Ideally the deadband should be probed only around the center
+
+    qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband;
 }
 
 bool JoystickConfigController::_stickSettleComplete(int axis, int value)
@@ -420,6 +421,7 @@ void JoystickConfigController::_resetInternalCalibrationValues(void)
         struct AxisInfo* info = &_rgAxisInfo[i];
         info->function = Joystick::maxFunction;
         info->reversed = false;
+        info->deadband = 0;
         info->axisMin = JoystickConfigController::_calCenterPoint;
         info->axisMax = JoystickConfigController::_calCenterPoint;
         info->axisTrim = JoystickConfigController::_calCenterPoint;
@@ -457,6 +459,7 @@ void JoystickConfigController::_setInternalCalibrationValuesFromSettings(void)
         info->axisMin = calibration.min;
         info->axisMax = calibration.max;
         info->reversed = calibration.reversed;
+        info->deadband = calibration.deadband;
         
         qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed;
     }
@@ -512,6 +515,7 @@ void JoystickConfigController::_validateCalibration(void)
             info->axisMin = _calDefaultMinValue;
             info->axisMax = _calDefaultMaxValue;
             info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2);
+            info->deadband = 0;
             info->reversed = false;
         }
     }
@@ -534,6 +538,7 @@ void JoystickConfigController::_writeCalibration(void)
         calibration.min = info->axisMin;
         calibration.max = info->axisMax;
         calibration.reversed = info->reversed;
+        calibration.deadband = info->deadband;
         
         joystick->setCalibration(axis, calibration);
     }

src/VehicleSetup/JoystickConfigController.h

@@ -144,6 +144,7 @@ private:
         int                         axisMin;    ///< Minimum axis value
         int                         axisMax;    ///< Maximum axis value
         int                         axisTrim;   ///< Trim position
+        int                         deadband;   ///< Deadband
     };
     
     Joystick* _activeJoystick;