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 }