云台设备指令通讯-串口通讯、UDP、TCP通讯应用

指令协议格式如下：

本文只针对大小端协议数据指令作为参考代码，不能直接运行。

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));
        }
    }
}