File indexing completed on 2024-06-09 05:46:34

0001 /*
0002  * Copyright 2020 Kitae Kim <develoot@gmail.com>
0003  *
0004  * This library is free software; you can redistribute it and/or
0005  * modify it under the terms of the GNU Lesser General Public
0006  * License as published by the Free Software Foundation; either
0007  * version 2.1 of the License, or (at your option) version 3, or any
0008  * later version accepted by the membership of KDE e.V. (or its
0009  * successor approved by the membership of KDE e.V.), which shall
0010  * act as a proxy defined in Section 6 of version 3 of the license.
0011  *
0012  * This library is distributed in the hope that it will be useful,
0013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
0014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
0015  * Lesser General Public License for more details.
0016  *
0017  * You should have received a copy of the GNU Lesser General Public
0018  * License along with this library.  If not, see <http://www.gnu.org/licenses/>.
0019  */
0020 
0021 #include "mavlinkudpconnection.h"
0022 
0023 #include <QNetworkDatagram>
0024 #include <QNetworkProxy>
0025 #include <QString>
0026 
0027 #include "mavlink_connection_debug.h"
0028 
0029 MAVLinkUdpConnection::MAVLinkUdpConnection(Kirogi::UdpConfiguration configuration, uint8_t channel, QObject *parent)
0030     : MAVLinkConnection(channel, parent)
0031     , m_configuration(configuration)
0032 {
0033 }
0034 
0035 MAVLinkUdpConnection::~MAVLinkUdpConnection()
0036 {
0037     disconnect();
0038 }
0039 
0040 Kirogi::ConnectionConfiguration *MAVLinkUdpConnection::configuration()
0041 {
0042     return &m_configuration;
0043 }
0044 
0045 bool MAVLinkUdpConnection::connect()
0046 {
0047     if (state() > State::Disconnected) {
0048         qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION)
0049             << QString("Tried to create UDP connection %1 when it's not disconnected.").arg(m_configuration.name());
0050         return false;
0051     }
0052 
0053     qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION) << QString("Connecting connection %1.").arg(m_configuration.name());
0054 
0055     m_socket = QSharedPointer<QUdpSocket>(new QUdpSocket(this));
0056     m_socket->setProxy(QNetworkProxy::NoProxy);
0057     if (!m_socket->bind(QHostAddress::AnyIPv4, m_configuration.port(), QAbstractSocket::ReuseAddressHint | QUdpSocket::ShareAddress)) {
0058         qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION) << QString("Failed to bind connection %1 to address.").arg(m_configuration.name());
0059         m_socket->deleteLater();
0060         return false;
0061     }
0062     // The 'All Hosts' multicast group addresses all hosts on the same network segment.
0063     m_socket->joinMulticastGroup(QHostAddress("224.0.0.1"));
0064 
0065     QObject::connect(m_socket.data(), &QUdpSocket::readyRead, this, &MAVLinkUdpConnection::processDataOnSocket);
0066     setState(State::Connected);
0067 
0068     return true;
0069 }
0070 
0071 bool MAVLinkUdpConnection::disconnect()
0072 {
0073     if (state() < State::Connected) {
0074         qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION) << QString("Tried to disconnect connection %1 when it's not connected.").arg(m_configuration.name());
0075         return false;
0076     }
0077 
0078     qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION) << QString("Disconnecting connection %1.").arg(m_configuration.name());
0079 
0080     m_socket->deleteLater();
0081     m_socket = nullptr;
0082 
0083     setState(State::Disconnected);
0084 
0085     return true;
0086 }
0087 
0088 void MAVLinkUdpConnection::processDataOnSocket()
0089 {
0090     while (m_socket->hasPendingDatagrams()) {
0091         const QNetworkDatagram &datagram = m_socket->receiveDatagram();
0092         auto checkOverlapped = [datagram](auto &target) {
0093             return datagram.senderAddress() == target.address && datagram.senderPort() == target.port;
0094         };
0095 
0096         if (std::find_if(m_targets.begin(), m_targets.end(), checkOverlapped) == m_targets.end()) {
0097             m_targets.append({datagram.senderAddress(), datagram.senderPort()});
0098         }
0099 
0100         parseData(datagram.data());
0101     }
0102 }
0103 
0104 void MAVLinkUdpConnection::sendBytes(const QByteArray &bytes)
0105 {
0106     if (state() < State::Connected) {
0107         qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION)
0108             << QString("Tried to send bytes on connection %1 when it's not connected.").arg(m_configuration.name());
0109         return;
0110     }
0111 
0112     for (const auto &target : m_targets) {
0113         if (m_socket->writeDatagram(bytes, target.address, target.port) < 0) {
0114             qCDebug(KIROGI_VEHICLESUPPORT_MAVLINK_CONNECTION) << "Error occured while writing data to " << target.address << ":" << target.port;
0115         }
0116     }
0117 }