kopia lustrzana https://gitlab.com/eliggett/wfview
547 wiersze
18 KiB
C++
547 wiersze
18 KiB
C++
#include "udpbase.h"
|
|
#include "logcategories.h"
|
|
|
|
void udpBase::init(quint16 lport)
|
|
{
|
|
//timeStarted.start();
|
|
udp = new QUdpSocket(this);
|
|
udp->bind(lport); // Bind to random port.
|
|
localPort = udp->localPort();
|
|
qInfo(logUdp()) << "UDP Stream bound to local port:" << localPort << " remote port:" << port;
|
|
uint32_t addr = localIP.toIPv4Address();
|
|
myId = (addr >> 8 & 0xff) << 24 | (addr & 0xff) << 16 | (localPort & 0xffff);
|
|
|
|
retransmitTimer = new QTimer();
|
|
connect(retransmitTimer, &QTimer::timeout, this, &udpBase::sendRetransmitRequest);
|
|
retransmitTimer->start(RETRANSMIT_PERIOD);
|
|
}
|
|
|
|
udpBase::~udpBase()
|
|
{
|
|
qInfo(logUdp()) << "Closing UDP stream :" << radioIP.toString() << ":" << port;
|
|
if (udp != Q_NULLPTR) {
|
|
sendControl(false, 0x05, 0x00); // Send disconnect
|
|
udp->close();
|
|
delete udp;
|
|
}
|
|
if (areYouThereTimer != Q_NULLPTR)
|
|
{
|
|
areYouThereTimer->stop();
|
|
delete areYouThereTimer;
|
|
}
|
|
|
|
if (pingTimer != Q_NULLPTR)
|
|
{
|
|
pingTimer->stop();
|
|
delete pingTimer;
|
|
}
|
|
if (idleTimer != Q_NULLPTR)
|
|
{
|
|
idleTimer->stop();
|
|
delete idleTimer;
|
|
}
|
|
if (retransmitTimer != Q_NULLPTR)
|
|
{
|
|
retransmitTimer->stop();
|
|
delete retransmitTimer;
|
|
}
|
|
|
|
pingTimer = Q_NULLPTR;
|
|
idleTimer = Q_NULLPTR;
|
|
areYouThereTimer = Q_NULLPTR;
|
|
retransmitTimer = Q_NULLPTR;
|
|
|
|
}
|
|
|
|
// Base class!
|
|
|
|
void udpBase::dataReceived(QByteArray r)
|
|
{
|
|
if (r.length() < 0x10)
|
|
{
|
|
return; // Packet too small do to anything with?
|
|
}
|
|
|
|
switch (r.length())
|
|
{
|
|
case (CONTROL_SIZE): // Empty response used for simple comms and retransmit requests.
|
|
{
|
|
control_packet_t in = (control_packet_t)r.constData();
|
|
if (in->type == 0x01 && in->len == 0x10)
|
|
{
|
|
// Single packet request
|
|
packetsLost++;
|
|
congestion = static_cast<double>(packetsSent) / packetsLost * 100;
|
|
txBufferMutex.lock();
|
|
auto match = txSeqBuf.find(in->seq);
|
|
if (match != txSeqBuf.end()) {
|
|
// Found matching entry?
|
|
// Send "untracked" as it has already been sent once.
|
|
// Don't constantly retransmit the same packet, give-up eventually
|
|
qDebug(logUdp()) << this->metaObject()->className() << ": Sending (single packet) retransmit of " << QString("0x%1").arg(match->seqNum, 0, 16);
|
|
match->retransmitCount++;
|
|
udpMutex.lock();
|
|
udp->writeDatagram(match->data, radioIP, port);
|
|
udpMutex.unlock();
|
|
}
|
|
else {
|
|
qDebug(logUdp()) << this->metaObject()->className() << ": Remote requested packet"
|
|
<< QString("0x%1").arg(in->seq, 0, 16) <<
|
|
"not found, have " << QString("0x%1").arg(txSeqBuf.firstKey(), 0, 16) <<
|
|
"to" << QString("0x%1").arg(txSeqBuf.lastKey(), 0, 16);
|
|
}
|
|
txBufferMutex.unlock();
|
|
}
|
|
if (in->type == 0x04) {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": Received I am here ";
|
|
areYouThereCounter = 0;
|
|
// I don't think that we will ever receive an "I am here" other than in response to "Are you there?"
|
|
remoteId = in->sentid;
|
|
if (areYouThereTimer != Q_NULLPTR && areYouThereTimer->isActive()) {
|
|
// send ping packets every second
|
|
areYouThereTimer->stop();
|
|
}
|
|
sendControl(false, 0x06, 0x01); // Send Are you ready - untracked.
|
|
}
|
|
else if (in->type == 0x06)
|
|
{
|
|
// Just get the seqnum and ignore the rest.
|
|
}
|
|
break;
|
|
}
|
|
case (PING_SIZE): // ping packet
|
|
{
|
|
ping_packet_t in = (ping_packet_t)r.constData();
|
|
if (in->type == 0x07)
|
|
{
|
|
// It is a ping request/response
|
|
if (in->reply == 0x00)
|
|
{
|
|
ping_packet p;
|
|
memset(p.packet, 0x0, sizeof(p)); // We can't be sure it is initialized with 0x00!
|
|
p.len = sizeof(p);
|
|
p.type = 0x07;
|
|
p.sentid = myId;
|
|
p.rcvdid = remoteId;
|
|
p.reply = 0x01;
|
|
p.seq = in->seq;
|
|
p.time = in->time;
|
|
udpMutex.lock();
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p.packet, sizeof(p)), radioIP, port);
|
|
udpMutex.unlock();
|
|
}
|
|
else if (in->reply == 0x01) {
|
|
if (in->seq == pingSendSeq)
|
|
{
|
|
// This is response to OUR request so increment counter
|
|
pingSendSeq++;
|
|
}
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << "Unhandled response to ping. I have never seen this! 0x10=" << r[16];
|
|
}
|
|
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
|
|
// All packets "should" be added to the incoming buffer.
|
|
// First check that we haven't already received it.
|
|
|
|
|
|
}
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
// All packets except ping and retransmit requests should trigger this.
|
|
control_packet_t in = (control_packet_t)r.constData();
|
|
|
|
// This is a variable length retransmit request!
|
|
if (in->type == 0x01 && in->len != 0x10)
|
|
{
|
|
|
|
for (quint16 i = 0x10; i < r.length(); i = i + 2)
|
|
{
|
|
quint16 seq = (quint8)r[i] | (quint8)r[i + 1] << 8;
|
|
auto match = txSeqBuf.find(seq);
|
|
if (match == txSeqBuf.end()) {
|
|
qDebug(logUdp()) << this->metaObject()->className() << ": Remote requested packet"
|
|
<< QString("0x%1").arg(seq, 0, 16) <<
|
|
"not found, have " << QString("0x%1").arg(txSeqBuf.firstKey(), 0, 16) <<
|
|
"to" << QString("0x%1").arg(txSeqBuf.lastKey(), 0, 16);
|
|
// Just send idle packet.
|
|
sendControl(false, 0, seq);
|
|
}
|
|
else {
|
|
// Found matching entry?
|
|
// Send "untracked" as it has already been sent once.
|
|
qDebug(logUdp()) << this->metaObject()->className() << ": Sending (multiple packet) retransmit of " << QString("0x%1").arg(match->seqNum, 0, 16);
|
|
match->retransmitCount++;
|
|
udpMutex.lock();
|
|
udp->writeDatagram(match->data, radioIP, port);
|
|
udpMutex.unlock();
|
|
packetsLost++;
|
|
congestion = static_cast<double>(packetsSent) / packetsLost * 100;
|
|
}
|
|
}
|
|
}
|
|
else if (in->len != PING_SIZE && in->type == 0x00 && in->seq != 0x00)
|
|
{
|
|
rxBufferMutex.lock();
|
|
if (rxSeqBuf.isEmpty()) {
|
|
rxSeqBuf.insert(in->seq, QTime::currentTime());
|
|
}
|
|
else
|
|
{
|
|
if (in->seq < rxSeqBuf.firstKey() || in->seq - rxSeqBuf.lastKey() > MAX_MISSING)
|
|
{
|
|
qDebug(logUdp()) << this->metaObject()->className() << "Large seq number gap detected, previous highest: " <<
|
|
QString("0x%1").arg(rxSeqBuf.lastKey(), 0, 16) << " current: " << QString("0x%1").arg(in->seq, 0, 16);
|
|
//seqPrefix++;
|
|
// Looks like it has rolled over so clear buffer and start again.
|
|
rxSeqBuf.clear();
|
|
// Add this packet to the incoming buffer
|
|
rxSeqBuf.insert(in->seq, QTime::currentTime());
|
|
rxBufferMutex.unlock();
|
|
missingMutex.lock();
|
|
rxMissing.clear();
|
|
missingMutex.unlock();
|
|
return;
|
|
}
|
|
|
|
if (!rxSeqBuf.contains(in->seq))
|
|
{
|
|
// Add incoming packet to the received buffer and if it is in the missing buffer, remove it.
|
|
|
|
if (in->seq > rxSeqBuf.lastKey() + 1) {
|
|
qDebug(logUdp()) << this->metaObject()->className() << "1 or more missing packets detected, previous: " <<
|
|
QString("0x%1").arg(rxSeqBuf.lastKey(), 0, 16) << " current: " << QString("0x%1").arg(in->seq, 0, 16);
|
|
// We are likely missing packets then!
|
|
missingMutex.lock();
|
|
//int missCounter = 0;
|
|
// Sanity check!
|
|
for (quint16 f = rxSeqBuf.lastKey() + 1; f <= in->seq; f++)
|
|
{
|
|
if (rxSeqBuf.size() > BUFSIZE)
|
|
{
|
|
rxSeqBuf.erase(rxSeqBuf.begin());
|
|
}
|
|
rxSeqBuf.insert(f, QTime::currentTime());
|
|
if (f != in->seq && !rxMissing.contains(f))
|
|
{
|
|
rxMissing.insert(f, 0);
|
|
}
|
|
}
|
|
missingMutex.unlock();
|
|
}
|
|
else {
|
|
if (rxSeqBuf.size() > BUFSIZE)
|
|
{
|
|
rxSeqBuf.erase(rxSeqBuf.begin());
|
|
}
|
|
rxSeqBuf.insert(in->seq, QTime::currentTime());
|
|
|
|
}
|
|
}
|
|
else {
|
|
// This is probably one of our missing packets!
|
|
missingMutex.lock();
|
|
auto s = rxMissing.find(in->seq);
|
|
if (s != rxMissing.end())
|
|
{
|
|
qDebug(logUdp()) << this->metaObject()->className() << ": Missing SEQ has been received! " << QString("0x%1").arg(in->seq, 0, 16);
|
|
|
|
s = rxMissing.erase(s);
|
|
}
|
|
missingMutex.unlock();
|
|
|
|
}
|
|
|
|
}
|
|
rxBufferMutex.unlock();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
void udpBase::sendRetransmitRequest()
|
|
{
|
|
// Find all gaps in received packets and then send requests for them.
|
|
// This will run every 100ms so out-of-sequence packets will not trigger a retransmit request.
|
|
if (rxMissing.isEmpty()) {
|
|
return;
|
|
}
|
|
else if (rxMissing.size() > MAX_MISSING) {
|
|
qInfo(logUdp()) << "Too many missing packets," << rxMissing.size() << "flushing all buffers";
|
|
missingMutex.lock();
|
|
rxMissing.clear();
|
|
missingMutex.unlock();
|
|
|
|
rxBufferMutex.lock();
|
|
rxSeqBuf.clear();
|
|
rxBufferMutex.unlock();
|
|
return;
|
|
}
|
|
|
|
QByteArray missingSeqs;
|
|
|
|
missingMutex.lock();
|
|
auto it = rxMissing.begin();
|
|
while (it != rxMissing.end())
|
|
{
|
|
if (it.key() != 0x0)
|
|
{
|
|
if (it.value() < 4)
|
|
{
|
|
missingSeqs.append(it.key() & 0xff);
|
|
missingSeqs.append(it.key() >> 8 & 0xff);
|
|
missingSeqs.append(it.key() & 0xff);
|
|
missingSeqs.append(it.key() >> 8 & 0xff);
|
|
it.value()++;
|
|
it++;
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": No response for missing packet" << QString("0x%1").arg(it.key(), 0, 16) << "deleting";
|
|
it = rxMissing.erase(it);
|
|
}
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": found empty key in missing buffer";
|
|
it++;
|
|
}
|
|
}
|
|
missingMutex.unlock();
|
|
|
|
if (missingSeqs.length() != 0)
|
|
{
|
|
control_packet p;
|
|
memset(p.packet, 0x0, sizeof(p)); // We can't be sure it is initialized with 0x00!
|
|
p.len = sizeof(p);
|
|
p.type = 0x01;
|
|
p.seq = 0x0000;
|
|
p.sentid = myId;
|
|
p.rcvdid = remoteId;
|
|
if (missingSeqs.length() == 4) // This is just a single missing packet so send using a control.
|
|
{
|
|
p.seq = (missingSeqs[0] & 0xff) | (quint16)(missingSeqs[1] << 8);
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": sending request for missing packet : " << QString("0x%1").arg(p.seq, 0, 16);
|
|
udpMutex.lock();
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p.packet, sizeof(p)), radioIP, port);
|
|
udpMutex.unlock();
|
|
}
|
|
else
|
|
{
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": sending request for multiple missing packets : " << missingSeqs.toHex(':');
|
|
missingMutex.lock();
|
|
p.len = (quint32)sizeof(p) + missingSeqs.size();
|
|
missingSeqs.insert(0, p.packet, sizeof(p));
|
|
missingMutex.unlock();
|
|
|
|
udpMutex.lock();
|
|
udp->writeDatagram(missingSeqs, radioIP, port);
|
|
udpMutex.unlock();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Used to send idle and other "control" style messages
|
|
void udpBase::sendControl(bool tracked = true, quint8 type = 0, quint16 seq = 0)
|
|
{
|
|
control_packet p;
|
|
memset(p.packet, 0x0, sizeof(p)); // We can't be sure it is initialized with 0x00!
|
|
p.len = sizeof(p);
|
|
p.type = type;
|
|
p.sentid = myId;
|
|
p.rcvdid = remoteId;
|
|
|
|
if (!tracked) {
|
|
p.seq = seq;
|
|
udpMutex.lock();
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p.packet, sizeof(p)), radioIP, port);
|
|
udpMutex.unlock();
|
|
}
|
|
else {
|
|
sendTrackedPacket(QByteArray::fromRawData((const char*)p.packet, sizeof(p)));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Send periodic ping packets
|
|
void udpBase::sendPing()
|
|
{
|
|
ping_packet p;
|
|
memset(p.packet, 0x0, sizeof(p)); // We can't be sure it is initialized with 0x00!
|
|
p.len = sizeof(p);
|
|
p.type = 0x07;
|
|
p.sentid = myId;
|
|
p.rcvdid = remoteId;
|
|
p.seq = pingSendSeq;
|
|
QTime now = QTime::currentTime();
|
|
p.time = (quint32)now.msecsSinceStartOfDay();
|
|
lastPingSentTime = QDateTime::currentDateTime();
|
|
udpMutex.lock();
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p.packet, sizeof(p)), radioIP, port);
|
|
udpMutex.unlock();
|
|
return;
|
|
}
|
|
|
|
|
|
void udpBase::sendTrackedPacket(QByteArray d)
|
|
{
|
|
// As the radio can request retransmission of these packets, store them in a buffer
|
|
d[6] = sendSeq & 0xff;
|
|
d[7] = (sendSeq >> 8) & 0xff;
|
|
SEQBUFENTRY s;
|
|
s.seqNum = sendSeq;
|
|
s.timeSent = QTime::currentTime();
|
|
s.retransmitCount = 0;
|
|
s.data = d;
|
|
if (txBufferMutex.tryLock(100))
|
|
{
|
|
|
|
if (sendSeq == 0) {
|
|
// We are either the first ever sent packet or have rolled-over so clear the buffer.
|
|
txSeqBuf.clear();
|
|
congestion = 0;
|
|
}
|
|
if (txSeqBuf.size() > BUFSIZE)
|
|
{
|
|
txSeqBuf.erase(txSeqBuf.begin());
|
|
}
|
|
txSeqBuf.insert(sendSeq, s);
|
|
|
|
txBufferMutex.unlock();
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": txBuffer mutex is locked";
|
|
}
|
|
// Stop using purgeOldEntries() as it is likely slower than just removing the earliest packet.
|
|
//qInfo(logUdp()) << this->metaObject()->className() << "RX:" << rxSeqBuf.size() << "TX:" <<txSeqBuf.size() << "MISS:" << rxMissing.size();
|
|
//purgeOldEntries(); // Delete entries older than PURGE_SECONDS seconds (currently 5)
|
|
sendSeq++;
|
|
|
|
udpMutex.lock();
|
|
udp->writeDatagram(d, radioIP, port);
|
|
|
|
/*if (congestion > 10) { // Poor quality connection?
|
|
udp->writeDatagram(d, radioIP, port);
|
|
if (congestion > 20) // Even worse so send again.
|
|
udp->writeDatagram(d, radioIP, port);
|
|
} */
|
|
if (idleTimer != Q_NULLPTR && idleTimer->isActive()) {
|
|
idleTimer->start(IDLE_PERIOD); // Reset idle counter if it's running
|
|
}
|
|
udpMutex.unlock();
|
|
packetsSent++;
|
|
return;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Once a packet has reached PURGE_SECONDS old (currently 10) then it is not likely to be any use.
|
|
/// </summary>
|
|
void udpBase::purgeOldEntries()
|
|
{
|
|
// Erase old entries from the tx packet buffer
|
|
if (txBufferMutex.tryLock(100))
|
|
{
|
|
if (!txSeqBuf.isEmpty())
|
|
{
|
|
// Loop through the earliest items in the buffer and delete if older than PURGE_SECONDS
|
|
for (auto it = txSeqBuf.begin(); it != txSeqBuf.end();) {
|
|
if (it.value().timeSent.secsTo(QTime::currentTime()) > PURGE_SECONDS) {
|
|
txSeqBuf.erase(it++);
|
|
}
|
|
else {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
txBufferMutex.unlock();
|
|
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": txBuffer mutex is locked";
|
|
}
|
|
|
|
|
|
|
|
if (rxBufferMutex.tryLock(100))
|
|
{
|
|
if (!rxSeqBuf.isEmpty()) {
|
|
// Loop through the earliest items in the buffer and delete if older than PURGE_SECONDS
|
|
for (auto it = rxSeqBuf.begin(); it != rxSeqBuf.end();) {
|
|
if (it.value().secsTo(QTime::currentTime()) > PURGE_SECONDS) {
|
|
rxSeqBuf.erase(it++);
|
|
}
|
|
else {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
rxBufferMutex.unlock();
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": rxBuffer mutex is locked";
|
|
}
|
|
|
|
if (missingMutex.tryLock(100))
|
|
{
|
|
// Erase old entries from the missing packets buffer
|
|
if (!rxMissing.isEmpty() && rxMissing.size() > 50) {
|
|
for (size_t i = 0; i < 25; ++i) {
|
|
rxMissing.erase(rxMissing.begin());
|
|
}
|
|
}
|
|
missingMutex.unlock();
|
|
}
|
|
else {
|
|
qInfo(logUdp()) << this->metaObject()->className() << ": missingBuffer mutex is locked";
|
|
}
|
|
}
|
|
|
|
void udpBase::printHex(const QByteArray& pdata)
|
|
{
|
|
printHex(pdata, false, true);
|
|
}
|
|
|
|
void udpBase::printHex(const QByteArray& pdata, bool printVert, bool printHoriz)
|
|
{
|
|
qDebug(logUdp()) << "---- Begin hex dump -----:";
|
|
QString sdata("DATA: ");
|
|
QString index("INDEX: ");
|
|
QStringList strings;
|
|
|
|
for (int i = 0; i < pdata.length(); i++)
|
|
{
|
|
strings << QString("[%1]: %2").arg(i, 8, 10, QChar('0')).arg((unsigned char)pdata[i], 2, 16, QChar('0'));
|
|
sdata.append(QString("%1 ").arg((unsigned char)pdata[i], 2, 16, QChar('0')));
|
|
index.append(QString("%1 ").arg(i, 2, 10, QChar('0')));
|
|
}
|
|
|
|
if (printVert)
|
|
{
|
|
for (int i = 0; i < strings.length(); i++)
|
|
{
|
|
//sdata = QString(strings.at(i));
|
|
qDebug(logUdp()) << strings.at(i);
|
|
}
|
|
}
|
|
|
|
if (printHoriz)
|
|
{
|
|
qDebug(logUdp()) << index;
|
|
qDebug(logUdp()) << sdata;
|
|
}
|
|
qDebug(logUdp()) << "----- End hex dump -----";
|
|
}
|
|
|
|
|