指令协议格式如下:
本文只针对大小端协议数据指令作为参考代码,不能直接运行。
1.方向控制:
// 上
void YTDH_Command::YT_Control_up(double m_Speed)
{
V_Speed = qRound(m_Speed * 10);
crc = (0x01 + 0x00 + 0x08 + 0x00 + V_Speed) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x08, 0x00, V_Speed, crc};
emit sg_sendCommnd(data, sizeof(data));
}
// 下
void YTDH_Command::YT_Control_down(double m_Speed)
{
V_Speed = qRound(m_Speed * 10);
crc = (0x01 + 0x00 + 0x10 + 0x00 + V_Speed) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x10, 0x00, V_Speed, crc};
emit sg_sendCommnd(data, sizeof(data));
}
// 左
void YTDH_Command::YT_Control_left(double m_Speed)
{
H_Speed = qRound(m_Speed * 10);
crc = (0x01 + 0x00 + 0x04 + H_Speed + 0x00) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x04, H_Speed, 0x00, crc};
emit sg_sendCommnd(data, sizeof(data));
}
// 右
void YTDH_Command::YT_Control_right(double m_Speed)
{
H_Speed = qRound(m_Speed * 10);
crc = (0x01 + 0x00 + 0x02 + H_Speed + 0x00) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x02, H_Speed, 0x00, crc};
emit sg_sendCommnd(data, sizeof(data));
}
// 左上
void YTDH_Command::YT_Control_leftUp(double m_Speed, double l_Speed)
{
H_Speed = m_Speed * 10;
V_Speed = l_Speed * 10;
crc = (0x01 + 0x00 + 0x0c + H_Speed + V_Speed) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x0c, H_Speed, V_Speed, crc};
emit sg_sendCommnd(data, sizeof(data));
}
2.角度回传
// 角度实时回传
void YTDH_Command::YT_Angle_Back(QString BackName)
{
qDebug() << "BackName:" << BackName;
if(BackName == "ON")
{
qDebug() << "111111";
crc = (0x01 + 0xe1 + 0x01 + (short int)(1 >> 8) + (short int)(1 & 0x00ff)) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0xe1, 0x01, (short int)(1 >> 8), (short int)(1 & 0x00ff), crc};
emit sg_sendCommnd(data, sizeof(data));
}
else if(BackName == "OFF")
{
crc = ( 0x01 + 0xe1 + 0x02 + 0x00 + 0x00) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0xe1, 0x02, 0x00, 0x00, crc};
emit sg_sendCommnd(data, sizeof(data));
}
}
// 水平角度
void YTDH_Command::YT_SPAngle_Location(short int Angle)
{
H_HAngle = (Angle * 100) >> 8;
H_LAngle = (Angle * 100) & 0x00ff;
crc = (0x01 + 0x00 + 0x4b + H_HAngle + H_LAngle) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x4b, H_HAngle, H_LAngle, crc};
emit sg_sendCommnd(data, sizeof(data));
}
// 垂直角度
void YTDH_Command::YT_CZAngle_Location(short int Angle)
{
H_HAngle = (Angle * 100) >> 8;
H_LAngle = (Angle * 100) & 0x00ff;
crc = (0x01 + 0x00 + 0x4d + H_HAngle + H_LAngle) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x4d, H_HAngle, H_LAngle, crc};
emit sg_sendCommnd(data, sizeof(data));
}
void YTDH_Command::sl_recvYTcrotrl(int x, int y)
{
if(x < 360. && x > 0.)
{
char H_HAngle = (x * 100) >> 8;
char H_LAngle = (x * 100) & 0x00ff;
BYTE crc = (0x01 + 0x00 + 0x4b + H_HAngle + H_LAngle) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x4b, H_HAngle, H_LAngle, crc};
emit sg_sendCommnd(data, sizeof(data));
}
else if(y > -60. && y < 60.)
{
char H_HAngle = (y * 100) >> 8;
char H_LAngle = (y * 100) & 0x00ff;
BYTE crc = (0x01 + 0x00 + 0x4d + H_HAngle + H_LAngle) & 0x00ff;
BYTE data[7] = {0xff, 0x01, 0x00, 0x4d, H_HAngle, H_LAngle, crc};
emit sg_sendCommnd(data, sizeof(data));
}
}
3.十六进制转换十进制
// 十六进制转换十进制
double YTDH_Command::YT_Angle_Show(string &st)
{
double sum = 0, times;
double m;
string::size_type sz = st.size();
for(string::size_type index = 0; index != sz; ++index)
{
// 变为小写
st[index] = tolower(st[index]);
if(st[index] >= 'a' && st[index] <= 'f')
{
m = st[index] - 'a' + 10;
times = pow(16, (sz -1 - index));
sum += m * times;
}
else if(isdigit(st[index]))
{
m = st[index] - 48;
times = pow(16, (sz - 1 - index));
sum += m * times;
}
else
{
qDebug() << "Don't recongnize #16";
break;
}
}
return sum;
}
4.发送指令给云台
// 发送指令给云台
void SocketThread::sl_sendData(unsigned char *data, int size)
{
m_YTSocket->writeDatagram((char*)data,size,QHostAddress("192.168.1.225"), 6666);
}
5.接收云台回传信息
void SocketThread::run()
{
// 接收识别模块的信号、发送周期控制指令到解码模块
m_recvSocket = new QUdpSocket;
m_recvSocket->bind(8001);
m_sendSocket = new QUdpSocket;
m_sendSocket->bind(8004);
m_YTSocket = new QUdpSocket;
m_YTSocket->bind(8020);
connect(m_YTSocket, SIGNAL(readyRead()), this, SLOT(sl_readPendingDatagrams()), Qt::DirectConnection);
connect(m_recvSocket,SIGNAL(readyRead()),this,SLOT(sl_hasPendingDatagrams()),Qt::DirectConnection);
connect(m_sendSocket,SIGNAL(readyRead()),this,SLOT(sl_recvRadarData()), Qt::DirectConnection);
exec();
}
// 接收云台回传信息
void SocketThread::sl_readPendingDatagrams()
{
while (m_YTSocket->hasPendingDatagrams())
{
QByteArray datagram;
datagram.resize(m_YTSocket->pendingDatagramSize());
QHostAddress ip;
quint16 Port;
m_YTSocket->readDatagram(datagram.data(), datagram.size(),&ip, &Port);
emit sg_receivedData(datagram);
}
}
// 解析接收到的回传信息(涉及部分角度计算 仅供参考)
void MainWindow::sl_controlData(QByteArray datagram)
{
QString str = QString(datagram.toHex().toUpper()); // 十六进制字符串
QString numl = "59";
QString numv = "5B";
QString numlt = str.section(QRegExp(""),7,8);
if(numlt == numl)
{
// 水平角度
str.section(QRegExp(""),9,12);
string st = str.section(QRegExp(""),9,12).toStdString();
double sum = m_pDHCommand->YT_Angle_Show(st);
ui->lineEdit_yaw->setText(QString::number(sum/100));
}
else if(numlt == numv)
{
// 垂直角度
str.section(QRegExp(""),9,12);
string st = str.section(QRegExp(""),9,12).toStdString();
double sum = m_pDHCommand->YT_Angle_Show(st);
if(sum > 0 && sum <= 6255)
{
str.section(QRegExp(""),9,12);
string st = str.section(QRegExp(""),9,12).toStdString();
double sum = m_pDHCommand->YT_Angle_Show(st);
ui->lineEdit_pitch->setText(QString::number(sum/100));
}
else if(sum > 6255 && sum < 65500)
{
str.section(QRegExp(""),9,12);
string st = str.section(QRegExp(""),9,12).toStdString();
double sum = m_pDHCommand->YT_Angle_Show(st);
double num = sum/100;
num = num - 655.37;
ui->lineEdit_pitch->setText(QString::number(num));
}
}
}