229 lines
6.7 KiB
C++
229 lines
6.7 KiB
C++
#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
|
||
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 idx = y * m_imageWidth + x;
|
||
|
||
uint16_t val = src[idx];
|
||
if (val == 0) {
|
||
dst[idx] = 0;
|
||
} else {
|
||
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);
|
||
}
|