feat: v0.1.0更新
This commit is contained in:
229
src/core/GVSPParser.cpp
Normal file
229
src/core/GVSPParser.cpp
Normal file
@@ -0,0 +1,229 @@
|
||||
#include "core/GVSPParser.h"
|
||||
#include <QDebug>
|
||||
#include <cstring>
|
||||
#include <winsock2.h>
|
||||
|
||||
GVSPParser::GVSPParser(QObject *parent)
|
||||
: QObject(parent)
|
||||
, m_isReceiving(false)
|
||||
, m_dataType(0)
|
||||
, m_currentBlockId(0)
|
||||
, m_expectedSize(0)
|
||||
, m_receivedSize(0)
|
||||
, m_imageWidth(0)
|
||||
, m_imageHeight(0)
|
||||
, m_pixelFormat(0)
|
||||
, m_lastBlockId(0)
|
||||
, m_packetCount(0)
|
||||
{
|
||||
}
|
||||
|
||||
GVSPParser::~GVSPParser()
|
||||
{
|
||||
}
|
||||
|
||||
void GVSPParser::reset()
|
||||
{
|
||||
m_isReceiving = false;
|
||||
m_dataType = 0;
|
||||
m_currentBlockId = 0;
|
||||
m_dataBuffer.clear();
|
||||
m_expectedSize = 0;
|
||||
m_receivedSize = 0;
|
||||
m_packetCount = 0;
|
||||
}
|
||||
|
||||
void GVSPParser::parsePacket(const QByteArray &packet)
|
||||
{
|
||||
if (packet.size() < sizeof(GVSPPacketHeader)) {
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
uint16_t blockId = ntohs(header->block_id);
|
||||
|
||||
// 注释掉频繁的日志输出以提高性能
|
||||
// static int leaderCount = 0;
|
||||
// static int trailerCount = 0;
|
||||
|
||||
switch (packetType) {
|
||||
case GVSP_LEADER_PACKET:
|
||||
handleLeaderPacket(data, packet.size());
|
||||
break;
|
||||
case GVSP_PAYLOAD_PACKET:
|
||||
handlePayloadPacket(data, packet.size());
|
||||
break;
|
||||
case GVSP_TRAILER_PACKET:
|
||||
handleTrailerPacket(data, packet.size());
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void GVSPParser::handleLeaderPacket(const uint8_t *data, size_t size)
|
||||
{
|
||||
if (size < sizeof(GVSPPacketHeader) + sizeof(GVSPImageDataLeader)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const GVSPPacketHeader *header = reinterpret_cast<const GVSPPacketHeader*>(data);
|
||||
m_currentBlockId = ntohs(header->block_id);
|
||||
|
||||
// Check payload type
|
||||
const uint16_t *payload_type_ptr = reinterpret_cast<const uint16_t*>(data + sizeof(GVSPPacketHeader) + 2);
|
||||
uint16_t payload_type = ntohs(*payload_type_ptr);
|
||||
|
||||
if (payload_type == PAYLOAD_TYPE_IMAGE) {
|
||||
// Image data leader
|
||||
const GVSPImageDataLeader *leader = reinterpret_cast<const GVSPImageDataLeader*>(data + sizeof(GVSPPacketHeader));
|
||||
|
||||
m_dataType = 1;
|
||||
m_imageWidth = ntohl(leader->size_x);
|
||||
m_imageHeight = ntohl(leader->size_y);
|
||||
m_pixelFormat = ntohl(leader->pixel_format);
|
||||
m_expectedSize = m_imageWidth * m_imageHeight * 2; // 12-bit packed in 16-bit
|
||||
|
||||
m_dataBuffer.clear();
|
||||
m_dataBuffer.reserve(m_expectedSize);
|
||||
m_receivedSize = 0;
|
||||
m_isReceiving = true;
|
||||
m_packetCount = 0;
|
||||
|
||||
// 注释掉频繁的日志输出
|
||||
// qDebug() << "Image Leader: Block" << m_currentBlockId
|
||||
// << "Size:" << m_imageWidth << "x" << m_imageHeight;
|
||||
}
|
||||
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;
|
||||
m_isReceiving = true;
|
||||
m_packetCount = 0;
|
||||
|
||||
// 注释掉频繁的日志输出
|
||||
// qDebug() << "Depth Leader: Block" << m_currentBlockId
|
||||
// << "Size:" << m_expectedSize << "bytes";
|
||||
}
|
||||
}
|
||||
|
||||
void GVSPParser::handlePayloadPacket(const uint8_t *data, size_t size)
|
||||
{
|
||||
if (!m_isReceiving) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (size <= sizeof(GVSPPacketHeader)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Extract payload data (skip header)
|
||||
const uint8_t *payload = data + sizeof(GVSPPacketHeader);
|
||||
size_t payload_size = size - sizeof(GVSPPacketHeader);
|
||||
|
||||
// Append to buffer
|
||||
m_dataBuffer.append(reinterpret_cast<const char*>(payload), payload_size);
|
||||
m_receivedSize += payload_size;
|
||||
m_packetCount++;
|
||||
}
|
||||
|
||||
void GVSPParser::handleTrailerPacket(const uint8_t *data, size_t size)
|
||||
{
|
||||
if (!m_isReceiving) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 注释掉频繁的日志输出
|
||||
// qDebug() << "Trailer received: Block" << m_currentBlockId
|
||||
// << "Received" << m_receivedSize << "/" << m_expectedSize << "bytes"
|
||||
// << "Packets:" << m_packetCount;
|
||||
|
||||
// Process complete data
|
||||
if (m_dataType == 1) {
|
||||
processImageData();
|
||||
} else if (m_dataType == 3) {
|
||||
processDepthData();
|
||||
}
|
||||
|
||||
// Reset state
|
||||
m_isReceiving = false;
|
||||
m_lastBlockId = m_currentBlockId;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize to 0-255 and flip vertically
|
||||
uint8_t *dst = image.bits();
|
||||
float scale = (maxVal > minVal) ? (255.0f / (maxVal - minVal)) : 0.0f;
|
||||
|
||||
for (size_t y = 0; y < m_imageHeight; y++) {
|
||||
for (size_t x = 0; x < m_imageWidth; x++) {
|
||||
size_t src_idx = y * m_imageWidth + x;
|
||||
size_t dst_idx = (m_imageHeight - 1 - y) * m_imageWidth + x; // 垂直翻转
|
||||
|
||||
uint16_t val = src[src_idx];
|
||||
if (val == 0) {
|
||||
dst[dst_idx] = 0;
|
||||
} else {
|
||||
dst[dst_idx] = static_cast<uint8_t>((val - minVal) * scale);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
emit imageReceived(image, m_currentBlockId);
|
||||
}
|
||||
|
||||
void GVSPParser::processDepthData()
|
||||
{
|
||||
if (m_dataBuffer.size() < m_expectedSize) {
|
||||
// 注释掉频繁的警告日志
|
||||
// qDebug() << "Warning: Incomplete depth data" << m_dataBuffer.size() << "/" << m_expectedSize;
|
||||
return;
|
||||
}
|
||||
|
||||
emit depthDataReceived(m_dataBuffer, m_currentBlockId);
|
||||
}
|
||||
Reference in New Issue
Block a user