gloamxun/d330viewer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 5 Wiki Activity

feat: 改写GIGE协议

Browse Source
This commit is contained in:
YusenZhao
2026-01-16 18:07:52 +08:00
parent ff4a4cabc8
commit 8b07397b5b
11 changed files with 321 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

155
src/core/GVSPParser.cpp
View File

@@ -1,5 +1,6 @@
#include "core/GVSPParser.h"
#include <QDebug>
#include <QtConcurrent>
#include <cstring>
#include <winsock2.h>
@@ -16,6 +17,7 @@ GVSPParser::GVSPParser(QObject *parent)
, m_lastBlockId(0)
, m_packetCount(0)
{
m_imageProcessingCount.storeRelaxed(0);
}
GVSPParser::~GVSPParser()
@@ -35,6 +37,8 @@ void GVSPParser::reset()
void GVSPParser::parsePacket(const QByteArray &packet)
{
static int debugCount = 0;
if (packet.size() < sizeof(GVSPPacketHeader)) {
return;
}
@@ -42,27 +46,20 @@ void GVSPParser::parsePacket(const QByteArray &packet)
const uint8_t *data = reinterpret_cast<const uint8_t*>(packet.constData());
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
// 注释掉调试日志以提高性能
// static int debugCount = 0;
// if (debugCount < 3) {
// qDebug() << "Packet" << debugCount << "first 16 bytes (hex):";
// QString hexStr;
// for (int i = 0; i < qMin(16, packet.size()); i++) {
// hexStr += QString("%1 ").arg(data[i], 2, 16, QChar('0'));
// }
// qDebug() << hexStr;
// debugCount++;
// }
// GVSP包头格式：status(2) + block_id(2) + packet_format(4)
// 包类型在packet_format的高字节
uint32_t packet_fmt = ntohl(header->packet_fmt_id);
uint8_t packetType = (packet_fmt >> 24) & 0xFF;
// GigE Vision 2.1 标准 GVSP 包头解析
uint16_t blockId = ntohs(header->block_id);
uint8_t packetType = header->packet_format;
uint16_t packetId = ntohs(header->packet_id);
// 注释掉频繁的日志输出以提高性能
// static int leaderCount = 0;
// static int trailerCount = 0;
// 打印前5个包的详细信息
if (debugCount < 5) {
// qDebug() << "[GVSPParser] Packet" << debugCount
// << "Type:" << packetType
// << "BlockID:" << blockId
// << "PacketID:" << packetId
// << "Size:" << packet.size();
debugCount++;
}
switch (packetType) {
case GVSP_LEADER_PACKET:
@@ -81,7 +78,9 @@ void GVSPParser::parsePacket(const QByteArray &packet)
void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
{
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
// 最小大小检查：至少要有包头 + 2字节reserved + 2字节payload_type
if (size < sizeof(GVSPPacketHeader) + 4) {
qDebug() << "[GVSPParser] Leader packet too small:" << size << "bytes";
return;
}
@@ -92,8 +91,16 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
const uint16_t *payload_type_ptr = reinterpret_cast<const uint16_t*>(data + sizeof(GVSPPacketHeader) + 2);
uint16_t payload_type = ntohs(*payload_type_ptr);
// qDebug() << "[GVSPParser] Leader packet: BlockID=" << m_currentBlockId
// << "PayloadType=0x" << Qt::hex << payload_type << Qt::dec
// << "Size=" << size;
if (payload_type == PAYLOAD_TYPE_IMAGE) {
// Image data leader
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
qDebug() << "[GVSPParser] Image leader too small";
return;
}
const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));
m_dataType = 1;
@@ -115,10 +122,10 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
else if (payload_type == PAYLOAD_TYPE_BINARY) {
// Depth data leader
const GVSPBinaryDataLeader *leader = reinterpret_cast<const GVSPBinaryDataLeader*>(data + sizeof(GVSPPacketHeader));
m_dataType = 3;
m_expectedSize = ntohl(leader->file_size);
m_dataBuffer.clear();
m_dataBuffer.reserve(m_expectedSize);
m_receivedSize = 0;
@@ -129,6 +136,22 @@ void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
// qDebug() << "Depth Leader: Block" << m_currentBlockId
// << "Size:" << m_expectedSize << "bytes";
}
else if (payload_type == PAYLOAD_TYPE_POINTCLOUD) {
// Point cloud data leader (vendor-specific 0x8000)
const GVSPPointCloudDataLeader *leader = reinterpret_cast<const GVSPPointCloudDataLeader*>(data + sizeof(GVSPPacketHeader));
m_dataType = 4; // 新类型：点云数据
m_expectedSize = ntohl(leader->data_size);
m_dataBuffer.clear();
m_dataBuffer.reserve(m_expectedSize);
m_receivedSize = 0;
m_isReceiving = true;
m_packetCount = 0;
// qDebug() << "[PointCloud Leader] Block:" << m_currentBlockId
// << "Expected Size:" << m_expectedSize << "bytes";
}
}
void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
@@ -167,6 +190,8 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
processImageData();
} else if (m_dataType == 3) {
processDepthData();
} else if (m_dataType == 4) {
processPointCloudData();
}
// Reset state
@@ -177,43 +202,60 @@ void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
void GVSPParser::processImageData()
{
if (m_dataBuffer.size() < m_expectedSize) {
// 注释掉频繁的警告日志
// qDebug() << "Warning: Incomplete image data" << m_dataBuffer.size() << "/" << m_expectedSize;
return;
}
// Convert 16-bit depth data to 8-bit grayscale for display
const uint16_t *src = reinterpret_cast<const uint16_t*>(m_dataBuffer.constData());
QImage image(m_imageWidth, m_imageHeight, QImage::Format_Grayscale8);
// Find min/max for normalization
uint16_t minVal = 65535, maxVal = 0;
for (size_t i = 0; i < m_imageWidth * m_imageHeight; i++) {
uint16_t val = src[i];
if (val > 0) {
if (val < minVal) minVal = val;
if (val > maxVal) maxVal = val;
}
// 节流机制：如果已有3个或更多图像在处理中，跳过当前帧
if (m_imageProcessingCount.loadAcquire() >= 3) {
return;
}
// Normalize to 0-255
uint8_t *dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
// 增加处理计数
m_imageProcessingCount.ref();
for (size_t y = 0; y < m_imageHeight; y++) {
for (size_t x = 0; x < m_imageWidth; x++) {
size_t idx = y * m_imageWidth + x;
// 复制数据到局部变量，避免在异步处理时数据被覆盖
QByteArray dataCopy = m_dataBuffer;
uint32_t blockId = m_currentBlockId;
size_t width = m_imageWidth;
size_t height = m_imageHeight;
uint16_t val = src[idx];
if (val == 0) {
dst[idx] = 0;
} else {
dst[idx] = static_cast<uint8_t>((val - minVal) * scale);
// 使用QtConcurrent在后台线程处理图像数据
QtConcurrent::run([this, dataCopy, blockId, width, height]() {
// Convert 16-bit depth data to 8-bit grayscale for display
const uint16_t *src = reinterpret_cast<const uint16_t*>(dataCopy.constData());
QImage image(width, height, QImage::Format_Grayscale8);
// 优化：使用采样方式快速估算min/max（每隔16个像素采样一次）
uint16_t minVal = 65535, maxVal = 0;
size_t totalPixels = width * height;
const size_t sampleStep = 16; // 采样步长
for (size_t i = 0; i < totalPixels; i += sampleStep) {
uint16_t val = src[i];
if (val > 0) {
if (val < minVal) minVal = val;
if (val > maxVal) maxVal = val;
}
}
}
emit imageReceived(image, m_currentBlockId);
// 归一化并水平翻转（修正左右镜像）
uint8_t *dst = image.bits();
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
for (size_t row = 0; row < height; row++) {
for (size_t col = 0; col < width; col++) {
size_t srcIdx = row * width + col;
size_t dstIdx = row * width + (width - 1 - col); // 水平翻转
uint16_t val = src[srcIdx];
dst[dstIdx] = (val == 0) ? 0 : static_cast<uint8_t>((val - minVal) * scale);
}
}
emit imageReceived(image, blockId);
// 减少处理计数
m_imageProcessingCount.deref();
});
}
void GVSPParser::processDepthData()
@@ -226,3 +268,18 @@ void GVSPParser::processDepthData()
emit depthDataReceived(m_dataBuffer, m_currentBlockId);
}
void GVSPParser::processPointCloudData()
{
// qDebug() << "[PointCloud] Received:" << m_dataBuffer.size()
// << "bytes, Expected:" << m_expectedSize << "bytes";
if (m_dataBuffer.size() < m_expectedSize) {
qDebug() << "[PointCloud] ERROR: Data incomplete, skipping!";
return;
}
// qDebug() << "[PointCloud] Data complete, emitting pointCloudDataReceived signal...";
// 点云数据直接发送，格式为 short 数组 (x, y, z, x, y, z, ...)
emit pointCloudDataReceived(m_dataBuffer, m_currentBlockId);
}