435 lines
14 KiB
C++
435 lines
14 KiB
C++
#include "SerialProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include <QStringList>
|
||
#include <QDebug>
|
||
#include <QThread>
|
||
#include <sstream>
|
||
#include <iomanip>
|
||
|
||
#include <QSerialPortInfo>
|
||
|
||
// 协议常量定义
|
||
const QString SerialProtocol::PROTOCOL_START = "$";
|
||
const QString SerialProtocol::PROTOCOL_END = "#";
|
||
const QString SerialProtocol::CMD_START = "START";
|
||
const QString SerialProtocol::CMD_DATA = "DATA";
|
||
const QString SerialProtocol::SEPARATOR = ",";
|
||
|
||
SerialProtocol::SerialProtocol(QObject* parent)
|
||
: QObject(parent)
|
||
, m_serialStatus(STATUS_DISCONNECTED)
|
||
{
|
||
m_pSerialPort = new QSerialPort(this);
|
||
|
||
// 连接信号和槽
|
||
connect(m_pSerialPort, &QSerialPort::readyRead, this, &SerialProtocol::OnSerialDataReceived);
|
||
connect(m_pSerialPort, &QSerialPort::errorOccurred, this, &SerialProtocol::OnSerialError);
|
||
}
|
||
|
||
SerialProtocol::~SerialProtocol()
|
||
{
|
||
CloseSerial();
|
||
}
|
||
|
||
int SerialProtocol::OpenSerial(const std::string& portName, int baudRate,
|
||
int dataBits, int stopBits, int parity, int flowControl)
|
||
{
|
||
if (m_pSerialPort->isOpen()) {
|
||
LOG_WARNING("Serial port is already open: %s\n", portName.c_str());
|
||
return SUCCESS;
|
||
}
|
||
|
||
LOG_INFO("Opening serial port: %s, baudRate: %d, dataBits: %d, stopBits: %d, parity: %d, flowControl: %d\n",
|
||
portName.c_str(), baudRate, dataBits, stopBits, parity, flowControl);
|
||
|
||
// 设置串口参数
|
||
m_pSerialPort->setPortName(QString::fromStdString(portName));
|
||
m_pSerialPort->setBaudRate(baudRate);
|
||
|
||
// 设置数据位
|
||
switch (dataBits) {
|
||
case 5: m_pSerialPort->setDataBits(QSerialPort::Data5); break;
|
||
case 6: m_pSerialPort->setDataBits(QSerialPort::Data6); break;
|
||
case 7: m_pSerialPort->setDataBits(QSerialPort::Data7); break;
|
||
case 8: m_pSerialPort->setDataBits(QSerialPort::Data8); break;
|
||
default: m_pSerialPort->setDataBits(QSerialPort::Data8); break;
|
||
}
|
||
|
||
// 设置停止位
|
||
switch (stopBits) {
|
||
case 1: m_pSerialPort->setStopBits(QSerialPort::OneStop); break;
|
||
case 2: m_pSerialPort->setStopBits(QSerialPort::TwoStop); break;
|
||
default: m_pSerialPort->setStopBits(QSerialPort::OneStop); break;
|
||
}
|
||
|
||
// 设置校验位
|
||
switch (parity) {
|
||
case 0: m_pSerialPort->setParity(QSerialPort::NoParity); break;
|
||
case 1: m_pSerialPort->setParity(QSerialPort::OddParity); break;
|
||
case 2: m_pSerialPort->setParity(QSerialPort::EvenParity); break;
|
||
default: m_pSerialPort->setParity(QSerialPort::NoParity); break;
|
||
}
|
||
|
||
// 设置流控制
|
||
switch (flowControl) {
|
||
case 0: m_pSerialPort->setFlowControl(QSerialPort::NoFlowControl); break;
|
||
case 1: m_pSerialPort->setFlowControl(QSerialPort::HardwareControl); break;
|
||
case 2: m_pSerialPort->setFlowControl(QSerialPort::SoftwareControl); break;
|
||
default: m_pSerialPort->setFlowControl(QSerialPort::NoFlowControl); break;
|
||
}
|
||
|
||
// 尝试打开串口
|
||
if (!m_pSerialPort->open(QIODevice::ReadWrite)) {
|
||
LOG_ERROR("Failed to open serial port: %s, error: %s\n", portName.c_str(), m_pSerialPort->errorString().toStdString().c_str());
|
||
m_serialStatus = STATUS_ERROR;
|
||
return ERR_CODE(DEV_OPEN_ERR);
|
||
}
|
||
|
||
// 清空接收缓冲区
|
||
m_receiveBuffer.clear();
|
||
m_pSerialPort->clear();
|
||
|
||
// 设置读取超时和缓冲区大小
|
||
m_pSerialPort->setReadBufferSize(1024);
|
||
|
||
m_serialStatus = STATUS_CONNECTED;
|
||
LOG_INFO("Serial port opened successfully: %s\n", portName.c_str());
|
||
|
||
// 触发连接状态回调
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(true);
|
||
}
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
void SerialProtocol::CloseSerial()
|
||
{
|
||
if (m_pSerialPort && m_pSerialPort->isOpen()) {
|
||
LOG_INFO("Closing serial port: %s\n", m_pSerialPort->portName().toStdString().c_str());
|
||
m_pSerialPort->close();
|
||
m_serialStatus = STATUS_DISCONNECTED;
|
||
|
||
// 触发连接状态回调
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(false);
|
||
}
|
||
}
|
||
}
|
||
|
||
int SerialProtocol::SendMultiTargetData(const MultiTargetData& multiTargetData, uint16_t cameraId)
|
||
{
|
||
if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
|
||
LOG_ERROR("Serial port is not open, cannot send data\n");
|
||
return -1;
|
||
}
|
||
|
||
LOG_DEBUG("Sending multi-target data, count: %d, camera ID: %d\n", multiTargetData.count, cameraId);
|
||
|
||
// 构造协议数据
|
||
std::string protocolData = BuildDataProtocol(multiTargetData, cameraId);
|
||
|
||
// 发送数据
|
||
int result = SendData(protocolData);
|
||
if (result == SUCCESS) {
|
||
LOG_INFO("Multi-target data sent successfully: %s\n", protocolData.c_str());
|
||
} else {
|
||
LOG_ERROR("Failed to send multi-target data\n");
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
SerialProtocol::SerialStatus SerialProtocol::GetSerialStatus() const
|
||
{
|
||
return m_serialStatus;
|
||
}
|
||
|
||
void SerialProtocol::SetConnectionCallback(const ConnectionCallback& callback)
|
||
{
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void SerialProtocol::SetWorkSignalCallback(const WorkSignalCallback& callback)
|
||
{
|
||
m_workSignalCallback = callback;
|
||
}
|
||
|
||
bool SerialProtocol::IsOpen() const
|
||
{
|
||
return m_pSerialPort && m_pSerialPort->isOpen();
|
||
}
|
||
|
||
void SerialProtocol::OnSerialDataReceived()
|
||
{
|
||
LOG_DEBUG("OnSerialDataReceived() called\n");
|
||
LOG_DEBUG("OnSerialDataReceived() called\n");
|
||
LOG_DEBUG("OnSerialDataReceived() called\n");
|
||
LOG_DEBUG("OnSerialDataReceived() called\n");
|
||
LOG_DEBUG("OnSerialDataReceived() called\n");
|
||
|
||
|
||
if (!m_pSerialPort) {
|
||
LOG_ERROR("Serial port is null in OnSerialDataReceived\n");
|
||
return;
|
||
}
|
||
|
||
if (!m_pSerialPort->isOpen()) {
|
||
LOG_ERROR("Serial port is not open in OnSerialDataReceived\n");
|
||
return;
|
||
}
|
||
|
||
// 检查可用字节数
|
||
qint64 bytesAvailable = m_pSerialPort->bytesAvailable();
|
||
LOG_INFO("Bytes available to read: %lld\n", bytesAvailable);
|
||
|
||
if (bytesAvailable <= 0) {
|
||
LOG_WARNING("No bytes available but readyRead signal was triggered\n");
|
||
return;
|
||
}
|
||
|
||
// 读取所有可用数据
|
||
QByteArray data = m_pSerialPort->readAll();
|
||
QString receivedData = QString::fromUtf8(data);
|
||
|
||
LOG_INFO("Actually read %d bytes from serial port\n", data.size());
|
||
|
||
if (data.isEmpty()) {
|
||
LOG_WARNING("readAll() returned empty data\n");
|
||
return;
|
||
}
|
||
|
||
// 添加到接收缓冲区
|
||
m_receiveBuffer += receivedData;
|
||
|
||
// 打印接收到的数据
|
||
LOG_INFO("Received [%d bytes]: \"%s\"\n", data.size(), receivedData.toStdString().c_str());
|
||
|
||
// 如果包含不可打印字符，额外打印ASCII码
|
||
bool hasNonPrintable = false;
|
||
for (char c : data) {
|
||
if (c < 32 || c > 126) {
|
||
hasNonPrintable = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (hasNonPrintable) {
|
||
QString asciiInfo = "ASCII: ";
|
||
for (char c : data) {
|
||
if (c >= 32 && c <= 126) {
|
||
asciiInfo += QString("'%1' ").arg(c);
|
||
} else {
|
||
asciiInfo += QString("[%1] ").arg((unsigned char)c);
|
||
}
|
||
}
|
||
LOG_INFO("%s\n", asciiInfo.toStdString().c_str());
|
||
}
|
||
|
||
// 查找完整的协议帧
|
||
int startIndex = -1;
|
||
int endIndex = -1;
|
||
|
||
while ((startIndex = m_receiveBuffer.indexOf(PROTOCOL_START)) != -1) {
|
||
endIndex = m_receiveBuffer.indexOf(PROTOCOL_END, startIndex);
|
||
if (endIndex != -1) {
|
||
// 提取完整的协议帧
|
||
QString protocolFrame = m_receiveBuffer.mid(startIndex, endIndex - startIndex + 1);
|
||
|
||
// 解析协议数据
|
||
ParseProtocolData(protocolFrame);
|
||
|
||
// 移除已处理的数据
|
||
m_receiveBuffer.remove(0, endIndex + 1);
|
||
} else {
|
||
// 没有找到结束标识，等待更多数据
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 如果缓冲区太大，清空一部分（防止内存溢出）
|
||
if (m_receiveBuffer.length() > 1024) {
|
||
m_receiveBuffer.clear();
|
||
}
|
||
}
|
||
|
||
void SerialProtocol::OnSerialError(QSerialPort::SerialPortError error)
|
||
{
|
||
if (error == QSerialPort::NoError) {
|
||
return;
|
||
}
|
||
|
||
QString errorString = m_pSerialPort ? m_pSerialPort->errorString() : "Unknown error";
|
||
LOG_ERROR("=== SERIAL PORT ERROR ===\n");
|
||
LOG_ERROR("Error type: %d\n", (int)error);
|
||
LOG_ERROR("Error message: %s\n", errorString.toStdString().c_str());
|
||
LOG_ERROR("Port open status: %s\n", (m_pSerialPort && m_pSerialPort->isOpen()) ? "OPEN" : "CLOSED");
|
||
|
||
// 详细的错误类型说明
|
||
switch (error) {
|
||
case QSerialPort::DeviceNotFoundError:
|
||
LOG_ERROR("Device not found error - port may be disconnected\n");
|
||
break;
|
||
case QSerialPort::PermissionError:
|
||
LOG_ERROR("Permission error - port may be in use by another application\n");
|
||
break;
|
||
case QSerialPort::OpenError:
|
||
LOG_ERROR("Open error - failed to open the port\n");
|
||
break;
|
||
case QSerialPort::WriteError:
|
||
LOG_ERROR("Write error - failed to write data\n");
|
||
break;
|
||
case QSerialPort::ReadError:
|
||
LOG_ERROR("Read error - failed to read data\n");
|
||
break;
|
||
case QSerialPort::ResourceError:
|
||
LOG_ERROR("Resource error - device disappeared or became unavailable\n");
|
||
break;
|
||
case QSerialPort::UnsupportedOperationError:
|
||
LOG_ERROR("Unsupported operation error\n");
|
||
break;
|
||
case QSerialPort::TimeoutError:
|
||
LOG_ERROR("Timeout error\n");
|
||
break;
|
||
default:
|
||
LOG_ERROR("Unknown error type: %d\n", (int)error);
|
||
break;
|
||
}
|
||
|
||
// 设置错误状态
|
||
m_serialStatus = STATUS_ERROR;
|
||
|
||
// 根据错误类型处理
|
||
switch (error) {
|
||
case QSerialPort::ResourceError:
|
||
case QSerialPort::DeviceNotFoundError:
|
||
case QSerialPort::PermissionError:
|
||
LOG_ERROR("Critical error - notifying connection callback\n");
|
||
// 连接断开错误
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(false);
|
||
}
|
||
break;
|
||
default:
|
||
LOG_WARNING("Non-critical error - continuing operation\n");
|
||
break;
|
||
}
|
||
|
||
LOG_ERROR("=== END SERIAL ERROR ===\n");
|
||
}
|
||
|
||
void SerialProtocol::ParseProtocolData(const QString& data)
|
||
{
|
||
LOG_DEBUG("Parsing protocol data: %s\n", data.toStdString().c_str());
|
||
|
||
// 移除协议开始和结束标识
|
||
QString cleanData = data;
|
||
cleanData.remove(PROTOCOL_START);
|
||
cleanData.remove(PROTOCOL_END);
|
||
|
||
// 按分隔符分割数据
|
||
QStringList parts = cleanData.split(SEPARATOR);
|
||
|
||
if (parts.isEmpty()) {
|
||
LOG_WARNING("Empty protocol data received\n");
|
||
return;
|
||
}
|
||
|
||
QString command = parts[0];
|
||
|
||
if (command == CMD_START) {
|
||
// 处理开始命令：$,START,#
|
||
LOG_INFO("Received START command\n");
|
||
|
||
// 默认相机ID为1，如果有更多参数可以解析
|
||
int cameraId = 1;
|
||
if (parts.size() > 1) {
|
||
bool ok;
|
||
cameraId = parts[1].toInt(&ok);
|
||
if (!ok) {
|
||
cameraId = 1;
|
||
}
|
||
}
|
||
|
||
// 触发工作信号回调
|
||
if (m_workSignalCallback) {
|
||
bool result = m_workSignalCallback(true, cameraId);
|
||
LOG_INFO("Work signal callback result: %s, camera ID: %d\n",
|
||
result ? "success" : "failed", cameraId);
|
||
}
|
||
|
||
m_serialStatus = STATUS_WORKING;
|
||
} else {
|
||
LOG_WARNING("Unknown command received: %s\n", command.toStdString().c_str());
|
||
}
|
||
}
|
||
|
||
int SerialProtocol::SendData(const std::string& data)
|
||
{
|
||
if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
|
||
LOG_ERROR("Serial port is not open, cannot send data\n");
|
||
return -1;
|
||
}
|
||
|
||
// 检查串口是否可写
|
||
if (!m_pSerialPort->isWritable()) {
|
||
LOG_ERROR("Serial port is not writable\n");
|
||
return -1;
|
||
}
|
||
|
||
// 检查待写入的字节数
|
||
qint64 bytesToWrite = m_pSerialPort->bytesToWrite();
|
||
LOG_INFO("Bytes pending write before send: %lld\n", bytesToWrite);
|
||
|
||
QByteArray dataToSend = data.c_str();
|
||
qint64 bytesWritten = m_pSerialPort->write(dataToSend);
|
||
|
||
LOG_INFO("Write operation result: %lld bytes (expected: %d)\n", bytesWritten, dataToSend.size());
|
||
|
||
if (bytesWritten == -1) {
|
||
LOG_ERROR("Failed to write data to serial port: %s\n", m_pSerialPort->errorString().toStdString().c_str());
|
||
m_serialStatus = STATUS_ERROR;
|
||
return -1;
|
||
}
|
||
|
||
if (bytesWritten != dataToSend.size()) {
|
||
LOG_WARNING("PARTIAL WRITE: %lld of %d bytes written\n", bytesWritten, dataToSend.size());
|
||
}
|
||
|
||
// 立即刷新缓冲区
|
||
m_pSerialPort->flush();
|
||
|
||
// 等待数据发送完成
|
||
if (!m_pSerialPort->waitForBytesWritten(3000)) {
|
||
LOG_ERROR("Timeout waiting for data to be written\n");
|
||
return -1;
|
||
}
|
||
|
||
LOG_DEBUG("Sent %lld bytes to serial port\n", bytesWritten);
|
||
return SUCCESS;
|
||
}
|
||
|
||
std::string SerialProtocol::BuildDataProtocol(const MultiTargetData& multiTargetData, uint16_t cameraId)
|
||
{
|
||
std::stringstream ss;
|
||
|
||
// 协议格式：$,DATA,NUM,posX,posY,posZ,posC,posX,posY,posZ,posC...,#
|
||
ss << PROTOCOL_START.toStdString() << SEPARATOR.toStdString()
|
||
<< CMD_DATA.toStdString() << SEPARATOR.toStdString()
|
||
<< multiTargetData.count;
|
||
|
||
// 添加每个目标的坐标数据
|
||
for (const auto& target : multiTargetData.targets) {
|
||
ss << SEPARATOR.toStdString()
|
||
<< std::fixed << std::setprecision(2) << target.x << SEPARATOR.toStdString()
|
||
<< std::fixed << std::setprecision(2) << target.y << SEPARATOR.toStdString()
|
||
<< std::fixed << std::setprecision(2) << target.z << SEPARATOR.toStdString()
|
||
<< std::fixed << std::setprecision(2) << target.rz;
|
||
}
|
||
|
||
ss << SEPARATOR.toStdString() << PROTOCOL_END.toStdString();
|
||
|
||
return ss.str();
|
||
}
|