diff --git a/src/plugins/mavlink/mavlinkconnection.cpp b/src/plugins/mavlink/mavlinkconnection.cpp index c64e495..72684cb 100644 --- a/src/plugins/mavlink/mavlinkconnection.cpp +++ b/src/plugins/mavlink/mavlinkconnection.cpp @@ -1,107 +1,101 @@ /* * Copyright 2019 Patrick José Pereira * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) version 3, or any * later version accepted by the membership of KDE e.V. (or its * successor approved by the membership of KDE e.V.), which shall * act as a proxy defined in Section 6 of version 3 of the license. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with this library. If not, see . */ #include "mavlinkconnection.h" #include #include MAVLinkConnection::MAVLinkConnection(const QString &vehicleName, QObject *parent) : QObject(parent) , m_vehicleName(vehicleName) , m_connectionHost({QHostAddress("0.0.0.0"), 14550}) { qRegisterMetaType("mavlink_message_t"); // Send periodically heartbeats to say that GCS is alive connect(&heartbeatTimer, &QTimer::timeout, this, [this] { sendByteArray(_heartbeatMessage); }); heartbeatTimer.start(1000); } void MAVLinkConnection::sendByteArray(const QByteArray &byteArray) const { if (m_controlSocket) { for (const auto &sender : m_senders) { m_controlSocket->writeDatagram(byteArray, sender.address, sender.port); } } } -void MAVLinkConnection::sendMessage(const mavlink_message_t &message) const -{ - const int length = mavlink_msg_to_send_buffer(m_buffer, &message); - sendByteArray({reinterpret_cast(m_buffer), length}); -} - void MAVLinkConnection::handshake() { initSockets(); } void MAVLinkConnection::reset() { if (m_controlSocket) { m_controlSocket->abort(); delete m_controlSocket; } emit stateChanged(Kirogi::AbstractVehicle::Disconnected); } void MAVLinkConnection::receiveData() { static mavlink_message_t message; mavlink_status_t status; while (m_controlSocket->hasPendingDatagrams()) { const QNetworkDatagram &datagram = m_controlSocket->receiveDatagram(); m_senders.append({datagram.senderAddress(), datagram.senderPort()}); if (datagram.isValid()) { const QByteArray &data = datagram.data(); for (const auto &byte : data) { if (mavlink_parse_char(0, byte, &message, &status)) { // GSC sysid if (message.sysid != 255) { emit mavlinkMessage(message); } } } } } m_senders.erase(std::unique(m_senders.begin(), m_senders.end(), [](const auto first, const auto second) { return (first.address == second.address && first.port == second.port); }), m_senders.end()); } void MAVLinkConnection::initSockets() { m_controlSocket = new QUdpSocket(this); QObject::connect(m_controlSocket, &QUdpSocket::readyRead, this, &MAVLinkConnection::receiveData); m_controlSocket->setProxy(QNetworkProxy::NoProxy); const bool bindState = m_controlSocket->bind(QHostAddress::AnyIPv4, m_connectionHost.port, QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress); if (bindState) { // The All Hosts multicast group addresses all hosts on the same network // segment. m_controlSocket->joinMulticastGroup(QHostAddress("224.0.0.1")); } emit stateChanged(Kirogi::AbstractVehicle::Connecting); } diff --git a/src/plugins/mavlink/mavlinkconnection.h b/src/plugins/mavlink/mavlinkconnection.h index 6f8e304..689cdbe 100644 --- a/src/plugins/mavlink/mavlinkconnection.h +++ b/src/plugins/mavlink/mavlinkconnection.h @@ -1,116 +1,145 @@ /* * Copyright 2019 Patrick José Pereira * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 2.1 of the License, or (at your option) version 3, or any * later version accepted by the membership of KDE e.V. (or its * successor approved by the membership of KDE e.V.), which shall * act as a proxy defined in Section 6 of version 3 of the license. * * This library is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with this library. If not, see . */ #pragma once #include #include "abstractvehicle.h" #include #include #include #include // It's necessary to disable some warnings to avoid the amount of noise // messages from MAVLink source code #ifdef __GNUC__ #if __GNUC__ > 8 #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Waddress-of-packed-member" #pragma GCC diagnostic ignored "-Wpedantic" #else #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wall" #endif #else #pragma warning(push, 0) #endif #include "mavlink.h" #ifdef __GNUC__ #pragma GCC diagnostic pop #else #pragma warning(pop, 0) #endif +struct MAVLinkHelper { + static constexpr uint8_t GCS_SYSTEM_ID = 255; +}; + namespace { /** * @brief Create a QByteArray with Heartbeat Message content * * @return QByteArray */ QByteArray createHeartbeatMessage() { mavlink_message_t message; uint8_t buffer[1024]; - mavlink_msg_heartbeat_pack(255, MAV_COMP_ID_MISSIONPLANNER, &message, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_SAFETY_ARMED, 0, MAV_STATE_ACTIVE); + mavlink_msg_heartbeat_pack(MAVLinkHelper::GCS_SYSTEM_ID, MAV_COMP_ID_MISSIONPLANNER, &message, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_FLAG_SAFETY_ARMED, 0, MAV_STATE_ACTIVE); auto length = mavlink_msg_to_send_buffer(buffer, &message); return QByteArray(reinterpret_cast(buffer), length); } } class MAVLinkConnection : public QObject { Q_OBJECT public: explicit MAVLinkConnection(const QString &vehicleName, QObject *parent = nullptr); ~MAVLinkConnection() = default; + template constexpr void sendMessage(const T &message) const + { + if constexpr (std::is_same::value) { + const int length = mavlink_msg_to_send_buffer(m_buffer, &message); + sendByteArray({reinterpret_cast(m_buffer), length}); + + } else if constexpr (std::is_same::value) { + mavlink_message_t mavlink_message = {}; + mavlink_msg_command_long_encode(MAVLinkHelper::GCS_SYSTEM_ID, MAV_COMP_ID_MISSIONPLANNER, &mavlink_message, &message); + sendMessage(mavlink_message); + + } else if constexpr (std::is_same::value) { + mavlink_message_t mavlink_message = {}; + mavlink_msg_manual_control_encode(MAVLinkHelper::GCS_SYSTEM_ID, MAV_COMP_ID_MISSIONPLANNER, &mavlink_message, &message); + sendMessage(mavlink_message); + + } else if constexpr (std::is_same::value) { + mavlink_message_t mavlink_message = {}; + mavlink_msg_param_request_list_encode(MAVLinkHelper::GCS_SYSTEM_ID, MAV_COMP_ID_MISSIONPLANNER, &mavlink_message, &message); + sendMessage(mavlink_message); + + } else { + static_assert(std::is_same::value, "Invalid message type."); + } + } + public Q_SLOTS: void handshake(); void reset(); void sendByteArray(const QByteArray &byteArray) const; - void sendMessage(const mavlink_message_t &message) const; Q_SIGNALS: void mavlinkMessage(const mavlink_message_t &message) const; void stateChanged(Kirogi::AbstractVehicle::ConnectionState state) const; void responseReceived(const QString &response) const; void stateReceived(const QByteArray &state) const; private Q_SLOTS: void receiveData(); private: void initSockets(); QString m_vehicleName; struct AddressPort { QHostAddress address; int port; }; AddressPort m_connectionHost; QPointer m_controlSocket; QVector m_senders; QTimer heartbeatTimer; // Used by sendMessage mutable uint8_t m_buffer[1024]; const QByteArray _heartbeatMessage = createHeartbeatMessage(); };