Add Saving PWM values for button RC override

zdanek · zdanek · commit 2cc339034be7 · 2023-08-04T13:49:46.000+02:00
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
@@ -185,7 +185,7 @@
         <file alias="QGroundControl/Controls/TerrainStatus.qml">src/PlanView/TerrainStatus.qml</file>
         <file alias="QGroundControl/Controls/TakeoffItemMapVisual.qml">src/PlanView/TakeoffItemMapVisual.qml</file>
         <file alias="QGroundControl/Controls/ToolStrip.qml">src/QmlControls/ToolStrip.qml</file>
-       	<file alias="QGroundControl/Controls/ToolStripHoverButton.qml">src/QmlControls/ToolStripHoverButton.qml</file>
+        <file alias="QGroundControl/Controls/ToolStripHoverButton.qml">src/QmlControls/ToolStripHoverButton.qml</file>
         <file alias="QGroundControl/Controls/TransectStyleComplexItemEditor.qml">src/PlanView/TransectStyleComplexItemEditor.qml</file>
         <file alias="QGroundControl/Controls/TransectStyleComplexItemStats.qml">src/PlanView/TransectStyleComplexItemStats.qml</file>
         <file alias="QGroundControl/Controls/TransectStyleComplexItemTabBar.qml">src/PlanView/TransectStyleComplexItemTabBar.qml</file>
diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt
@@ -12,6 +12,7 @@ add_library(Joystick
 	JoystickManager.cc
 	JoystickSDL.cc
 	JoystickMavCommand.cc
+	JoystickRcOverride.cc
 	${EXTRA_SRC}
 )
 
diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc
@@ -26,6 +26,8 @@ const char* Joystick::_settingsGroup =                  "Joysticks";
 const char* Joystick::_calibratedSettingsKey =          "Calibrated4"; // Increment number to force recalibration
 const char* Joystick::_buttonActionNameKey =            "ButtonActionName%1";
 const char* Joystick::_buttonActionRepeatKey =          "ButtonActionRepeat%1";
+const char* Joystick::_buttonActionLowPwmValueKey =     "ButtonActionLowPwm%1";
+const char* Joystick::_buttonActionHighPwmValueKey =    "ButtonActionHighPwm%1";
 const char* Joystick::_throttleModeSettingsKey =        "ThrottleMode";
 const char* Joystick::_negativeThrustSettingsKey =      "NegativeThrust";
 const char* Joystick::_exponentialSettingsKey =         "Exponential";
@@ -126,6 +128,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
 
     _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json");
+    JoystickRcOverride rco(8, 1200, 1800, true);
+    _rcOverrides.append(rco);
 }
 
 void Joystick::stop()
@@ -860,6 +864,59 @@ QString Joystick::getButtonAction(int button)
     return QString(_buttonActionNone);
 }
 
+bool Joystick::assignableActionIsPWM(int button) {
+    qDebug(JoystickLog) << "assignableActionIsPWM: " << button;
+    if (_validButton(button)) {
+        auto action = getButtonAction(button);
+        qDebug(JoystickLog) << "action " << action;
+        return action.contains("PWM");
+    }
+    return false;
+}
+
+void Joystick::setButtonPWM(int button, bool lowPwm, int value) {
+    qDebug(JoystickLog) << "setButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ") << value;
+    if (_validButton(button)) {
+        if (assignableActionIsPWM(button)) {
+            auto action = getButtonAction(button);
+            int rc = getRcChannelFromAction(action);
+            if (rc < 0) {
+                return;
+            }
+            QSettings settings;
+            settings.beginGroup(_settingsGroup);
+            settings.beginGroup(_name);
+            if (lowPwm) {
+                settings.setValue(QString(_buttonActionLowPwmValueKey).arg(button), value);
+            } else {
+                settings.setValue(QString(_buttonActionHighPwmValueKey).arg(button), value);
+            }
+        }
+    }
+}
+
+int Joystick::getButtonPWM(int button, bool lowPwm) {
+    qDebug(JoystickLog) << "getButtonPWM: " << button << (lowPwm ? "LOW " : "HIGH ");
+    if (_validButton(button)) {
+        if (assignableActionIsPWM(button)) {
+            auto action = getButtonAction(button);
+            int rc = getRcChannelFromAction(action);
+            if (rc < 0) {
+                return -1;
+            }
+            QSettings settings;
+            settings.beginGroup(_settingsGroup);
+            settings.beginGroup(_name);
+            if (lowPwm) {
+                return settings.value(QString(_buttonActionLowPwmValueKey).arg(button), -1).toInt();
+            } else {
+                return settings.value(QString(_buttonActionHighPwmValueKey).arg(button), -1).toInt();
+            }
+        }
+    }
+    return -1;
+}
+
 QStringList Joystick::buttonActions()
 {
     QStringList list;
@@ -1045,13 +1102,22 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown)
             emit gripperAction(GRIPPER_ACTION_RELEASE);
         }
     } else {
-        if (buttonDown && _activeVehicle) {
-            for (auto& item : _customMavCommands) {
-                if (action == item.name()) {
-                    item.send(_activeVehicle);
-                    return;
+        qCDebug(JoystickLog) << "Action " << action << " button " << (buttonDown ? "down" : "up");
+        if (_activeVehicle) {
+            bool actionServed = false;
+            if (action.startsWith("Channel")) {
+                actionServed = _executeRcOverrideButtonAction(action, buttonDown);
+            }
+            if (buttonDown && !actionServed) {
+                for (auto& item : _customMavCommands) {
+                    if (action == item.name()) {
+                        item.send(_activeVehicle);
+                        return;
+                    }
                 }
             }
+        } else {
+            qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
         }
     }
 }
@@ -1143,6 +1209,11 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab));
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease));
 
+    for (int ch = 8; ch <= 16;ch++) {
+         _assignableButtonActions.append(new AssignableButtonAction(this, QString("Channel %1 direct PWM").arg(ch)));
+    }
+
+    _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
     for (auto& item : _customMavCommands) {
         _assignableButtonActions.append(new AssignableButtonAction(this, item.name()));
     }
@@ -1153,3 +1224,34 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
     }
     emit assignableActionsChanged();
 }
+
+bool Joystick::_executeRcOverrideButtonAction(const QString &action, bool buttonDown)
+{
+    QRegularExpression re("^Channel (\\d{1,2}) direct PWM");
+    QRegularExpressionMatch match = re.match(action);
+    if (match.hasMatch()) {
+        QString channelNoStr = match.captured(1);
+        qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr;
+        uint8_t channelNo = channelNoStr.toInt();
+
+        for (auto& item : _rcOverrides) {
+            if (channelNo == item.channel()) {
+                item.send(_activeVehicle, buttonDown);
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+int Joystick::getRcChannelFromAction(QString actionName) {
+    QRegularExpression re("^Channel (\\d{1,2}) direct PWM");
+    QRegularExpressionMatch match = re.match(actionName);
+    if (match.hasMatch()) {
+        QString channelNoStr = match.captured(1);
+        qCDebug(JoystickLog) << "RC override matched for channel " << channelNoStr;
+        uint8_t channelNo = channelNoStr.toInt();
+        return channelNo;
+    }
+    return -1;
+}
diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h
@@ -16,10 +16,11 @@
 #include <QThread>
 #include <atomic>
 
+#include "JoystickMavCommand.h"
+#include "JoystickRcOverride.h"
+#include "MultiVehicleManager.h"
 #include "QGCLoggingCategory.h"
 #include "Vehicle.h"
-#include "MultiVehicleManager.h"
-#include "JoystickMavCommand.h"
 
 Q_DECLARE_LOGGING_CATEGORY(JoystickLog)
 Q_DECLARE_LOGGING_CATEGORY(JoystickValuesLog)
@@ -118,6 +119,10 @@ class Joystick : public QThread
     Q_INVOKABLE bool    getButtonRepeat     (int button);
     Q_INVOKABLE void    setButtonAction     (int button, const QString& action);
     Q_INVOKABLE QString getButtonAction     (int button);
+    Q_INVOKABLE bool    assignableActionIsPWM     (int button);
+    Q_INVOKABLE void    setButtonPWM     (int button, bool lowPwm, int value);
+    Q_INVOKABLE int     getButtonPWM     (int button, bool lowPwm);
+
 
     // Property accessors
 
@@ -307,6 +312,7 @@ class Joystick : public QThread
     MultiVehicleManager*            _multiVehicleManager = nullptr;
 
     QList<JoystickMavCommand> _customMavCommands;
+    QList<JoystickRcOverride> _rcOverrides;
 
     static const float  _minAxisFrequencyHz;
     static const float  _maxAxisFrequencyHz;
@@ -320,6 +326,8 @@ class Joystick : public QThread
     static const char* _calibratedSettingsKey;
     static const char* _buttonActionNameKey;
     static const char* _buttonActionRepeatKey;
+    static const char* _buttonActionLowPwmValueKey;
+    static const char* _buttonActionHighPwmValueKey;
     static const char* _throttleModeSettingsKey;
     static const char* _negativeThrustSettingsKey;
     static const char* _exponentialSettingsKey;
@@ -365,4 +373,6 @@ class Joystick : public QThread
 
 private slots:
     void _activeVehicleChanged(Vehicle* activeVehicle);
+    bool _executeRcOverrideButtonAction(const QString &action, bool buttonDown);
+    int getRcChannelFromAction(QString actionName);
 };
diff --git a/src/Joystick/JoystickRcOverride.cc b/src/Joystick/JoystickRcOverride.cc
@@ -0,0 +1,37 @@
+//
+// Created by zdanek on 08.07.2022.
+//
+
+#include "JoystickRcOverride.h"
+
+#include "Vehicle.h"
+
+JoystickRcOverride::JoystickRcOverride(const uint8_t rcChannel,
+                                       const uint16_t loPwmValue,
+                                       const uint16_t hiPwmValue,
+                                       bool latch)
+    : rcChannel(rcChannel)
+    , loPwmValue(loPwmValue)
+    , hiPwmValue(hiPwmValue)
+    , latchMode(latch)
+{}
+
+void JoystickRcOverride::send(Vehicle *vehicle, bool buttonDown)
+{
+    uint16_t pwmValue = buttonDown ? hiPwmValue : loPwmValue;
+
+    if (latchMode) {
+        if (buttonDown) {
+            latchButtonDown = !latchButtonDown;
+            pwmValue = latchButtonDown ? hiPwmValue : loPwmValue;
+        } else {
+            return;
+        }
+    }
+    vehicle->rcChannelOverride(rcChannel, pwmValue);
+}
+
+uint8_t JoystickRcOverride::channel() const
+{
+    return rcChannel;
+};
diff --git a/src/Joystick/JoystickRcOverride.h b/src/Joystick/JoystickRcOverride.h
@@ -0,0 +1,26 @@
+//
+// Created by zdanek on 08.07.2022.
+//
+
+#pragma once
+
+#include "Vehicle.h"
+
+class JoystickRcOverride
+{
+public:
+    JoystickRcOverride(const uint8_t rcChannel,
+                       const uint16_t loPwmValue,
+                       const uint16_t hiPwmValue,
+                       bool latch);
+    void send(Vehicle* vehicle, bool buttonDown);
+    uint8_t channel() const;
+
+private:
+    uint8_t rcChannel;
+    uint16_t loPwmValue;
+    uint16_t hiPwmValue;
+    bool latchMode;
+    bool latchButtonDown = false;
+};
+
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
@@ -4310,3 +4310,62 @@ void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption)
         break;
     }
 }
+
+void Vehicle::rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue)
+{
+    const int maxRcChannels = 16;
+    if (rcChannel > maxRcChannels) {
+        qCWarning(VehicleLog) << "Unsupported rc channel " << rcChannel << " to override";
+        return;
+    }
+    if (pwmValue > 2000 || pwmValue < 1000) {
+        qCWarning(VehicleLog) << "Bad PWM override value " << pwmValue << " for channel " << rcChannel;
+        return;
+    }
+    SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock();
+    if (!sharedLink) {
+        qCDebug(VehicleLog)<< "rcChannelOverride: primary link gone!";
+        return;
+    }
+
+    if (sharedLink->linkConfiguration()->isHighLatency()) {
+        return;
+    }
+
+    qCDebug(VehicleLog) << "Sending RC channel " << rcChannel << " PWM override to " << pwmValue;
+
+    uint16_t override_data[18] = {};
+    for (int i = 0; i < 18; i++) {
+        override_data[i] = UINT16_MAX;
+    }
+    override_data[rcChannel - 1] = pwmValue;
+
+    mavlink_message_t message;
+
+    mavlink_msg_rc_channels_override_pack_chan(
+        static_cast<uint8_t>(_mavlink->getSystemId()),
+        static_cast<uint8_t>(_mavlink->getComponentId()),
+        sharedLink->mavlinkChannel(),
+        &message,
+        static_cast<uint8_t>(_id),
+        0,
+        override_data[0],
+        override_data[1],
+        override_data[2],
+        override_data[3],
+        override_data[4],
+        override_data[5],
+        override_data[6],
+        override_data[7],
+        override_data[8],
+        override_data[9],
+        override_data[10],
+        override_data[11],
+        override_data[12],
+        override_data[13],
+        override_data[14],
+        override_data[15],
+        override_data[16],
+        override_data[17]);
+    sendMessageOnLinkThreadSafe(sharedLink.get(), message);
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
@@ -485,6 +485,14 @@ class Vehicle : public FactGroup
     void setJoystickEnabled         (bool enabled);
     void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons);
 
+    /// Sends Channel override value
+    ///     @param rcChannel channel number 1-16
+    ///     @param pwmValue direct value 1000 - 2000
+    void rcChannelOverride(uint8_t rcChannel, uint16_t pwmValue);
+    /// Sends disabling of channel override
+    ///     @param rcChannel channel number 1-16
+    void disableChannelOverride(uint8_t rcChannel);
+
     // Property accesors
     int id() const{ return _id; }
     int compId() const{ return _compID; }
diff --git a/src/VehicleSetup/JoystickConfigButtons.qml b/src/VehicleSetup/JoystickConfigButtons.qml

Original file line number	Diff line number	Diff line change
`@@ -12,6 +12,7 @@ add_library(Joystick`
`12`	`12`	`JoystickManager.cc`
`13`	`13`	`JoystickSDL.cc`
`14`	`14`	`JoystickMavCommand.cc`
	`15`	`+ JoystickRcOverride.cc`
`15`	`16`	`${EXTRA_SRC}`
`16`	`17`	`)`
`17`	`18`