diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3d3293c..4de7b66 100644 --- a/qgroundcontrol.pro +++ b/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 \ diff --git a/src/Joystick/CMakeLists.txt b/src/Joystick/CMakeLists.txt index da21bc6..83b1065 100644 --- a/src/Joystick/CMakeLists.txt +++ b/src/Joystick/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(Joystick Joystick.cc JoystickManager.cc JoystickSDL.cc + JoystickMavCommand.cc ${EXTRA_SRC} ) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 05e59b6..e937624 100644 --- a/src/Joystick/Joystick.cc +++ b/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(_assignableButtonActions[i]); _availableActionTitles << p->action(); diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index f971967..81ccb07 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -18,6 +18,7 @@ #include "QGCLoggingCategory.h" #include "Vehicle.h" #include "MultiVehicleManager.h" +#include "JoystickMavCommand.h" #include Q_DECLARE_LOGGING_CATEGORY(JoystickLog) @@ -298,6 +299,8 @@ protected: QStringList _availableActionTitles; MultiVehicleManager* _multiVehicleManager = nullptr; + QList _customMavCommands; + static const float _minAxisFrequencyHz; static const float _maxAxisFrequencyHz; static const float _minButtonFrequencyHz; diff --git a/src/Joystick/JoystickMavCommand.cc b/src/Joystick/JoystickMavCommand.cc new file mode 100644 index 0000000..66e72f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.cc @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#include +#include + +QGC_LOGGING_CATEGORY(JoystickMavCommandLog, "JoystickMavCommandLog") + +static void parseJsonValue(const QJsonObject& jsonObject, const QString& key, float& param) +{ + if (jsonObject.contains(key)) + param = static_cast(jsonObject.value(key).toDouble()); +} + +QList JoystickMavCommand::load(const QString& jsonFilename) +{ + qCDebug(JoystickMavCommandLog) << "Loading" << jsonFilename; + QList 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(_id), + _showError, + _param1, _param2, _param3, _param4, _param5, _param6, _param7); +} diff --git a/src/Joystick/JoystickMavCommand.h b/src/Joystick/JoystickMavCommand.h new file mode 100644 index 0000000..96cf9f4 --- /dev/null +++ b/src/Joystick/JoystickMavCommand.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 +#include + +class Vehicle; + +/// Custom MAV command +class JoystickMavCommand +{ +public: + static QList 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; +}; + diff --git a/src/Joystick/JoystickMavCommands.json b/src/Joystick/JoystickMavCommands.json new file mode 100644 index 0000000..ba28ed4 --- /dev/null +++ b/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 + } + ] +}