148 lines
3.0 KiB
C++
148 lines
3.0 KiB
C++
#include "dlink.h"
|
|
#include <string.h>
|
|
//#include "windows.h"
|
|
#include "QDateTime"
|
|
|
|
|
|
|
|
DLink::DLink(QObject *parent) : QObject(parent)
|
|
{
|
|
qDebug() << "Dlink " << QThread::currentThreadId();
|
|
|
|
mavlinknode = new MavLinkNode();
|
|
}
|
|
DLink::~DLink()
|
|
{
|
|
mavlinknode->stop();
|
|
delete mavlinknode;
|
|
}
|
|
|
|
|
|
int DLink::SendMessageTo(uint8_t, uint8_t *msg, uint16_t len)
|
|
{
|
|
if (DLink::Clientsock)
|
|
{
|
|
foreach(Node node,clientSockets)
|
|
{
|
|
DLink::Clientsock->writeDatagram((const char *)msg, len,
|
|
node.addr, node.port);
|
|
}
|
|
}
|
|
|
|
if (DLink::serialPort)
|
|
{
|
|
DLink::serialPort->write((const char *)msg,len);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
void DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity parity)
|
|
{
|
|
if (serialPort)
|
|
{
|
|
serialPort->close();
|
|
delete serialPort;
|
|
}
|
|
serialPort = new QSerialPort(port,this);
|
|
|
|
if (serialPort->open(QIODevice::ReadWrite))
|
|
{
|
|
serialPort->setBaudRate(baudrate);
|
|
serialPort->setParity(parity);
|
|
serialPort->setDataBits(QSerialPort::Data8);
|
|
serialPort->setStopBits(QSerialPort::OneStop);
|
|
|
|
mavlinknode->start();
|
|
|
|
connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagramsSerialPort()));
|
|
|
|
qDebug() << "Serial Port Open Success";
|
|
}
|
|
else
|
|
{
|
|
delete serialPort;
|
|
serialPort = nullptr;
|
|
}
|
|
}
|
|
|
|
bool DLink::statesPort()
|
|
{
|
|
return (serialPort != nullptr)?(true):(false);
|
|
}
|
|
|
|
void DLink::stopPort()
|
|
{
|
|
serialPort->close();
|
|
delete serialPort;
|
|
serialPort = nullptr;
|
|
}
|
|
|
|
|
|
|
|
void DLink::setupClient(const QHostAddress &remote_addr, int remote_port, int local_port)
|
|
{
|
|
Clientsock = new QUdpSocket(this);
|
|
if (Clientsock->bind(QHostAddress::Any, local_port))
|
|
{
|
|
if(Clientsock->open(QIODevice::ReadWrite))
|
|
{
|
|
connect(Clientsock, SIGNAL(readyRead()),
|
|
this, SLOT(readPendingDatagramsClient()));
|
|
Node node;
|
|
node.sock = Clientsock;
|
|
node.addr = remote_addr;
|
|
node.port = remote_port;
|
|
|
|
clientSockets.append(node);
|
|
qDebug() << "UdpSocket open:"
|
|
<< remote_addr
|
|
<< local_port
|
|
<< remote_port;
|
|
|
|
mavlinknode->start();
|
|
}
|
|
else
|
|
{
|
|
qDebug() << "sock not open";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
qWarning("sock is not binded.");
|
|
delete Clientsock;
|
|
}
|
|
|
|
}
|
|
|
|
void DLink::readPendingDatagramsSerialPort(void)
|
|
{
|
|
QByteArray datagram = serialPort->readAll();
|
|
mavlinknode->setbuff(SourceType::s_port,datagram);
|
|
}
|
|
|
|
void DLink::readPendingDatagramsClient(void)
|
|
{
|
|
//轮番查询
|
|
while (Clientsock->hasPendingDatagrams()) {
|
|
QByteArray datagram;
|
|
datagram.resize(Clientsock->pendingDatagramSize());
|
|
Clientsock->readDatagram(datagram.data(), datagram.size());
|
|
mavlinknode->setbuff(SourceType::c_sock,datagram);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|