Merge pull request #5185 from khancyr/negativethrust

Joystick: add a button to support negative thrust

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc

@@ -106,3 +106,13 @@ void ArduRoverFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
 
     qgcApp()->showMessage(QStringLiteral("Change altitude not supported."));
 }
+
+bool ArduRoverFirmwarePlugin::supportsNegativeThrust(void)
+{
+    return true;
+}
+
+bool ArduRoverFirmwarePlugin::supportsManualControl(void)
+{
+    return true;
+}

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -55,6 +55,8 @@ public:
     void    guidedModeChangeAltitude                (Vehicle* vehicle, double altitudeChange) final;
     int     remapParamNameHigestMinorVersionNumber  (int majorVersionNumber) const final;
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
+    bool supportsNegativeThrust(void) final;
+    bool supportsManualControl(void) final;
 
 private:
     static bool _remapParamNameIntialized;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -129,6 +129,12 @@ bool FirmwarePlugin::supportsThrottleModeCenterZero(void)
     return true;
 }
 
+bool FirmwarePlugin::supportsNegativeThrust(void)
+{
+    // By default, this is not supported
+    return false;
+}
+
 bool FirmwarePlugin::supportsManualControl(void)
 {
     return false;

src/FirmwarePlugin/FirmwarePlugin.h

@@ -158,6 +158,10 @@ public:
     /// throttle.
     virtual bool supportsThrottleModeCenterZero(void);
 
+    /// Returns true if the vehicle and firmware supports the use of negative thrust
+    /// Typically supported rover.
+    virtual bool supportsNegativeThrust(void);
+
     /// Returns true if the firmware supports the use of the MAVlink "MANUAL_CONTROL" message.
     /// By default, this returns false unless overridden in the firmware plugin.
     virtual bool supportsManualControl(void);

src/Joystick/Joystick.cc

@@ -55,6 +55,7 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     , _rgButtonValues(NULL)
     , _lastButtonBits(0)
     , _throttleMode(ThrottleModeCenterZero)
+    , _negativeThrust(false)
     , _exponential(0)
     , _accumulator(false)
     , _deadband(false)
@@ -462,7 +463,9 @@ void Joystick::run(void)
 
             // Adjust throttle to 0:1 range
             if (_throttleMode == ThrottleModeCenterZero && _activeVehicle->supportsThrottleModeCenterZero()) {
-                throttle = std::max(0.0f, throttle);
+                if (!_activeVehicle->supportsNegativeThrust() || !_negativeThrust) {
+                    throttle = std::max(0.0f, throttle);
+                }
             } else {
                 throttle = (throttle + 1.0f) / 2.0f;
             }
@@ -687,6 +690,22 @@ void Joystick::setThrottleMode(int mode)
     emit throttleModeChanged(_throttleMode);
 }
 
+bool Joystick::negativeThrust(void)
+{
+    return _negativeThrust;
+}
+
+void Joystick::setNegativeThrust(bool allowNegative)
+{
+    if (_negativeThrust == allowNegative) {
+        return;
+    }
+    _negativeThrust = allowNegative;
+
+    _saveSettings();
+    emit negativeThrustChanged(_negativeThrust);
+}
+
 float Joystick::exponential(void)
 {
     return _exponential;

src/Joystick/Joystick.h

@@ -72,6 +72,7 @@ public:
     Q_INVOKABLE QString getButtonAction(int button);
 
     Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
+    Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged)
     Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
     Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
 	Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT)
@@ -106,6 +107,9 @@ public:
     int throttleMode(void);
     void setThrottleMode(int mode);
 
+    bool negativeThrust(void);
+    void setNegativeThrust(bool allowNegative);
+
     float exponential(void);
     void setExponential(float expo);
 
@@ -141,6 +145,8 @@ signals:
 
     void throttleModeChanged(int mode);
 
+    void negativeThrustChanged(bool allowNegative);
+
     void exponentialChanged(float exponential);
 
     void accumulatorChanged(bool accumulator);
@@ -206,6 +212,8 @@ protected:
 
     ThrottleMode_t      _throttleMode;
 
+    bool                _negativeThrust;
+
     float                _exponential;
     bool                _accumulator;
     bool                _deadband;

src/Vehicle/Vehicle.cc

@@ -2118,6 +2118,11 @@ bool Vehicle::supportsThrottleModeCenterZero(void) const
     return _firmwarePlugin->supportsThrottleModeCenterZero();
 }
 
+bool Vehicle::supportsNegativeThrust(void) const
+{
+    return _firmwarePlugin->supportsNegativeThrust();
+}
+
 bool Vehicle::supportsRadio(void) const
 {
     return _firmwarePlugin->supportsRadio();

src/Vehicle/Vehicle.h

@@ -284,6 +284,7 @@ public:
     Q_PROPERTY(bool                 sub                     READ sub                                                    NOTIFY vehicleTypeChanged)
     Q_PROPERTY(bool                 supportsManualControl   READ supportsManualControl                                  CONSTANT)
     Q_PROPERTY(bool        supportsThrottleModeCenterZero   READ supportsThrottleModeCenterZero                         CONSTANT)
+    Q_PROPERTY(bool                supportsNegativeThrust   READ supportsNegativeThrust                                 CONSTANT)
     Q_PROPERTY(bool                 supportsJSButton        READ supportsJSButton                                       CONSTANT)
     Q_PROPERTY(bool                 supportsRadio           READ supportsRadio                                          CONSTANT)
     Q_PROPERTY(bool               supportsMotorInterference READ supportsMotorInterference                              CONSTANT)
@@ -522,6 +523,7 @@ public:
 
     bool supportsManualControl(void) const;
     bool supportsThrottleModeCenterZero(void) const;
+    bool supportsNegativeThrust(void) const;
     bool supportsRadio(void) const;
     bool supportsJSButton(void) const;
     bool supportsMotorInterference(void) const;

src/VehicleSetup/JoystickConfig.qml

@@ -296,7 +296,7 @@ SetupPage {
                         Connections {
                             target: _activeJoystick
 
-                            onManualControl: throttleLoader.item.axisValue = (-2*throttle+1)*32768.0
+                            onManualControl: throttleLoader.item.axisValue = _activeJoystick.negativeThrust ? -throttle*32768.0 : (-2*throttle+1)*32768.0
                         }
                     }
                 } // Column - Attitude Control labels
@@ -453,6 +453,15 @@ SetupPage {
 
                                     onClicked: _activeJoystick.throttleMode = 1
                                 }
+
+                                QGCCheckBox {
+                                    visible:        _activeVehicle.supportsNegativeThrust
+                                    id:             negativeThrust
+                                    text:           qsTr("Allow negative Thrust")
+                                    enabled:        _activeJoystick.negativeThrust = _activeVehicle.supportsNegativeThrust
+                                    checked:        _activeJoystick ? _activeJoystick.negativeThrust : false
+                                    onClicked:      _activeJoystick.negativeThrust = checked
+                                }
                             }
 
                             Column {