Skip to content

Commit

Permalink
Comms: Update UdpIODevice
Browse files Browse the repository at this point in the history
  • Loading branch information
HTRamsey committed Dec 13, 2024
1 parent a5ba761 commit 56870f5
Show file tree
Hide file tree
Showing 4 changed files with 155 additions and 49 deletions.
40 changes: 21 additions & 19 deletions src/Comms/LinkManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -500,6 +500,27 @@ void LinkManager::_updateAutoConnectLinks()
#ifdef QGC_ZEROCONF_ENABLED
_addZeroConfAutoConnectLink();
#endif

// check to see if nmea gps is configured for UDP input, if so, set it up to connect
if (_autoConnectSettings->autoConnectNmeaPort()->cookedValueString() == "UDP Port") {
if ((_nmeaSocket->localPort() != _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt()) || (_nmeaSocket->state() != UdpIODevice::BoundState)) {
qCDebug(LinkManagerLog) << "Changing port for UDP NMEA stream";
_nmeaSocket->close();
_nmeaSocket->bind(QHostAddress::AnyIPv4, _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt());
QGCPositionManager::instance()->setNmeaSourceDevice(_nmeaSocket);
}
#ifndef NO_SERIAL_LINK
if (_nmeaPort) {
_nmeaPort->close();
delete _nmeaPort;
_nmeaPort = nullptr;
_nmeaDeviceName = "";
}
#endif
} else {
_nmeaSocket->close();
}

#ifndef NO_SERIAL_LINK
_addSerialAutoConnectLink();
#endif
Expand Down Expand Up @@ -760,25 +781,6 @@ void LinkManager::resetMavlinkSigning()

void LinkManager::_addSerialAutoConnectLink()
{
// check to see if nmea gps is configured for UDP input, if so, set it up to connect
if (_autoConnectSettings->autoConnectNmeaPort()->cookedValueString() == "UDP Port") {
if ((_nmeaSocket->localPort() != _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt())
|| (_nmeaSocket->state() != UdpIODevice::BoundState)) {
qCDebug(LinkManagerLog) << "Changing port for UDP NMEA stream";
_nmeaSocket->close();
_nmeaSocket->bind(QHostAddress::AnyIPv4, _autoConnectSettings->nmeaUdpPort()->rawValue().toUInt());
QGCPositionManager::instance()->setNmeaSourceDevice(_nmeaSocket);
}
if (_nmeaPort) {
_nmeaPort->close();
delete _nmeaPort;
_nmeaPort = nullptr;
_nmeaDeviceName = "";
}
} else {
_nmeaSocket->close();
}

QList<QGCSerialPortInfo> portList;
#ifdef Q_OS_ANDROID
// Android builds only support a single serial connection. Repeatedly calling availablePorts after that one serial
Expand Down
79 changes: 61 additions & 18 deletions src/Comms/UdpIODevice.cc
Original file line number Diff line number Diff line change
Expand Up @@ -8,38 +8,81 @@
****************************************************************************/

#include "UdpIODevice.h"
#include "QGCLoggingCategory.h"

#include <QtNetwork/QNetworkDatagram>

UdpIODevice::UdpIODevice(QObject *parent) : QUdpSocket(parent)
QGC_LOGGING_CATEGORY(UdpIODeviceLog, "qgc.comms.udpiodevice")

UdpIODevice::UdpIODevice(QObject *parent)
: QUdpSocket(parent)
{
// qCDebug(UdpIODeviceLog) << Q_FUNC_INFO << this;

(void) connect(this, &QUdpSocket::readyRead, this, &UdpIODevice::_readAvailableData);
}

UdpIODevice::~UdpIODevice()
{
// this might cause data to be available only after a second readyRead() signal
connect(this, &QUdpSocket::readyRead, this, &UdpIODevice::_readAvailableData);
// qCDebug(UdpIODeviceLog) << Q_FUNC_INFO << this;
}

bool UdpIODevice::canReadLine() const
{
return _buffer.indexOf('\n') > -1;
return _buffer.contains('\n');
}

qint64 UdpIODevice::readLineData(char *data, qint64 maxSize)
{
int length = _buffer.indexOf('\n') + 1; // add 1 to include the '\n'
if (length == 0) {
return 0;
if (_buffer.isEmpty()) {
return -1;
}
length = std::min(length, static_cast<int>(maxSize));
// copy lines to output
std::copy(_buffer.data(), _buffer.data() + length, data);
// trim buffer to remove consumed line
_buffer = _buffer.right(_buffer.size() - length);
// return number of bytes read
return length;

const qsizetype newlineIndex = _buffer.indexOf('\n');
qsizetype length;
if (newlineIndex != -1) {
length = newlineIndex + 1;
} else {
length = qMin(_buffer.size(), static_cast<qsizetype>(maxSize));
}

const qsizetype bytesToCopy = qMin(length, static_cast<qsizetype>(maxSize));
(void) memcpy(data, _buffer.constData(), bytesToCopy);
(void) _buffer.remove(0, bytesToCopy);

return bytesToCopy;
}

void UdpIODevice::_readAvailableData() {
qint64 UdpIODevice::readData(char* data, qint64 maxSize)
{
if (_buffer.isEmpty()) {
return -1;
}

const qsizetype bytesToCopy = qMin(static_cast<qsizetype>(maxSize), _buffer.size());
(void) memcpy(data, _buffer.constData(), bytesToCopy);
(void) _buffer.remove(0, bytesToCopy);

return bytesToCopy;
}

qint64 UdpIODevice::bytesAvailable() const
{
return (_buffer.size() + QIODevice::bytesAvailable());
}

void UdpIODevice::_readAvailableData()
{
while (hasPendingDatagrams()) {
int previousSize = _buffer.size();
_buffer.resize(static_cast<int>(_buffer.size() + pendingDatagramSize()));
readDatagram((_buffer.data() + previousSize), pendingDatagramSize());
const QNetworkDatagram datagram = receiveDatagram(pendingDatagramSize());
if (datagram.isValid()) {
(void) _buffer.append(datagram.data());
} else {
qCWarning(UdpIODeviceLog) << "Error reading datagram:" << errorString();
}
}

if (!_buffer.isEmpty()) {
emit readyRead();
}
}
25 changes: 14 additions & 11 deletions src/Comms/UdpIODevice.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,27 +6,30 @@
* COPYING.md in the root of the source code directory.
*
****************************************************************************/

#pragma once

#include <QtCore/QByteArray>
#include <QtCore/QLoggingCategory>
#include <QtNetwork/QUdpSocket>

/**
* @brief QUdpSocket implementation of canReadLine() readLineData() in server mode.
* The UdpIODevice class works almost exactly as a QUdpSocket, but
* also implements canReadLine() and readLineData() while in the bound state.
* Regular QUdpSocket only allows to use these QIODevice interfaces when using
* connectToHost(), which means it is working as a client instead of server.
*
**/
Q_DECLARE_LOGGING_CATEGORY(UdpIODeviceLog)

/// UdpIODevice provides a QIODevice interface over a QUdpSocket in server mode.
/// It allows line-based reading using canReadLine() and readLineData() even when the socket is in bound mode.
class UdpIODevice: public QUdpSocket
{
Q_OBJECT

public:
UdpIODevice(QObject *parent = nullptr);
bool canReadLine() const;
qint64 readLineData(char *data, qint64 maxSize);
explicit UdpIODevice(QObject *parent = nullptr);
~UdpIODevice();

bool canReadLine() const override;
qint64 readLineData(char* data, qint64 maxSize) override;
qint64 readData(char* data, qint64 maxSize) override;
qint64 bytesAvailable() const override;
bool isSequential() const override { return true; }

private slots:
void _readAvailableData();
Expand Down
60 changes: 59 additions & 1 deletion src/UI/preferences/LinkSettings.qml
Original file line number Diff line number Diff line change
Expand Up @@ -49,12 +49,70 @@ SettingsPage {
}
}

SettingsGroupLayout {
heading: qsTr("NMEA GPS")
visible: QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaPort.visible && QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaBaud.visible

LabelledComboBox {
id: nmeaPortCombo
label: qsTr("Device")

model: ListModel {}

onActivated: (index) => {
if (index !== -1) {
QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaPort.value = comboBox.textAt(index);
}
}

Component.onCompleted: {
model.append({text: "Disabled"})
model.append({text: "UDP Port"})

for (var i in QGroundControl.linkManager.serialPorts) {
nmeaPortCombo.model.append({text:QGroundControl.linkManager.serialPorts[i]})
}

const index = nmeaPortCombo.comboBox.find(QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaPort.valueString);
nmeaPortCombo.currentIndex = index;

if (QGroundControl.linkManager.serialPorts.length === 0) {
nmeaPortCombo.model.append({text: "Serial <none available>"})
}
}
}

LabelledComboBox {
id: nmeaBaudCombo
visible: (nmeaPortCombo.currentText !== "UDP Port") && (nmeaPortCombo.currentText !== "Disabled")
label: qsTr("Baudrate")
model: QGroundControl.linkManager.serialBaudRates

onActivated: (index) => {
if (index !== -1) {
QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaBaud.value = parseInt(comboBox.textAt(index));
}
}

Component.onCompleted: {
const index = nmeaBaudCombo.comboBox.find(QGroundControl.settingsManager.autoConnectSettings.autoConnectNmeaBaud.valueString);
nmeaBaudCombo.currentIndex = index;
}
}

LabelledFactTextField {
visible: nmeaPortCombo.currentText === "UDP Port"
label: qsTr("NMEA stream UDP port")
fact: QGroundControl.settingsManager.autoConnectSettings.nmeaUdpPort
}
}

SettingsGroupLayout {
heading: qsTr("Links")

Repeater {
model: _linkManager.linkConfigurations

RowLayout {
Layout.fillWidth: true
visible: !object.dynamic
Expand Down

0 comments on commit 56870f5

Please sign in to comment.