|
@@ -0,0 +1,422 @@
|
|
|
+#include "mainwindow.h"
|
|
|
+#include "ui_mainwindow.h"
|
|
|
+
|
|
|
+#include <QMessageBox>
|
|
|
+#include <iostream>
|
|
|
+
|
|
|
+MainWindow::MainWindow(QWidget *parent) :
|
|
|
+ QMainWindow(parent),
|
|
|
+ ui(new Ui::MainWindow)
|
|
|
+{
|
|
|
+ ui->setupUi(this);
|
|
|
+
|
|
|
+ ui->lineEdit_DataPort->setText("2368");
|
|
|
+ ui->lineEdit_DevPort->setText("2369");
|
|
|
+ ui->comboBox_MotorSpeed->addItem("5Hz");
|
|
|
+ ui->comboBox_MotorSpeed->addItem("10Hz");
|
|
|
+ ui->comboBox_MotorSpeed->addItem("20Hz");
|
|
|
+ ui->comboBox_MotorSpeed->setCurrentIndex(1);
|
|
|
+
|
|
|
+ SetParamState();
|
|
|
+
|
|
|
+ QTimer * timer = new QTimer(this);
|
|
|
+ connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
|
|
|
+ timer->start(1000);
|
|
|
+
|
|
|
+// unsigned char strip[4];
|
|
|
+// int nrtn = GetIP("192.168.1.103",strip);
|
|
|
+// nrtn = GetIP(" 192.168.1.103 ",strip);
|
|
|
+// nrtn = GetIP(" 192. 168 . 1. 103 ",strip);
|
|
|
+// nrtn = GetIP(" 192, 168 . 1. 103 ",strip);
|
|
|
+// nrtn = GetIP(" 192. 368 . 1. 103 ",strip);
|
|
|
+
|
|
|
+
|
|
|
+ setWindowTitle(tr("设置镭神C系列激光雷达IP和端口"));
|
|
|
+}
|
|
|
+
|
|
|
+MainWindow::~MainWindow()
|
|
|
+{
|
|
|
+ delete ui;
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::on_pushButton_clicked()
|
|
|
+{
|
|
|
+ if(mbListenning == false)
|
|
|
+ {
|
|
|
+
|
|
|
+ unsigned short nDevPort = static_cast<unsigned short>( atoi(ui->lineEdit_DevPort->text().toLatin1().data()));
|
|
|
+ unsigned short nDataPort = static_cast<unsigned short>(atoi(ui->lineEdit_DataPort->text().toLatin1().data())) ;
|
|
|
+ mpUDPSocket_Dev = new QUdpSocket(this);
|
|
|
+ bool bbindDev = mpUDPSocket_Dev->bind(QHostAddress::Any, nDevPort);
|
|
|
+ if(bbindDev == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("设备端口无法bind"),QMessageBox::YesAll);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ connect(mpUDPSocket_Dev,SIGNAL(readyRead()),this,SLOT(onSocketDevRead()));
|
|
|
+ mpUDPSocket_Data = new QUdpSocket(this);
|
|
|
+ bool bbindData = mpUDPSocket_Data->bind(QHostAddress::Any, nDataPort);
|
|
|
+ if(bbindData == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("数据端口无法bind"),QMessageBox::YesAll);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ connect(mpUDPSocket_Data,SIGNAL(readyRead()),this,SLOT(onSocketDataRead()));
|
|
|
+ mbListenning = true;
|
|
|
+ mbParam_Get = false;
|
|
|
+ ui->pushButton->setText(tr("停止监听"));
|
|
|
+ ui->pushButton_Param_Get->setEnabled(false);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ mpUDPSocket_Dev->close();
|
|
|
+ mpUDPSocket_Data->close();
|
|
|
+
|
|
|
+ mvectorUDPPac_Data.clear();
|
|
|
+ mvectorUDPPac_Dev.clear();
|
|
|
+
|
|
|
+ ui->pushButton->setText(tr("开始监听"));
|
|
|
+ ui->pushButton_Param_Get->setEnabled(false);
|
|
|
+ mbListenning = false;
|
|
|
+ mbParam_Get = false;
|
|
|
+ SetParamState();
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::on_pushButton_Param_Get_clicked()
|
|
|
+{
|
|
|
+ if(mbListenning == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("请先打开监听"),QMessageBox::YesAll);
|
|
|
+ return ;
|
|
|
+ }
|
|
|
+ if(mvectorUDPPac_Dev.size() == 0)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("没有获得设备包"),QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<char> xvecotrfactype;
|
|
|
+ if(mvectorUDPPac_Data.size()>0)
|
|
|
+ {
|
|
|
+ int nsize = static_cast<int>(mvectorUDPPac_Data.size());
|
|
|
+ int i;
|
|
|
+ for(i=0;i<nsize;i++)
|
|
|
+ {
|
|
|
+ char * pdata = mvectorUDPPac_Data[i].mba.data();
|
|
|
+ if(mvectorUDPPac_Data[i].mba.size() == 1206)
|
|
|
+ {
|
|
|
+ xvecotrfactype.push_back(pdata[1205]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ bool bSame = true;
|
|
|
+ nsize = static_cast<int>(xvecotrfactype.size());
|
|
|
+ if(nsize>1)
|
|
|
+ {
|
|
|
+ for(i=1;i<nsize;i++)
|
|
|
+ {
|
|
|
+ if(xvecotrfactype[i] != xvecotrfactype[i-1])
|
|
|
+ {
|
|
|
+ bSame = false;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(bSame == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("16线和32线雷达同时存在,请只保留一台雷达的网线连接"),QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(CheckMacSame() == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("多个雷达同时存在,请只保留一台雷达的网线连接"),QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ QByteArray ba = mvectorUDPPac_Dev[mvectorUDPPac_Dev.size() -1].mba;
|
|
|
+ if(ba.size() == 1206)
|
|
|
+ {
|
|
|
+ mbaDevPac = ba;
|
|
|
+ char * pdata = ba.data();
|
|
|
+ int nMotorSpeed = pdata[8] * 256 + pdata[9];
|
|
|
+ if(abs(nMotorSpeed - 5*60) <100)
|
|
|
+ {
|
|
|
+ ui->comboBox_MotorSpeed->setCurrentIndex(0);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(abs(nMotorSpeed - 20*60) <100)
|
|
|
+ {
|
|
|
+ ui->comboBox_MotorSpeed->setCurrentIndex(2);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if(abs(nMotorSpeed - 10*60) <100)
|
|
|
+ {
|
|
|
+ ui->comboBox_MotorSpeed->setCurrentIndex(1);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ char strout[1000];
|
|
|
+ snprintf(strout,1000,"Motor Speed: %d",nMotorSpeed);
|
|
|
+ QMessageBox::warning(this,tr("电机转速错误"),tr(strout),QMessageBox::YesAll);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ unsigned char ethparam[22];
|
|
|
+ memcpy(ethparam,pdata+10,22);
|
|
|
+ char strsrcip[256];
|
|
|
+ snprintf(strsrcip,256,"%d.%d.%d.%d",ethparam[0],ethparam[1],ethparam[2],ethparam[3]);
|
|
|
+ mstrDevIP = strsrcip;
|
|
|
+ ui->lineEdit_Param_IPSrc->setText(strsrcip);
|
|
|
+ char strdstip[256];
|
|
|
+ snprintf(strdstip,256,"%d.%d.%d.%d",ethparam[4],ethparam[5],ethparam[6],ethparam[7]);
|
|
|
+ ui->lineEdit_Param_IPSrc->setText(strdstip);
|
|
|
+ int nDataPort = ethparam[14]*256 + ethparam[15];
|
|
|
+ ui->lineEdit_DataPort->setText(QString::number(nDataPort));
|
|
|
+ int nDevPort = ethparam[16]*256 + ethparam[17];
|
|
|
+ mnDevPort = nDevPort;
|
|
|
+ ui->lineEdit_DevPort->setText(QString::number(nDevPort));
|
|
|
+ mbParam_Get = true;
|
|
|
+ ui->pushButton_Param_Set->setEnabled(true);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("设备包格式不对"),QMessageBox::YesAll);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::on_pushButton_Param_Set_clicked()
|
|
|
+{
|
|
|
+ if(mbListenning == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("请先打开监听"),QMessageBox::YesAll);
|
|
|
+ return ;
|
|
|
+ }
|
|
|
+ if(mbParam_Get == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("请先获取参数"),QMessageBox::YesAll);
|
|
|
+ return ;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(CheckMacSame() == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("多个雷达同时存在,请只保留一台雷达的网线连接"),QMessageBox::YesAll);
|
|
|
+ mbParam_Get = false;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(mvectorUDPPac_Dev.size()>0)
|
|
|
+ {
|
|
|
+ QByteArray ba = mvectorUDPPac_Dev[mvectorUDPPac_Dev.size() -1].mba;
|
|
|
+ if((ba.size() == 1206) && (mbaDevPac.size() == 1206))
|
|
|
+ {
|
|
|
+ char * str1 = ba.data();
|
|
|
+ char * str2 = mbaDevPac.data();
|
|
|
+ int i;
|
|
|
+ bool bSame = true;
|
|
|
+ for(i=0;i<6;i++)
|
|
|
+ {
|
|
|
+ if(str1[i+10+8] != str2[i+10+8])
|
|
|
+ {
|
|
|
+ bSame = false;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(bSame == false)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("多个雷达同时存在,请只保留一台雷达的网线连接"),QMessageBox::YesAll);
|
|
|
+ mbParam_Get = false;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("雷达设备包长度不是1206"),QMessageBox::YesAll);
|
|
|
+ mbParam_Get = false;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ QByteArray basend = mbaDevPac;
|
|
|
+ char * pdata = basend.data();
|
|
|
+ unsigned char strsrcip[4];
|
|
|
+ int nrtn = GetIP(ui->lineEdit_Param_IPSrc->text(),strsrcip);
|
|
|
+ if(nrtn != 1)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("设备IP设置错误"),QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ unsigned char strdstip[4];
|
|
|
+ nrtn = GetIP(ui->lineEdit_Param_IPDest->text(),strdstip);
|
|
|
+ if(nrtn != 1)
|
|
|
+ {
|
|
|
+ QMessageBox::warning(this,tr("Warning"),tr("目标IP设置错误"),QMessageBox::YesAll);
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ unsigned short nDataPort = static_cast<unsigned short>(ui->lineEdit_Param_DataPort->text().toInt());
|
|
|
+ unsigned short nDevPort = static_cast<unsigned short>(ui->lineEdit_Param_DevPort->text().toInt());
|
|
|
+ unsigned short nDataPort_High = nDataPort/256;
|
|
|
+ unsigned short nDataPort_Low = nDataPort - nDataPort_High*256;
|
|
|
+ unsigned short nDevPort_High = nDevPort/256;
|
|
|
+ unsigned short nDevPort_Low = nDevPort - nDevPort_High*256;
|
|
|
+ unsigned char strDataPort[2];
|
|
|
+ unsigned char strDevPort[2];
|
|
|
+ strDataPort[0] = static_cast<unsigned char>(nDataPort_High);
|
|
|
+ strDataPort[1] = static_cast<unsigned char>(nDataPort_Low);
|
|
|
+ strDevPort[0] = static_cast<unsigned char>(nDevPort_High);
|
|
|
+ strDevPort[1] = static_cast<unsigned char>(nDevPort_Low);
|
|
|
+ memcpy(pdata + 10,strsrcip,4);
|
|
|
+ memcpy(pdata + 10 + 4,strdstip,4);
|
|
|
+ memcpy(pdata + 10 + 14,strDataPort,2);
|
|
|
+ memcpy(pdata + 10 + 16,strDevPort,2);
|
|
|
+
|
|
|
+// mpUDPSocket_Dev->writeDatagram(basend.data(),basend.size(),QHostAddress(mstrDevIP),mnDevPort);
|
|
|
+ QMessageBox::information(this,tr("Info"),tr("设置成功"),QMessageBox::YesAll);
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::SetParamState()
|
|
|
+{
|
|
|
+ if(mbParam_Get)
|
|
|
+ {
|
|
|
+ ui->pushButton_Param_Set->setEnabled(true);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ui->pushButton_Param_Set->setEnabled(false);
|
|
|
+ ui->lineEdit_Param_DataPort->setText("");
|
|
|
+ ui->lineEdit_Param_DevPort->setText("");
|
|
|
+ ui->lineEdit_Param_IPDest->setText("");
|
|
|
+ ui->lineEdit_Param_IPSrc->setText("");
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::onSocketDevRead()
|
|
|
+{
|
|
|
+ while(mpUDPSocket_Dev->hasPendingDatagrams())
|
|
|
+ {
|
|
|
+ QNetworkDatagram datagram = mpUDPSocket_Dev->receiveDatagram();
|
|
|
+ if(datagram.isValid())
|
|
|
+ {
|
|
|
+ iv::UDPPac xUDPPac;
|
|
|
+ xUDPPac.mba = datagram.data();
|
|
|
+ xUDPPac.mhost = datagram.senderAddress();
|
|
|
+ xUDPPac.mnPort = static_cast<unsigned short>(datagram.senderPort()) ;
|
|
|
+ if(mvectorUDPPac_Dev.size()>=10)
|
|
|
+ {
|
|
|
+ mvectorUDPPac_Dev.erase(mvectorUDPPac_Dev.begin());
|
|
|
+ }
|
|
|
+ mvectorUDPPac_Dev.push_back(xUDPPac);
|
|
|
+ mnDevPacCount++;
|
|
|
+ if(mvectorUDPPac_Dev.size()>=3)
|
|
|
+ {
|
|
|
+ ui->pushButton_Param_Get->setEnabled(true);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::onSocketDataRead()
|
|
|
+{
|
|
|
+ while(mpUDPSocket_Data->hasPendingDatagrams())
|
|
|
+ {
|
|
|
+ QNetworkDatagram datagram = mpUDPSocket_Data->receiveDatagram();
|
|
|
+ if(datagram.isValid())
|
|
|
+ {
|
|
|
+ iv::UDPPac xUDPPac;
|
|
|
+ xUDPPac.mba = datagram.data();
|
|
|
+ xUDPPac.mhost = datagram.senderAddress();
|
|
|
+ xUDPPac.mnPort = static_cast<unsigned short>(datagram.senderPort()) ;
|
|
|
+ if(mvectorUDPPac_Data.size()>=100)
|
|
|
+ {
|
|
|
+ mvectorUDPPac_Data.erase(mvectorUDPPac_Data.begin());
|
|
|
+ }
|
|
|
+ mvectorUDPPac_Data.push_back(xUDPPac);
|
|
|
+ mnDataPacCount++;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MainWindow::onTimer()
|
|
|
+{
|
|
|
+ if(mbListenning)
|
|
|
+ {
|
|
|
+ char strout[1000];
|
|
|
+ snprintf(strout,1000,"Data Packet:%d Device Packete:%d",mnDataPacCount,mnDevPacCount);
|
|
|
+ ui->plainTextEdit->setPlainText(strout);
|
|
|
+
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ ui->plainTextEdit->setPlainText("");
|
|
|
+ }
|
|
|
+ mnDevPacCount = 0;
|
|
|
+ mnDataPacCount = 0;
|
|
|
+}
|
|
|
+
|
|
|
+bool MainWindow::CheckMacSame()
|
|
|
+{
|
|
|
+ int nsize = static_cast<int>(mvectorUDPPac_Dev.size());
|
|
|
+ int i;
|
|
|
+ if(nsize<=1)return true;
|
|
|
+ for(i=1;i<nsize;i++)
|
|
|
+ {
|
|
|
+ QByteArray ba = mvectorUDPPac_Dev[i].mba.data();
|
|
|
+ char * pdata = ba.data();
|
|
|
+ if(ba.size() != 1206)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ char * pdataold = mvectorUDPPac_Dev[i-1].mba.data();
|
|
|
+ if(mvectorUDPPac_Dev[i-1].mba.size() != 1206)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ int j;
|
|
|
+ for(j=0;j<6;j++)
|
|
|
+ {
|
|
|
+ if(pdata[10+8+j] != pdataold[10+8+j])
|
|
|
+ {
|
|
|
+ std::cout<<" Mac Diff."<<std::endl;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+int MainWindow::GetIP(QString str,unsigned char * strip)
|
|
|
+{
|
|
|
+ str = str.trimmed();
|
|
|
+ QStringList strlist = str.split(".");
|
|
|
+ if(strlist.size() != 4)
|
|
|
+ {
|
|
|
+ std::cout<<" ip addr is not 4."<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ int i;
|
|
|
+ for(i=0;i<4;i++)
|
|
|
+ {
|
|
|
+ QString strdata = strlist[i];
|
|
|
+ int ip;
|
|
|
+ ip = strdata.toInt();
|
|
|
+ if((ip<0)||(ip>=256))
|
|
|
+ {
|
|
|
+ std::cout<<"ip addr error."<<std::endl;
|
|
|
+ return -2;
|
|
|
+ }
|
|
|
+ strip[i] = static_cast<unsigned char>(ip);
|
|
|
+ }
|
|
|
+ return 1;
|
|
|
+}
|