Merge pull request #6304 from mavlink/joystickFrequency

Allow defining Joystick message frequency

src/Joystick/Joystick.cc

@@ -26,6 +26,7 @@ const char* Joystick::_exponentialSettingsKey =         "Exponential";
 const char* Joystick::_accumulatorSettingsKey =         "Accumulator";
 const char* Joystick::_deadbandSettingsKey =            "Deadband";
 const char* Joystick::_circleCorrectionSettingsKey =    "Circle_Correction";
+const char* Joystick::_frequencySettingsKey =           "Frequency";
 const char* Joystick::_txModeSettingsKey =              NULL;
 const char* Joystick::_fixedWingTXModeSettingsKey =     "TXMode_FixedWing";
 const char* Joystick::_multiRotorTXModeSettingsKey =    "TXMode_MultiRotor";
@@ -66,6 +67,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     , _accumulator(false)
     , _deadband(false)
     , _circleCorrection(true)
+    , _frequency(25.0f)
     , _activeVehicle(NULL)
     , _pollingStartedForCalibration(false)
     , _multiVehicleManager(multiVehicleManager)
@@ -127,6 +129,7 @@ void Joystick::_setDefaultCalibration(void) {
     _accumulator = false;
     _deadband = false;
     _circleCorrection = false;
+    _frequency = 25.0f;
     _throttleMode = ThrottleModeCenterZero;
     _calibrated = true;
 
@@ -192,6 +195,7 @@ void Joystick::_loadSettings(void)
     _accumulator = settings.value(_accumulatorSettingsKey, false).toBool();
     _deadband = settings.value(_deadbandSettingsKey, false).toBool();
     _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool();
+    _frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat();
 
     _throttleMode = (ThrottleMode_t)settings.value(_throttleModeSettingsKey, ThrottleModeCenterZero).toInt(&convertOk);
     badSettings |= !convertOk;
@@ -269,6 +273,7 @@ void Joystick::_saveSettings(void)
     settings.setValue(_accumulatorSettingsKey, _accumulator);
     settings.setValue(_deadbandSettingsKey, _deadband);
     settings.setValue(_circleCorrectionSettingsKey, _circleCorrection);
+    settings.setValue(_frequencySettingsKey, _frequency);
     settings.setValue(_throttleModeSettingsKey, _throttleMode);
 
     qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode;
@@ -542,8 +547,9 @@ void Joystick::run(void)
             emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode());
         }
 
-        // Sleep, update rate of joystick is approx. 25 Hz (1000 ms / 25 = 40 ms)
-        QGC::SLEEP::msleep(40);
+        // Sleep. Update rate of joystick is by default 25 Hz
+        int mswait = (int)(1000.0f / _frequency);
+        QGC::SLEEP::msleep(mswait);
     }
 
     _close();
@@ -787,6 +793,21 @@ void Joystick::setCircleCorrection(bool circleCorrection)
     emit circleCorrectionChanged(_circleCorrection);
 }
 
+float Joystick::frequency()
+{
+    return _frequency;
+}
+
+void Joystick::setFrequency(float val)
+{
+    //-- Arbitrary limits
+    if(val < 0.25f)  val = 0.25f;
+    if(val > 100.0f) val = 100.0f;
+    _frequency = val;
+    _saveSettings();
+    emit frequencyChanged();
+}
+
 void Joystick::setCalibrationMode(bool calibrating)
 {
     _calibrationMode = calibrating;

src/Joystick/Joystick.h

@@ -77,6 +77,7 @@ public:
     Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
     Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT)
     Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged)
+    Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged)
 
     // Property accessors
 
@@ -129,6 +130,9 @@ public:
     /// Set the current calibration mode
     void setCalibrationMode(bool calibrating);
 
+    float frequency();
+    void setFrequency(float val);
+
 signals:
     void calibratedChanged(bool calibrated);
 
@@ -160,6 +164,8 @@ signals:
 
     void buttonActionTriggered(int action);
 
+    void frequencyChanged();
+
 protected:
     void    _setDefaultCalibration(void);
     void    _saveSettings(void);
@@ -216,6 +222,7 @@ protected:
     bool                _accumulator;
     bool                _deadband;
     bool                _circleCorrection;
+    float               _frequency;
 
     Vehicle*            _activeVehicle;
     bool                _pollingStartedForCalibration;
@@ -233,6 +240,7 @@ private:
     static const char* _accumulatorSettingsKey;
     static const char* _deadbandSettingsKey;
     static const char* _circleCorrectionSettingsKey;
+    static const char* _frequencySettingsKey;
     static const char* _txModeSettingsKey;
     static const char* _fixedWingTXModeSettingsKey;
     static const char* _multiRotorTXModeSettingsKey;

src/VehicleSetup/JoystickConfig.qml

@@ -531,6 +531,24 @@ SetupPage {
                                 }
                             }
 
+                            Row {
+                                width:      parent.width
+                                spacing:    ScreenTools.defaultFontPixelWidth
+                                visible:    advancedSettings.checked
+                                QGCLabel {
+                                    text:       qsTr("Message frequency (Hz):")
+                                    anchors.verticalCenter: parent.verticalCenter
+                                }
+                                QGCTextField {
+                                    text:       _activeJoystick.frequency
+                                    validator:  DoubleValidator { bottom: 0.25; top: 100.0; }
+                                    inputMethodHints: Qt.ImhFormattedNumbersOnly
+                                    onEditingFinished: {
+                                        _activeJoystick.frequency = parseFloat(text)
+                                    }
+                                }
+                            }
+
                             Row {
                                 width:      parent.width
                                 spacing:    ScreenTools.defaultFontPixelWidth