From 831055b0d1101ce5909fea7cc3b00252954f63bf Mon Sep 17 00:00:00 2001 From: hm Date: Fri, 23 Sep 2022 09:07:50 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=AF=E6=8C=81=E5=A4=9A=E4=B8=AA=E9=A3=9E?= =?UTF-8?q?=E6=9C=BA=E8=BF=9E=E6=8E=A5=EF=BC=8C=E6=8C=87=E4=BB=A4=E5=8F=91?= =?UTF-8?q?=E9=80=81=EF=BC=8C=E6=8E=A5=E6=94=B6=E7=AD=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- App/CommandUI/CommandUI.cpp | 16 +- App/MenuBarUI/MenuBarUI.cpp | 23 ++- App/MenuBarUI/MenuBarUI.h | 15 +- App/mainwindow.cpp | 28 ++-- MavLinkNode/missionprocess.cpp | 4 +- MavLinkNode/missionprocess.h | 2 +- dlink/dlink.cpp | 275 ++++++++++++++++++++++---------- dlink/dlink.h | 42 ++++- opmap/mapwidget/opmapwidget.cpp | 65 ++++++-- opmap/mapwidget/opmapwidget.h | 7 +- 10 files changed, 348 insertions(+), 129 deletions(-) diff --git a/App/CommandUI/CommandUI.cpp b/App/CommandUI/CommandUI.cpp index 3e93986..59f814b 100644 --- a/App/CommandUI/CommandUI.cpp +++ b/App/CommandUI/CommandUI.cpp @@ -907,17 +907,25 @@ void CommandUI::on_pushButton_setUAV_clicked() { if(ui->comboBox_uav->currentIndex() != -1) { - int seq = ui->comboBox_uav->currentData(Qt::UserRole).toInt(); + int index = ui->comboBox_uav->currentData(Qt::UserRole).toInt(); //根据这个comfirm 确定要输入的参数 Confirm *confirmor = new Confirm(this); confirmor->setGeometry(0,0,this->width(),this->height()); //设置警告界面 - confirmor->setNotice(tr("click confirm to set uav %1 as current").arg(seq)); + confirmor->setNotice(tr("click confirm to set uav %1 as current").arg(index)); + + connect(confirmor,&Confirm::confirmValue,[=](QVariant flag){ + + if(flag.toBool() == true) + { + CurrentUAV(index,0); + emit SetCurrentUAV(index); + } + + }); - connect(confirmor,SIGNAL(confirmValue(QVariant)), - this,SLOT(setCurrent(QVariant))); confirmor->show(); } diff --git a/App/MenuBarUI/MenuBarUI.cpp b/App/MenuBarUI/MenuBarUI.cpp index fc0c468..f9b663e 100644 --- a/App/MenuBarUI/MenuBarUI.cpp +++ b/App/MenuBarUI/MenuBarUI.cpp @@ -262,12 +262,27 @@ void MenuBarUI::MessageTimeOut(void) -void MenuBarUI::setTargetPoint(int Value) +void MenuBarUI::setTargetPoint(int sysid,int Value) { - int group = Value /1000; - int seq = Value %1000; + //qDebug() << "sysid menubar" << sysid << sysID; + if(sysID == sysid) + { + int group = Value /1000; + int seq = Value %1000; - ui->label_TargetPoint->setText(tr("%1Group%2Point").arg(QString::number(group + 1)).arg(QString::number(seq,'f',0))); + ui->label_TargetPoint->setText(tr("%1Group%2Point").arg(QString::number(group + 1)).arg(QString::number(seq,'f',0))); + + } +} + + +void MenuBarUI::uav_selected(int sysid,int comp) +{ + Q_UNUSED(comp) + if(sysid > 0) + { + sysID = sysid; + } } void MenuBarUI::setTargetAlt(QVariant Value) diff --git a/App/MenuBarUI/MenuBarUI.h b/App/MenuBarUI/MenuBarUI.h index 28d9b32..c747da6 100644 --- a/App/MenuBarUI/MenuBarUI.h +++ b/App/MenuBarUI/MenuBarUI.h @@ -44,7 +44,7 @@ public slots: - void setTargetPoint(int Value); + void setTargetPoint(int sysid, int Value); void setTargetAlt(QVariant Value); void setX(float Value); void setwp_Dist(float Value); @@ -63,6 +63,17 @@ public slots: } + void setsysid(int value) + { + if(value > 0) + { + sysID = value; + } + + } + + void uav_selected(int sysid,int comp); + signals: void IndexChanged(int); @@ -103,6 +114,8 @@ private: int missionGroup = 3; + int sysID = 1; + }; diff --git a/App/mainwindow.cpp b/App/mainwindow.cpp index 70d3a43..b214677 100644 --- a/App/mainwindow.cpp +++ b/App/mainwindow.cpp @@ -545,16 +545,24 @@ MainWindow::MainWindow(QWidget *parent) map,SLOT(find_PointNumber(int))); - connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int)), - map,SLOT(WPSetCurrent(int))); + connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int,int)), + map,SLOT(WPSetCurrent(int,int))); + + + connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int,int)), + menuBarUI,SLOT(setTargetPoint(int,int))); + + connect(commandUI,SIGNAL(SetCurrentUAV(int)), + map,SLOT(setUAVCurrent(int))); + + connect(map,SIGNAL(uav_selected(int,int)), + menuBarUI,SLOT(uav_selected(int,int)),Qt::DirectConnection); - connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int)), - menuBarUI,SLOT(setTargetPoint(int))); /* - connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int)), - toolsui->diagram,SLOT(setTargetPoint(int))); + connect(dlink->mavlinknode->Mission,SIGNAL(currentPoint(int,int)), + toolsui->diagram,SLOT(setTargetPoint(int,int))); */ @@ -643,7 +651,6 @@ MainWindow::~MainWindow() if(dlink) { - //dlink->stopPort(); delete dlink; } @@ -999,8 +1006,11 @@ void MainWindow::showMessage(const QString &message, int TimeOut) void MainWindow::beep(int id) { - Q_UNUSED(id) - QApplication::beep(); + if(map->getUAVCurrent() == id) + { + QApplication::beep(); + } + } diff --git a/MavLinkNode/missionprocess.cpp b/MavLinkNode/missionprocess.cpp index 7c18b9c..49b9f91 100644 --- a/MavLinkNode/missionprocess.cpp +++ b/MavLinkNode/missionprocess.cpp @@ -382,8 +382,8 @@ void MissionProcess::Parse(mavlink_message_t msg) int seq = mission_current.seq %1000; //emit currentGroup(group); - // emit currentPoint(seq + 1); - emit currentPoint(mission_current.seq + 1); + // emit currentPoint(seq + 1); + emit currentPoint(msg.sysid,mission_current.seq + 1); }break; case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { diff --git a/MavLinkNode/missionprocess.h b/MavLinkNode/missionprocess.h index 0c70188..b3cfd66 100644 --- a/MavLinkNode/missionprocess.h +++ b/MavLinkNode/missionprocess.h @@ -184,7 +184,7 @@ signals: uint8_t mission_type); //void currentGroup(int group); - void currentPoint(int seq); + void currentPoint(int sysid,int seq); private: diff --git a/dlink/dlink.cpp b/dlink/dlink.cpp index 6a7f4b4..8fd7ce9 100644 --- a/dlink/dlink.cpp +++ b/dlink/dlink.cpp @@ -37,7 +37,29 @@ DLink::DLink(QObject *parent) : QObject(parent) } DLink::~DLink() { - stopPort(); + foreach (serialNode node, serials) { + if(node.port) + { + if(node.port->isOpen()) + { + node.port->close(); + } + delete node.port; + node.port = nullptr; + } + } + + foreach(clientNode node,clientSockets) + { + if(node.sock) + { + node.sock->leaveMulticastGroup(node.addr); + node.sock->abort(); + qWarning("leave multicast Group"); + delete node.sock; + node.sock = nullptr; + } + } delete mavlinknode; mavlinknode = nullptr; @@ -86,29 +108,46 @@ int DLink::SendMessageTo(quint8 ch, quint8 *msg, quint16 len) Byte_Out_ALL += len; - //更加ch选择 - if (DLink::Clientsock) + foreach(clientNode node,clientSockets) { - foreach(Node node,clientSockets) + if(node.sock->isOpen()||node.sock->isWritable()||node.sock->isValid()) { - qint64 flag = DLink::Clientsock->writeDatagram((const char *)msg,len,node.addr, node.port); - if(flag != -1) + if(node.type == devtype::datalink) { - //qDebug() << "send"; + qint64 flag = node.sock->writeDatagram((const char *)msg,len,node.addr, node.port); + if(flag != -1) + { + //qDebug() << "Client Send Msg"; + } + else + { + qDebug() << tr("udp Send Msg failure"); + } } } } - if (DLink::serialPort) - { - qint64 flag = DLink::serialPort->write((const char *)msg,len); - if(flag != -1) - { - //Byte_Out_ALL += flag; - } + foreach (serialNode node, serials) { + if(node.port) + { + if(node.port->isOpen()||node.port->isWritable()) + { + if(node.type == devtype::datalink) + { + qint64 flag = node.port->write((const char *)msg,len); + if(flag != -1) + { + //qDebug() << "serialPort Send Msg"; + } + else + { + qDebug() << tr("serialPort Send Msg failure"); + } + } + } + } } - if (multicast_sock) { qint64 flag = multicast_sock->writeDatagram((const char *)msg, len,multicast_transmit_addr, multicast_transmit_port); @@ -125,21 +164,31 @@ int DLink::SendMessageTo(quint8 ch, quint8 *msg, quint16 len) //这个函数就在本线程内运行 bool DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity parity) { - - qWarning() << port - << baudrate - << parity; - bool isSuccess = false; - if (serialPort)//多串口接入 - { - serialPort->close(); - delete serialPort; - } - serialPort = new QSerialPort(this); + foreach (serialNode node, serials) { + if(node.port) + { + if(node.name == port) + { + if(node.port->isOpen()) + { + node.port->close(); + } - serialPort->setPortName(port); + //断掉所有的连接 + disconnect(node.port,nullptr,nullptr,nullptr); + + delete node.port; + node.port = nullptr; + + + serials.removeAt(serials.indexOf(node)); + } + } + } + + QSerialPort *serialPort = new QSerialPort(port,this); if (serialPort->open(QIODevice::ReadWrite)) { @@ -148,11 +197,21 @@ bool DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity p serialPort->setDataBits(QSerialPort::Data8); serialPort->setStopBits(QSerialPort::OneStop); - DLink::serialPort->write("serial connet ok",16); - - connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagramsSerialPort()));//本线程内调用 isSuccess = true; + serialNode node; + + node.port = serialPort; + node.baud = baudrate; + node.parity = parity; + node.name = port; + node.type = devtype::datalink; + + serials.append(node); + + connect(serialPort, SIGNAL(readyRead()), + this, SLOT(readPendingDatagramsSerialPort()));//本线程内调用 + emit showMessage(tr("Serial Port Open Success")); qWarning() << "Serial Port Open Success"; } @@ -166,6 +225,7 @@ bool DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity p } return isSuccess; + } bool DLink::statesPort() @@ -200,8 +260,6 @@ void DLink::connectSignal(QVariant m_state, if(m_state.toBool() == true) { - - if(m_Type.toString() == "SerialPort") { QSerialPort::Parity parity; @@ -261,30 +319,46 @@ void DLink::connectSignal(QVariant m_state, qWarning() << "disconnect"; if(m_Type.toString() == "SerialPort") { - stopPort(); + foreach (serialNode node, serials) { + if(node.port) + { + if(node.name == m_Param1.toString()) + { + if(node.port->isOpen()) + { + emit showMessage(tr("%1 close").arg(node.name)); + node.port->close(); + } + + //断掉所有的连接 + disconnect(node.port,nullptr,nullptr,nullptr); + + delete node.port; + node.port = nullptr; + + serials.removeOne(node); + } + } + } + emit PortConnected(false,m_usrName,m_Type,m_Param1,m_Param2,m_Param3,m_Param4,m_Param5); } else if(m_Type.toString() == "UDP") { //断开连接 - if(Clientsock) + for(int i = 0;i < clientSockets.count();i++) { - for(int i = 0;i < clientSockets.count();i++) + if((clientSockets.at(i).addr == QHostAddress(m_Param3.toString()))&&(clientSockets.at(i).port == m_Param4.toInt())) { - qDebug() << clientSockets.at(i).addr; - if(clientSockets.at(i).addr == QHostAddress(m_Param3.toString())) - { - clientSockets.removeAt(i); - } + clientSockets.at(i).sock->leaveMulticastGroup(QHostAddress(m_Param3.toString())); + clientSockets.at(i).sock->close(); + qWarning("leave multicast Group"); + clientSockets.at(i).sock->deleteLater(); + clientSockets.removeAt(i); } - Clientsock->leaveMulticastGroup(QHostAddress(m_Param3.toString())); - Clientsock->close(); - qWarning("leave multicast Group"); - delete Clientsock; - Clientsock = nullptr; } emit PortConnected(false,m_usrName,m_Type,m_Param1,m_Param2,m_Param3,m_Param4,m_Param5); @@ -308,39 +382,47 @@ void DLink::connectSignal(QVariant m_state, bool DLink::setupClient(const QHostAddress &local_addr,int local_port,const QHostAddress &remote_addr,int remote_port) { - if(Clientsock) - { - Clientsock->disconnect(); - delete Clientsock; - Clientsock = nullptr; + bool isSuccess = false; + + foreach (clientNode node, clientSockets) { + if(node.sock) + { + if((node.addr == remote_addr)&&(node.port == remote_port)) + { + if(node.sock->isOpen()) + { + node.sock->leaveMulticastGroup(node.addr); + node.sock->close(); + } + //断掉所有的连接 + disconnect(node.sock,nullptr,nullptr,nullptr); + + node.sock->deleteLater(); + //node.sock = nullptr; + + clientSockets.removeAt(clientSockets.indexOf(node)); + + } + } } - qDebug() << "sock connect"; - bool isSuccess = false; - Clientsock = new QUdpSocket(this); - - Clientsock->setSocketOption(QAbstractSocket::MulticastLoopbackOption,1); - Clientsock->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption,1024*1024*8); - //QHostAddress::AnyIPv4 - int rst = Clientsock->bind(QHostAddress::AnyIPv4,local_port, QUdpSocket::ShareAddress|QUdpSocket::ReuseAddressHint); - - qDebug() << "rst" << rst; + QUdpSocket *clientsock = new QUdpSocket(this); + int rst = clientsock->open(QIODevice::ReadWrite); if(rst) { - Clientsock->joinMulticastGroup(remote_addr); + clientsock->setSocketOption(QAbstractSocket::MulticastLoopbackOption,1); + clientsock->setSocketOption(QAbstractSocket::ReceiveBufferSizeSocketOption,1024*1024*8); + clientsock->setProxy(QNetworkProxy::NoProxy); - if(Clientsock->open(QIODevice::ReadWrite)) - //if(Clientsock->joinMulticastGroup(remote_addr)) + if(clientsock->bind(local_addr,local_port,QAbstractSocket::ShareAddress|QUdpSocket::ReuseAddressHint)) { - connect(Clientsock, SIGNAL(readyRead()), - this, SLOT(readPendingDatagramsClient())); - - Node node; - node.sock = Clientsock; + clientNode node; + node.sock = clientsock; node.addr = remote_addr; node.port = remote_port; + node.type = devtype::datalink; clientSockets.append(node); qWarning() << "UdpSocket open:" @@ -349,26 +431,31 @@ bool DLink::setupClient(const QHostAddress &local_addr,int local_port,const QHos << remote_port; isSuccess = true; + + + connect(clientsock, SIGNAL(readyRead()), + this, SLOT(readPendingDatagramsClient())); + + //clientsock->disconnectFromHost(); + emit showMessage(tr("UdpSocket open")); } else { - //Clientsock->leaveMulticastGroup(remote_addr); - Clientsock->close(); + clientsock->close(); isSuccess = false; - qWarning() << "sock not open"; - delete Clientsock; - Clientsock = nullptr; + qWarning() << "sock open failure" << clientsock->state(); + delete clientsock; + clientsock = nullptr; } } else { isSuccess = false; - //Clientsock->leaveMulticastGroup(remote_addr); - Clientsock->close(); + clientsock->close(); qWarning("sock is not binded."); - delete Clientsock; - Clientsock = nullptr; + delete clientsock; + clientsock = nullptr; } return isSuccess; @@ -425,10 +512,17 @@ bool DLink::setup_multicast(const QString &recieveIP, qint32 recievePort, void DLink::readPendingDatagramsSerialPort(void) { - if(serialPort) + QObject *obj = sender(); + + if(obj) { - QByteArray datagram = serialPort->readAll(); - emit recieveMessage(SourceType::s_port,datagram); + QSerialPort *serialport = qobject_cast(obj); + if(serialport) + { + QByteArray datagram = serialport->readAll(); + + emit recieveMessage(SourceType::s_port,datagram); + } } } @@ -437,16 +531,23 @@ void DLink::readPendingDatagramsClient(void) QElapsedTimer time; time.start(); //轮番查询 - if(Clientsock) - { - while (Clientsock->hasPendingDatagrams()) { - QByteArray datagram; - datagram.resize(Clientsock->pendingDatagramSize()); - Clientsock->readDatagram(datagram.data(), datagram.size()); + QObject *obj = sender(); - emit recieveMessage(SourceType::c_sock,datagram); + if(obj) + { + QUdpSocket *clientsock = qobject_cast(obj); + if(clientsock) + { + while (clientsock->hasPendingDatagrams()) { + QByteArray datagram; + datagram.resize(clientsock->pendingDatagramSize()); + clientsock->readDatagram(datagram.data(), datagram.size()); + + emit recieveMessage(SourceType::c_sock,datagram); + } } } + int milsec = time.elapsed(); //qDebug() << "a loop using time:" << milsec << "ms"; diff --git a/dlink/dlink.h b/dlink/dlink.h index 58fe964..a143de5 100644 --- a/dlink/dlink.h +++ b/dlink/dlink.h @@ -28,12 +28,48 @@ class DLink : public QObject Q_OBJECT //Q_DECLARE_PRIVATE(DLink) - struct Node{ + enum devtype{ + ganeral = 0, + datalink, + remote, + rtk, + video + }; + + struct clientNode{ QUdpSocket * sock; QHostAddress addr; quint16 port; + devtype type; + + bool operator==(const clientNode& other) + { + if (this->sock == other.sock) + { + return true; + } + return false; + } }; + struct serialNode{ + QSerialPort * port; + QString name; + qint32 baud; + QSerialPort::Parity parity; + devtype type; + + bool operator==(const serialNode& other) + { + if (this->name == other.name) + { + return true; + } + return false; + } + }; + + public: explicit DLink(QObject *parent = nullptr); @@ -112,7 +148,9 @@ protected: QSerialPort *serialPort = nullptr; QUdpSocket *Clientsock = nullptr; - QList clientSockets; + + QList clientSockets; + QList serials; QUdpSocket * multicast_sock=NULL; diff --git a/opmap/mapwidget/opmapwidget.cpp b/opmap/mapwidget/opmapwidget.cpp index f320ab4..7e1fe81 100644 --- a/opmap/mapwidget/opmapwidget.cpp +++ b/opmap/mapwidget/opmapwidget.cpp @@ -469,6 +469,22 @@ void OPMapWidget::setUAVSpeed(int sysid,int compid,qreal Ma,qreal Speed) } } +void OPMapWidget::setUAVCurrent(int sysid) +{ + foreach(QGraphicsItem * i, map->childItems()) { + UAVItem *u = qgraphicsitem_cast(i); + if (u) { + if(u->SysID() == sysid) + { + u->setSelect(true); + } + else + { + u->setSelect(false); + } + } + } +} int OPMapWidget::getUAVCurrent(void) @@ -1656,7 +1672,7 @@ void OPMapWidget::userRipMap(bool start, qreal lat_l, qreal lng_l, qreal lat_r, if(start == true) { - //生成16个线程 + //根据CPU数量分配线程，例如生成16个线程 //每个线程分配不同的矩形 @@ -1859,25 +1875,32 @@ void OPMapWidget::setWPLock(bool const & lock) } -void OPMapWidget::WPSetCurrent(int seq) +void OPMapWidget::WPSetCurrent(int sysid,int seq) { - int group = seq /1000 + 3; - int num = seq %1000; - groupchanged(group); - foreach(QGraphicsItem * i, map->childItems()) { - WayPointItem *w = qgraphicsitem_cast(i); - if (w) { - if(w->MissionType() == group) - { - if(w->Number() == num) + if(getUAVCurrent() == sysid) + { + int group = seq /1000 + 3; + int num = seq %1000; + + //qDebug() << "sysid opmap" << sysid << getUAVCurrent() << group << num; + + groupchanged(group); + + foreach(QGraphicsItem * i, map->childItems()) { + WayPointItem *w = qgraphicsitem_cast(i); + if (w) { + if(w->MissionType() == group) { - w->WPSetCurrent(true); - } - else - { - w->WPSetCurrent(false); + if(w->Number() == num) + { + w->WPSetCurrent(true); + } + else + { + w->WPSetCurrent(false); + } } } } @@ -3566,6 +3589,11 @@ void OPMapWidget::vehicleChanged(float param1,float param2,float param3,float pa w->emitWPProperty(); + if(isWPCreate == false) + { + w->WPSetEdit(false); + } + } } } @@ -3589,6 +3617,7 @@ void OPMapWidget::vehicleChanged(float param1,float param2,float param3,float pa emit WPCreated(position, Item); setOverlayOpacity(overlayOpacity); + setSelectedWP(Item); emit setWPProperty(param1,param2,param3,param4, x,y,z, @@ -3606,6 +3635,10 @@ void OPMapWidget::vehicleChanged(float param1,float param2,float param3,float pa Item->emitWPProperty(); Item->setisShowTip(isShowTip); + if(isWPCreate == false) + { + Item->WPSetEdit(false); + } //===========连线 WayPointItem *w_last = WPFind(Item->MissionType(),Item->Number() - 1); diff --git a/opmap/mapwidget/opmapwidget.h b/opmap/mapwidget/opmapwidget.h index 80ff4f7..ccf4c86 100644 --- a/opmap/mapwidget/opmapwidget.h +++ b/opmap/mapwidget/opmapwidget.h @@ -463,6 +463,8 @@ public: void setUAVPos(int sysid,int compid,double x,double y,double z); void setUAVHeading(int sysid,int compid,float Heading); void setUAVSpeed(int sysid,int compid,qreal Ma,qreal Speed); + + void DeleteTrail(void); @@ -672,7 +674,6 @@ signals: void measureState(bool); - void createFenceCircle(int group,qreal radius,bool inclusion,qreal lat,qreal lng); void createFencePolygon(int group,qreal vertex,bool inclusion,QList latlng); @@ -722,7 +723,7 @@ public slots: void addUAV(int sysid, int compid); - + void setUAVCurrent(int sysid); void getMapTypes(void); @@ -784,7 +785,7 @@ public slots: void WPFollowPrevious(bool flag,WayPointItem *waypoint); - void WPSetCurrent(int seq); + void WPSetCurrent(int sysid, int seq); void WPLoad(QString path);