Joystick: Introduce custom MAV commands

In case when you would like to bind some MAV commands
to some joystick's buttons, you can now create a json file
which will be parsed at runtime.

It is usefull when you need to send a custom command from the joystick.
But this command is not supported by the firmware, or not relevant to be included to QGC.

Example:
Create a file JoystickMavCommands.json and place it with a binary:
"commands": [
        {
            "id":                   31010,
            "name":                 "Enable Autonomy",
            "param1":               1.0
        }
]

After executing the QGC, you will be able to bind "Enable Autonomy" to a button,
by clicking to the button, MAV_CMD_USER_1 (31010) with param1 1.0 will be sent.

If there is no json file, nothing will happen.

qgroundcontrol.pro

@@ -583,6 +583,7 @@ HEADERS += \
     src/FollowMe/FollowMe.h \
     src/Joystick/Joystick.h \
     src/Joystick/JoystickManager.h \
+    src/Joystick/JoystickMavCommand.h \
     src/JsonHelper.h \
     src/KMLDomDocument.h \
     src/KMLHelper.h \
@@ -826,6 +827,7 @@ SOURCES += \
     src/FollowMe/FollowMe.cc \
     src/Joystick/Joystick.cc \
     src/Joystick/JoystickManager.cc \
+    src/Joystick/JoystickMavCommand.cc \
     src/JsonHelper.cc \
     src/KMLDomDocument.cc \
     src/KMLHelper.cc \

src/Joystick/CMakeLists.txt

@@ -11,6 +11,7 @@ add_library(Joystick
 	Joystick.cc
 	JoystickManager.cc
 	JoystickSDL.cc
+	JoystickMavCommand.cc
 	${EXTRA_SRC}
 )
 

src/Joystick/Joystick.cc

@@ -120,6 +120,8 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
     _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
     _loadSettings();
     connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged);
+
+    _customMavCommands = JoystickMavCommand::load("JoystickMavCommands.json");
 }
 
 void Joystick::stop()
@@ -1024,6 +1026,14 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown)
     } else if(action == _buttonActionEmergencyStop) {
       if(buttonDown) emit emergencyStop();
     } else {
+        if (buttonDown && _activeVehicle) {
+            for (auto& item : _customMavCommands) {
+                if (action == item.name()) {
+                    item.send(_activeVehicle);
+                    return;
+                }
+            }
+        }
         qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
     }
 }
@@ -1112,6 +1122,9 @@ void Joystick::_buildActionList(Vehicle* activeVehicle)
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight,   true));
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter));
     _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop));
+    for (auto& item : _customMavCommands)
+        _assignableButtonActions.append(new AssignableButtonAction(this, item.name()));
+
     for(int i = 0; i < _assignableButtonActions.count(); i++) {
         AssignableButtonAction* p = qobject_cast<AssignableButtonAction*>(_assignableButtonActions[i]);
         _availableActionTitles << p->action();

src/Joystick/Joystick.h

@@ -18,6 +18,7 @@
 #include "QGCLoggingCategory.h"
 #include "Vehicle.h"
 #include "MultiVehicleManager.h"
+#include "JoystickMavCommand.h"
 #include <atomic>
 
 Q_DECLARE_LOGGING_CATEGORY(JoystickLog)
@@ -298,6 +299,8 @@ protected:
     QStringList                     _availableActionTitles;
     MultiVehicleManager*            _multiVehicleManager = nullptr;
 
+    QList<JoystickMavCommand> _customMavCommands;
+
     static const float  _minAxisFrequencyHz;
     static const float  _maxAxisFrequencyHz;
     static const float  _minButtonFrequencyHz;

src/Joystick/JoystickMavCommand.cc

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "JoystickMavCommand.h"
+#include "QGCLoggingCategory.h"
+#include "Vehicle.h"
+#include <QJsonDocument>
+#include <QJsonParseError>
+#include <QJsonArray>
+
+QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog")
+
+static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param)
+{
+    if (jsonObject.contains(key))
+        param = static_cast<float>(jsonObject.value(key).toDouble());
+}
+
+QList<JoystickMavCommand> JoystickMavCommand::load(const QString& jsonFilename)
+{
+    qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename;
+    QList<JoystickMavCommand> result;
+
+    QFile jsonFile(jsonFilename);
+    if (!jsonFile.open(QIODevice::ReadOnly | QIODevice::Text)) {
+        qCDebug(JoystickMavCommandLog) << "Could not open" << jsonFilename;
+        return result;
+    }
+
+    QByteArray bytes = jsonFile.readAll();
+    jsonFile.close();
+    QJsonParseError jsonParseError;
+    QJsonDocument doc = QJsonDocument::fromJson(bytes, &jsonParseError);
+    if (jsonParseError.error != QJsonParseError::NoError) {
+        qWarning() << jsonFilename << "Unable to open json document" << jsonParseError.errorString();
+        return result;
+    }
+
+    QJsonObject json = doc.object();
+
+    const int version = json.value("version").toInt();
+    if (version != 1) {
+        qWarning() << jsonFilename << ": invalid version" << version;
+        return result;
+    }
+
+    QJsonValue jsonValue = json.value("commands");
+    if (!jsonValue.isArray()) {
+        qWarning() << jsonFilename << ": 'commands' is not an array";
+        return result;
+    }
+
+    QJsonArray jsonArray = jsonValue.toArray();
+    for (QJsonValue info: jsonArray) {
+        if (!info.isObject()) {
+            qWarning() << jsonFilename << ": 'commands' should contain objects";
+            return result;
+        }
+
+        auto jsonObject = info.toObject();
+        JoystickMavCommand item;
+        if (!jsonObject.contains("id")) {
+            qWarning() << jsonFilename << ": 'id' is required";
+            continue;
+        }
+        item._id = jsonObject.value("id").toInt();
+        if (!jsonObject.contains("name")) {
+            qWarning() << jsonFilename << ": 'name' is required";
+            continue;
+        }
+        item._name = jsonObject.value("name").toString();
+        item._showError = jsonObject.value("showError").toBool();
+        parseJsonValue(jsonObject, "param1", item._param1);
+        parseJsonValue(jsonObject, "param2", item._param2);
+        parseJsonValue(jsonObject, "param3", item._param3);
+        parseJsonValue(jsonObject, "param4", item._param4);
+        parseJsonValue(jsonObject, "param5", item._param5);
+        parseJsonValue(jsonObject, "param6", item._param6);
+        parseJsonValue(jsonObject, "param7", item._param7);
+
+        qCDebug(JoystickMavCommandLog) << jsonObject;
+
+        result.append(item);
+    }
+
+    return result;
+}
+
+void JoystickMavCommand::send(Vehicle* vehicle)
+{
+    vehicle->sendMavCommand(vehicle->defaultComponentId(),
+                            static_cast<MAV_CMD>(_id),
+                            _showError,
+                            _param1, _param2, _param3, _param4, _param5, _param6, _param7);
+}

src/Joystick/JoystickMavCommand.h

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+/// @file
+/// @brief  Custom Joystick MAV command
+
+#pragma once
+
+#include <QString>
+#include <QList>
+
+class Vehicle;
+
+/// Custom MAV command
+class JoystickMavCommand
+{
+public:
+    static QList<JoystickMavCommand> load(const QString& jsonFilename);
+    QString name() const { return _name; }
+
+    void send(Vehicle* vehicle);
+private:
+    QString _name;
+    int _id = 0;
+    bool _showError = false;
+    float _param1 = 0.0f;
+    float _param2 = 0.0f;
+    float _param3 = 0.0f;
+    float _param4 = 0.0f;
+    float _param5 = 0.0f;
+    float _param6 = 0.0f;
+    float _param7 = 0.0f;
+};
+

src/Joystick/JoystickMavCommands.json

@@ -0,0 +1,17 @@
+{
+    "comment": "Joystick MAV commands",
+    "version": 1,
+
+    "commands": [
+        {
+            "id":                   31010,
+            "name":                 "MAV_CMD_USER_1",
+            "param1":               1.0
+        },
+        {
+            "id":                   31011,
+            "name":                 "MAV_CMD_USER_2",
+            "param1":               0.0
+        }
+    ]
+}