551 lines
18 KiB
C++
551 lines
18 KiB
C++
#include "SerialProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include <QStringList>
|
||
#include <QDebug>
|
||
#include <QThread>
|
||
#include <sstream>
|
||
#include <iomanip>
|
||
#include <chrono>
|
||
#include <thread>
|
||
|
||
#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_threadRunning(false)
|
||
{
|
||
m_pSerialPort = new QSerialPort(this);
|
||
|
||
// 连接信号和槽（保留原有的信号槽机制作为备用）
|
||
connect(m_pSerialPort, &QSerialPort::readyRead, this, &SerialProtocol::OnSerialDataReceived);
|
||
connect(m_pSerialPort, &QSerialPort::errorOccurred, this, &SerialProtocol::OnSerialError);
|
||
}
|
||
|
||
SerialProtocol::~SerialProtocol()
|
||
{
|
||
// 先停止接收线程
|
||
StopReceiveThread();
|
||
// 再关闭串口
|
||
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());
|
||
|
||
// 启动接收线程
|
||
StartReceiveThread();
|
||
|
||
// 触发连接状态回调
|
||
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());
|
||
|
||
// 先停止接收线程
|
||
StopReceiveThread();
|
||
|
||
// 再关闭串口
|
||
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()
|
||
{
|
||
// 注意：这个方法现在主要作为Qt信号槽的备用机制
|
||
// 主要的数据接收由独立线程处理
|
||
LOG_DEBUG("[Signal] OnSerialDataReceived() called (backup mechanism)\n");
|
||
|
||
// 如果线程正在运行，优先使用线程处理
|
||
if (m_threadRunning.load()) {
|
||
LOG_DEBUG("[Signal] Thread is running, skipping signal-based processing\n");
|
||
return;
|
||
}
|
||
|
||
if (!m_pSerialPort) {
|
||
LOG_ERROR("[Signal] Serial port is null in OnSerialDataReceived\n");
|
||
return;
|
||
}
|
||
|
||
if (!m_pSerialPort->isOpen()) {
|
||
LOG_ERROR("[Signal] Serial port is not open in OnSerialDataReceived\n");
|
||
return;
|
||
}
|
||
|
||
// 检查可用字节数
|
||
qint64 bytesAvailable = m_pSerialPort->bytesAvailable();
|
||
LOG_INFO("[Signal] Bytes available to read: %lld\n", bytesAvailable);
|
||
|
||
if (bytesAvailable <= 0) {
|
||
LOG_WARNING("[Signal] No bytes available but readyRead signal was triggered\n");
|
||
return;
|
||
}
|
||
|
||
// 读取所有可用数据
|
||
QByteArray data = m_pSerialPort->readAll();
|
||
QString receivedData = QString::fromUtf8(data);
|
||
|
||
LOG_INFO("[Signal] Actually read %d bytes from serial port\n", data.size());
|
||
|
||
if (data.isEmpty()) {
|
||
LOG_WARNING("[Signal] readAll() returned empty data\n");
|
||
return;
|
||
}
|
||
|
||
// 使用互斥锁保护缓冲区
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_bufferMutex);
|
||
m_receiveBuffer += receivedData;
|
||
}
|
||
|
||
// 打印接收到的数据
|
||
LOG_INFO("[Signal] 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("[Signal] %s\n", asciiInfo.toStdString().c_str());
|
||
}
|
||
|
||
// 处理接收到的数据
|
||
ProcessReceivedData();
|
||
}
|
||
|
||
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);
|
||
|
||
// 移除空的部分
|
||
QStringList validParts;
|
||
for (const QString& part : parts) {
|
||
if (!part.isEmpty()) {
|
||
validParts.append(part);
|
||
}
|
||
}
|
||
|
||
|
||
if (validParts.isEmpty()) {
|
||
LOG_WARNING("No valid parts found in protocol data\n");
|
||
return;
|
||
}
|
||
|
||
QString command = validParts[0];
|
||
LOG_DEBUG("Extracted command: '%s'\n", command.toStdString().c_str());
|
||
|
||
if (command == CMD_START) {
|
||
// 处理开始命令：$,START,# 或 $,START,cameraId,#
|
||
LOG_INFO("Received START command\n");
|
||
|
||
// 默认相机ID为1，如果有更多参数可以解析
|
||
int cameraId = 1;
|
||
if (validParts.size() > 1) {
|
||
bool ok;
|
||
cameraId = validParts[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();
|
||
|
||
|
||
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();
|
||
}
|
||
|
||
// 线程方法实现
|
||
void SerialProtocol::StartReceiveThread()
|
||
{
|
||
if (m_threadRunning.load()) {
|
||
LOG_WARNING("Receive thread is already running\n");
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("Starting serial receive thread\n");
|
||
m_threadRunning.store(true);
|
||
|
||
// 启动接收线程
|
||
m_receiveThread = std::thread(&SerialProtocol::SerialReceiveThreadFunction, this);
|
||
}
|
||
|
||
void SerialProtocol::StopReceiveThread()
|
||
{
|
||
if (!m_threadRunning.load()) {
|
||
LOG_DEBUG("Receive thread is not running\n");
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("Stopping serial receive thread\n");
|
||
m_threadRunning.store(false);
|
||
|
||
// 等待线程结束
|
||
if (m_receiveThread.joinable()) {
|
||
m_receiveThread.join();
|
||
LOG_INFO("Serial receive thread stopped\n");
|
||
}
|
||
}
|
||
|
||
void SerialProtocol::SerialReceiveThreadFunction()
|
||
{
|
||
LOG_INFO("Serial receive thread started\n");
|
||
|
||
while (m_threadRunning.load()) {
|
||
// 检查串口是否打开
|
||
if (!m_pSerialPort || !m_pSerialPort->isOpen()) {
|
||
// 串口未打开，短暂休眠后继续检查
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
continue;
|
||
}
|
||
|
||
try {
|
||
// 使用waitForReadyRead进行阻塞式等待，超时时间100ms
|
||
if (m_pSerialPort->waitForReadyRead(100)) {
|
||
// 有数据可读
|
||
qint64 bytesAvailable = m_pSerialPort->bytesAvailable();
|
||
if (bytesAvailable > 0) {
|
||
LOG_INFO("[Thread] Bytes available to read: %lld\n", bytesAvailable);
|
||
|
||
// 读取所有可用数据
|
||
QByteArray data = m_pSerialPort->readAll();
|
||
if (!data.isEmpty()) {
|
||
QString receivedData = QString::fromUtf8(data);
|
||
|
||
LOG_INFO("[Thread] Actually read %d bytes from serial port\n", data.size());
|
||
LOG_INFO("[Thread] Received [%d bytes]: \"%s\"\n", data.size(), receivedData.toStdString().c_str());
|
||
|
||
// 使用互斥锁保护缓冲区
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_bufferMutex);
|
||
m_receiveBuffer += receivedData;
|
||
}
|
||
|
||
// 处理接收到的数据
|
||
ProcessReceivedData();
|
||
}
|
||
}
|
||
}
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("[Thread] Exception in receive thread: %s\n", e.what());
|
||
} catch (...) {
|
||
LOG_ERROR("[Thread] Unknown exception in receive thread\n");
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Serial receive thread finished\n");
|
||
}
|
||
|
||
void SerialProtocol::ProcessReceivedData()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_bufferMutex);
|
||
|
||
// 查找完整的协议帧
|
||
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);
|
||
|
||
LOG_INFO("[Thread] Found complete protocol frame: %s\n", protocolFrame.toStdString().c_str());
|
||
|
||
// 解析协议数据
|
||
ParseProtocolData(protocolFrame);
|
||
|
||
// 移除已处理的数据
|
||
m_receiveBuffer.remove(0, endIndex + 1);
|
||
} else {
|
||
// 没有找到结束标识，等待更多数据
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 如果缓冲区太大，清空一部分（防止内存溢出）
|
||
if (m_receiveBuffer.length() > 1024) {
|
||
LOG_WARNING("[Thread] Receive buffer too large (%d bytes), clearing\n", m_receiveBuffer.length());
|
||
m_receiveBuffer.clear();
|
||
}
|
||
}
|