1286 lines
39 KiB
C++
1286 lines
39 KiB
C++
#include "RsLidarDevice.h"
|
||
#include <cmath>
|
||
#include <cstdio>
|
||
#include <chrono>
|
||
#include <cstring>
|
||
#include <thread>
|
||
#include <cstdlib>
|
||
#include <new>
|
||
#ifdef _WIN32
|
||
#include <malloc.h>
|
||
#endif
|
||
#include <rs_driver/driver/decoder/compress_algo.hpp>
|
||
|
||
namespace
|
||
{
|
||
constexpr uint32_t kRsem4DefaultRings = 520;
|
||
constexpr uint32_t kRsem4ReserveColumns = 1200;
|
||
constexpr uint32_t kRsem4HalfRings = kRsem4DefaultRings / 2;
|
||
constexpr size_t kRsem4VecselsPerColumn = 26;
|
||
constexpr size_t kRsem4PixelsPerVcsel = 20;
|
||
constexpr size_t kRsem4SurfaceCount = 4;
|
||
constexpr size_t kRsem4BlocksPerPacket = 260;
|
||
constexpr size_t kRsem4MsopTailLen = 12;
|
||
constexpr size_t kDeliveryFramePoolSize = 6;
|
||
constexpr float kRsem4DistanceResolution = 0.005f;
|
||
constexpr float kRsem4MinDistance = 0.5f;
|
||
constexpr float kRsem4MaxDistance = 350.0f;
|
||
constexpr double kDeg01ToRad = 3.14159265358979323846 / 18000.0;
|
||
|
||
#pragma pack(push, 1)
|
||
struct Rsem4DifopPkt
|
||
{
|
||
uint8_t id[8];
|
||
uint8_t reserved0[106];
|
||
int8_t yawOffset[26];
|
||
int16_t pitchAngle[520];
|
||
int16_t surfacePitchOffset[4];
|
||
uint8_t reserved1[110];
|
||
uint16_t dataLength;
|
||
uint16_t counter;
|
||
uint32_t dataId;
|
||
uint32_t crc32;
|
||
};
|
||
|
||
struct Rsem4Difop2Pkt
|
||
{
|
||
uint8_t id[4];
|
||
uint8_t reserved0[63];
|
||
uint8_t surfaceId;
|
||
uint8_t pixelCnt;
|
||
uint8_t vcselCnt;
|
||
int8_t yawOffset[26];
|
||
int16_t pitchAngle[520];
|
||
int16_t surfacePitchOffset[4];
|
||
int16_t rollOffset;
|
||
uint8_t reserved1[4];
|
||
uint16_t dataLength;
|
||
uint16_t counter;
|
||
uint32_t dataId;
|
||
uint32_t crc32;
|
||
};
|
||
|
||
struct Rsem4Difop0624Pkt
|
||
{
|
||
uint8_t id[4];
|
||
uint8_t reserved0[306];
|
||
int8_t yawOffset[26];
|
||
int16_t pitchAngle[520];
|
||
int16_t surfacePitchOffset[4];
|
||
uint8_t reserved1[6];
|
||
uint16_t dataLength;
|
||
uint16_t counter;
|
||
uint32_t dataId;
|
||
uint32_t crc32;
|
||
};
|
||
|
||
struct Rsem4Channel
|
||
{
|
||
uint16_t distance;
|
||
uint8_t intensity;
|
||
uint8_t pointAttribute;
|
||
};
|
||
|
||
struct Rsem4Block
|
||
{
|
||
Rsem4Channel channel[1];
|
||
};
|
||
|
||
struct Rsem4MsopHeader
|
||
{
|
||
uint8_t id[4];
|
||
uint16_t pktSeq;
|
||
uint16_t protocolVersion;
|
||
uint8_t returnMode;
|
||
uint8_t timeMode;
|
||
RSTimestampUTC timestamp;
|
||
uint8_t frameSync;
|
||
uint8_t frameRate;
|
||
uint16_t columnNum;
|
||
int16_t yawAngle;
|
||
uint8_t packMode;
|
||
uint8_t surfaceId;
|
||
uint16_t reserved;
|
||
uint8_t lidarType;
|
||
uint8_t temperature;
|
||
};
|
||
|
||
struct Rsem4MsopHeader2
|
||
{
|
||
uint8_t id[4];
|
||
uint16_t pktSeq;
|
||
uint8_t reserved[2];
|
||
};
|
||
|
||
struct Rsem4MsopPkt
|
||
{
|
||
Rsem4MsopHeader header;
|
||
Rsem4Block blocks[260];
|
||
uint16_t dataLength;
|
||
uint16_t counter;
|
||
uint32_t dataId;
|
||
uint32_t crc32;
|
||
};
|
||
#pragma pack(pop)
|
||
|
||
inline void SetZeroPoint(SVzNLPointXYZI& dst)
|
||
{
|
||
dst.fData[0] = 0.0f;
|
||
dst.fData[1] = 0.0f;
|
||
dst.fData[2] = 0.0f;
|
||
dst.fData_c[0] = 0.0f;
|
||
}
|
||
|
||
inline int16_t SwapI16(int16_t value)
|
||
{
|
||
uint16_t v = static_cast<uint16_t>(value);
|
||
v = static_cast<uint16_t>((v << 8) | (v >> 8));
|
||
return static_cast<int16_t>(v);
|
||
}
|
||
|
||
inline uint16_t ReadU16BE(uint16_t value)
|
||
{
|
||
return static_cast<uint16_t>((value << 8) | (value >> 8));
|
||
}
|
||
|
||
inline int32_t NormalizeAngle01(int32_t angle)
|
||
{
|
||
angle %= 36000;
|
||
if (angle < 0)
|
||
angle += 36000;
|
||
return angle;
|
||
}
|
||
|
||
struct Deg01TrigTable
|
||
{
|
||
std::array<float, 36000> sinValues;
|
||
std::array<float, 36000> cosValues;
|
||
|
||
Deg01TrigTable()
|
||
{
|
||
for (size_t i = 0; i < sinValues.size(); ++i)
|
||
{
|
||
const double rad = static_cast<double>(i) * kDeg01ToRad;
|
||
sinValues[i] = static_cast<float>(std::sin(rad));
|
||
cosValues[i] = static_cast<float>(std::cos(rad));
|
||
}
|
||
}
|
||
};
|
||
|
||
inline const Deg01TrigTable& TrigTable()
|
||
{
|
||
static const Deg01TrigTable table;
|
||
return table;
|
||
}
|
||
|
||
inline float SinDeg01(int32_t angle)
|
||
{
|
||
return TrigTable().sinValues[NormalizeAngle01(angle)];
|
||
}
|
||
|
||
inline float CosDeg01(int32_t angle)
|
||
{
|
||
return TrigTable().cosValues[NormalizeAngle01(angle)];
|
||
}
|
||
|
||
inline bool IsCompleteRsem4Packet(const uint8_t* data, size_t size)
|
||
{
|
||
static const uint8_t kHeader[] = {0x55, 0xAA, 0x5A, 0xA5};
|
||
return size >= sizeof(kHeader) && std::memcmp(data, kHeader, sizeof(kHeader)) == 0;
|
||
}
|
||
|
||
inline void UpdateMax(std::atomic<uint64_t>& target, uint64_t value)
|
||
{
|
||
uint64_t prev = target.load(std::memory_order_relaxed);
|
||
while (value > prev &&
|
||
!target.compare_exchange_weak(prev, value, std::memory_order_relaxed))
|
||
{}
|
||
}
|
||
|
||
inline void UpdateMaxSize(std::atomic<size_t>& target, size_t value)
|
||
{
|
||
size_t prev = target.load(std::memory_order_relaxed);
|
||
while (value > prev &&
|
||
!target.compare_exchange_weak(prev, value, std::memory_order_relaxed))
|
||
{}
|
||
}
|
||
|
||
inline uint64_t ElapsedUs(std::chrono::steady_clock::time_point begin,
|
||
std::chrono::steady_clock::time_point end)
|
||
{
|
||
return static_cast<uint64_t>(
|
||
std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count());
|
||
}
|
||
}
|
||
|
||
// WSAStartup 引用计数
|
||
static std::atomic<int> g_sockRefCount{0};
|
||
|
||
// ============================================================
|
||
// SDK ErrCode → 统计桶映射
|
||
// SDK error_code.hpp: MSOPTIMEOUT=0x40 WRONGMSOPLEN=0x42 PKTBUFOVERFLOW=0x48 CLOUDOVERFLOW=0x49
|
||
// 桶顺序与日志列对齐：0=msopto 1=pktof 2=cldof 3=wlen 4=other
|
||
// ============================================================
|
||
size_t CRsLidarDevice::errCodeToBucket(int errCode)
|
||
{
|
||
switch (errCode)
|
||
{
|
||
case 0x40: return 0; // MSOPTIMEOUT
|
||
case 0x48: return 1; // PKTBUFOVERFLOW
|
||
case 0x49: return 2; // CLOUDOVERFLOW
|
||
case 0x42: return 3; // WRONGMSOPLEN
|
||
default: return 4; // other
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 工厂方法
|
||
// ============================================================
|
||
int IRsLidarDevice::CreateObject(IRsLidarDevice** ppDevice)
|
||
{
|
||
if (!ppDevice) return -1;
|
||
CRsLidarDevice* p = new CRsLidarDevice();
|
||
*ppDevice = p;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// 构造 / 析构
|
||
// ============================================================
|
||
CRsLidarDevice::CRsLidarDevice()
|
||
: m_pDriver(std::make_unique<LidarDriver<SdkCloudMsg>>())
|
||
{
|
||
initializeRsem4DefaultAngles();
|
||
}
|
||
|
||
CRsLidarDevice::~CRsLidarDevice()
|
||
{
|
||
Stop();
|
||
CloseDevice();
|
||
}
|
||
|
||
void* CRsLidarDevice::operator new(std::size_t size)
|
||
{
|
||
#ifdef _WIN32
|
||
void* ptr = _aligned_malloc(size, 64);
|
||
if (!ptr)
|
||
throw std::bad_alloc();
|
||
return ptr;
|
||
#else
|
||
void* ptr = nullptr;
|
||
if (posix_memalign(&ptr, 64, size) != 0)
|
||
throw std::bad_alloc();
|
||
return ptr;
|
||
#endif
|
||
}
|
||
|
||
void CRsLidarDevice::operator delete(void* ptr) noexcept
|
||
{
|
||
#ifdef _WIN32
|
||
_aligned_free(ptr);
|
||
#else
|
||
std::free(ptr);
|
||
#endif
|
||
}
|
||
|
||
void CRsLidarDevice::operator delete(void* ptr, std::size_t) noexcept
|
||
{
|
||
CRsLidarDevice::operator delete(ptr);
|
||
}
|
||
|
||
// ============================================================
|
||
// InitDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::InitDevice()
|
||
{
|
||
#ifdef _WIN32
|
||
if (g_sockRefCount == 0)
|
||
{
|
||
WSADATA wsaData;
|
||
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
g_sockRefCount++;
|
||
#endif
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// 类型转换: RsLidarConfig → RSDriverParam
|
||
// ============================================================
|
||
RSDriverParam CRsLidarDevice::toDriverParam(const RsLidarConfig& config)
|
||
{
|
||
RSDriverParam param;
|
||
param.lidar_type = strToLidarType(config.lidarType);
|
||
param.input_type = InputType::ONLINE_LIDAR;
|
||
param.frame_id = config.frameId;
|
||
param.input_param.host_address = config.hostAddress;
|
||
param.input_param.msop_port = config.msopPort;
|
||
param.input_param.difop_port = config.difopPort;
|
||
param.input_param.imu_port = config.imuPort;
|
||
param.decoder_param.min_distance = config.minDistance;
|
||
param.decoder_param.max_distance = config.maxDistance;
|
||
param.decoder_param.dense_points = config.densePoints;
|
||
param.decoder_param.use_lidar_clock = config.useLidarClock;
|
||
param.decoder_param.ts_first_point = config.tsFirstPoint;
|
||
param.decoder_param.wait_for_difop = config.waitForDifop;
|
||
param.decoder_param.start_angle = config.startAngle;
|
||
param.decoder_param.end_angle = config.endAngle;
|
||
param.decoder_param.split_angle = config.splitAngle;
|
||
|
||
param.input_param.socket_recv_buf = (config.socketRecvBufBytes > 0)
|
||
? config.socketRecvBufBytes
|
||
: 33554432; // 32MB，低配电脑防内核缓冲溢出
|
||
|
||
return param;
|
||
}
|
||
|
||
// ============================================================
|
||
// OpenDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::OpenDevice(const RsLidarConfig& config)
|
||
{
|
||
if (!m_pDriver)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
CloseDevice();
|
||
|
||
m_param = toDriverParam(config);
|
||
if (config.minDistance > 0.0f || config.maxDistance > 0.0f)
|
||
{
|
||
m_rsem4MinDistance = (config.minDistance > 0.0f) ? config.minDistance : 0.0f;
|
||
m_rsem4MaxDistance = (config.maxDistance > 0.0f) ? config.maxDistance : kRsem4MaxDistance;
|
||
}
|
||
else
|
||
{
|
||
m_rsem4MinDistance = kRsem4MinDistance;
|
||
m_rsem4MaxDistance = kRsem4MaxDistance;
|
||
}
|
||
|
||
m_pDriver->regPointCloudCallback(
|
||
[this]() -> SdkCloudPtr { return this->getFreeCloud(); },
|
||
[this](SdkCloudPtr msg) { this->putStuffedCloud(msg); }
|
||
);
|
||
|
||
m_pDriver->regPacketCallback(
|
||
[this](const Packet& pkt) { this->onPacket(pkt); }
|
||
);
|
||
|
||
m_pDriver->regExceptionCallback(
|
||
[this](const Error& code) { this->onException(code); }
|
||
);
|
||
|
||
if (!m_pDriver->init(m_param))
|
||
{
|
||
return -2;
|
||
}
|
||
|
||
m_bOpened = true;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// CloseDevice
|
||
// ============================================================
|
||
int CRsLidarDevice::CloseDevice()
|
||
{
|
||
if (m_bRunning)
|
||
{
|
||
Stop();
|
||
}
|
||
|
||
m_bOpened = false;
|
||
|
||
m_freeQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
m_deliveryQueue.clear();
|
||
|
||
#ifdef _WIN32
|
||
if (--g_sockRefCount == 0)
|
||
{
|
||
WSACleanup();
|
||
}
|
||
#endif
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool CRsLidarDevice::IsOpened() const
|
||
{
|
||
return m_bOpened;
|
||
}
|
||
|
||
// ============================================================
|
||
// Start / Stop
|
||
// ============================================================
|
||
int CRsLidarDevice::Start()
|
||
{
|
||
if (!m_bOpened || !m_pDriver)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
if (m_bRunning)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
// 重置诊断计数（每次 Start 开始新会话统计）
|
||
m_droppedFrameCount.store(0, std::memory_order_relaxed);
|
||
m_totalPushCount.store(0, std::memory_order_relaxed);
|
||
for (auto& c : m_exceptionCounts) c.store(0, std::memory_order_relaxed);
|
||
m_parseAccumUs.store(0, std::memory_order_relaxed);
|
||
m_parseMaxUs.store(0, std::memory_order_relaxed);
|
||
m_parseCount.store(0, std::memory_order_relaxed);
|
||
m_frameWaitAccumUs.store(0, std::memory_order_relaxed);
|
||
m_frameWaitMaxUs.store(0, std::memory_order_relaxed);
|
||
m_frameWaitCount.store(0, std::memory_order_relaxed);
|
||
m_frameWaitTimeoutCount.store(0, std::memory_order_relaxed);
|
||
m_deliveryDropCount.store(0, std::memory_order_relaxed);
|
||
m_deliveredFrameCount.store(0, std::memory_order_relaxed);
|
||
m_deliveryQueuePeak.store(0, std::memory_order_relaxed);
|
||
m_cbMaxUs.store(0, std::memory_order_relaxed);
|
||
m_cbAccumUs.store(0, std::memory_order_relaxed);
|
||
m_cbCount.store(0, std::memory_order_relaxed);
|
||
m_diagLastPushCount = 0;
|
||
m_diagLastQueueDropCount = 0;
|
||
m_diagLastParseCount = 0;
|
||
m_diagLastParseAccumUs = 0;
|
||
m_diagLastFrameWaitCount = 0;
|
||
m_diagLastFrameWaitAccumUs = 0;
|
||
m_diagLastFrameWaitTimeoutCount = 0;
|
||
m_diagLastDeliveryDropCount = 0;
|
||
m_diagLastDeliveredFrameCount = 0;
|
||
m_diagLastCbCount = 0;
|
||
m_diagLastCbAccumUs = 0;
|
||
m_diagLastExceptionCounts.fill(0);
|
||
m_diagLastLogTime = std::chrono::steady_clock::now();
|
||
resetPacketFrameBuilder();
|
||
|
||
// 清空可能残留的预热缓冲（防止 Stop→Start 循环累积膨胀）
|
||
m_freeQueue.clear();
|
||
|
||
for (int i = 0; i < 4; ++i)
|
||
{
|
||
auto msg = std::make_shared<SdkCloudMsg>();
|
||
m_freeQueue.push(std::move(msg));
|
||
}
|
||
|
||
m_bStopProcess = false;
|
||
|
||
// 预分配交付帧：6 帧 × 1200 线 × 520 点 × 32B ≈ 120MB
|
||
// 帧 0=SDK packet callback 填充中，其余用于交付队列和短时回调抖动缓冲
|
||
m_deliveryQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
for (size_t i = 0; i < kDeliveryFramePoolSize; ++i)
|
||
{
|
||
auto frame = std::make_shared<DeliveryFrame>();
|
||
frame->reserveLines(kRsem4ReserveColumns, kRsem4DefaultRings);
|
||
m_frameFreeQueue.push(frame);
|
||
}
|
||
|
||
m_bRunning = true;
|
||
m_bStopDelivery = false;
|
||
m_deliveryThread = std::thread(&CRsLidarDevice::deliveryThread, this);
|
||
m_pDriver->start();
|
||
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::Stop()
|
||
{
|
||
if (!m_bRunning)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
// ① 停止包级解析
|
||
m_bStopProcess = true;
|
||
|
||
if (m_pDriver)
|
||
{
|
||
m_pDriver->stop();
|
||
}
|
||
resetPacketFrameBuilder();
|
||
|
||
// ② 停止交付线程（推入空帧唤醒 popWait，避免等超时）
|
||
m_bStopDelivery = true;
|
||
m_deliveryQueue.push(std::make_shared<DeliveryFrame>());
|
||
if (m_deliveryThread.joinable())
|
||
{
|
||
m_deliveryThread.join();
|
||
}
|
||
|
||
// ③ 释放所有队列内存
|
||
m_deliveryQueue.clear();
|
||
m_frameFreeQueue.clear();
|
||
|
||
m_bRunning = false;
|
||
return 0;
|
||
}
|
||
|
||
bool CRsLidarDevice::IsRunning() const
|
||
{
|
||
return m_bRunning;
|
||
}
|
||
|
||
// ============================================================
|
||
// 回调注册
|
||
// ============================================================
|
||
int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_pointCloudCallback = std::move(callback);
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::SetPacketCallback(PacketCallback callback)
|
||
{
|
||
bool hasCallback = false;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_packetCallback = std::move(callback);
|
||
hasCallback = static_cast<bool>(m_packetCallback);
|
||
}
|
||
m_hasPacketCallback.store(hasCallback, std::memory_order_release);
|
||
return 0;
|
||
}
|
||
|
||
int CRsLidarDevice::SetExceptionCallback(ExceptionCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
m_exceptionCallback = std::move(callback);
|
||
return 0;
|
||
}
|
||
|
||
std::string CRsLidarDevice::GetVersion()
|
||
{
|
||
return getDriverVersion();
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：5s 窗口诊断日志输出 + 字段重置（仅当距上次输出 ≥5s 时实际输出）
|
||
// 由 SDK 包处理线程触发，计数器只做 relaxed 采样，避免诊断影响实时路径。
|
||
// ============================================================
|
||
void CRsLidarDevice::emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime)
|
||
{
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::seconds>(now - lastLogTime).count() < 5)
|
||
{
|
||
return;
|
||
}
|
||
|
||
const double sec = std::chrono::duration_cast<std::chrono::duration<double>>(now - lastLogTime).count();
|
||
|
||
const uint64_t pushCount = m_totalPushCount.load(std::memory_order_relaxed);
|
||
const uint64_t queueDropCount = m_droppedFrameCount.load(std::memory_order_relaxed);
|
||
const uint64_t frameWaitTimeoutCount = m_frameWaitTimeoutCount.load(std::memory_order_relaxed);
|
||
const uint64_t cbCount = m_cbCount.load(std::memory_order_relaxed);
|
||
const uint64_t cbAccumUs = m_cbAccumUs.load(std::memory_order_relaxed);
|
||
|
||
const uint64_t pushDelta = pushCount - m_diagLastPushCount;
|
||
const uint64_t queueDropDelta = queueDropCount - m_diagLastQueueDropCount;
|
||
const uint64_t frameWaitTimeoutDelta = frameWaitTimeoutCount - m_diagLastFrameWaitTimeoutCount;
|
||
const uint64_t cbDelta = cbCount - m_diagLastCbCount;
|
||
const uint64_t cbAccumDelta = cbAccumUs - m_diagLastCbAccumUs;
|
||
|
||
uint64_t excDelta[EXC_BUCKET_COUNT] = {};
|
||
for (size_t i = 0; i < EXC_BUCKET_COUNT; ++i)
|
||
{
|
||
const uint64_t current = m_exceptionCounts[i].load(std::memory_order_relaxed);
|
||
excDelta[i] = current - m_diagLastExceptionCounts[i];
|
||
m_diagLastExceptionCounts[i] = current;
|
||
}
|
||
|
||
fprintf(stderr,
|
||
"[RsLidarDevice][diag %.1fs] fps=%.1f frames=%llu(+%llu) "
|
||
"drop=%llu(+%llu) q=%zu/%zu waitTO=%llu(+%llu) "
|
||
"cb_us=%llu/%llu n=%llu exc48=%llu exc49=%llu exc42=%llu\n",
|
||
sec,
|
||
(sec > 0.0) ? (static_cast<double>(pushDelta) / sec) : 0.0,
|
||
static_cast<unsigned long long>(pushCount),
|
||
static_cast<unsigned long long>(pushDelta),
|
||
static_cast<unsigned long long>(queueDropCount),
|
||
static_cast<unsigned long long>(queueDropDelta),
|
||
static_cast<size_t>(m_deliveryQueue.size()),
|
||
static_cast<size_t>(m_deliveryQueuePeak.load(std::memory_order_relaxed)),
|
||
static_cast<unsigned long long>(frameWaitTimeoutCount),
|
||
static_cast<unsigned long long>(frameWaitTimeoutDelta),
|
||
static_cast<unsigned long long>((cbDelta > 0) ? (cbAccumDelta / cbDelta) : 0),
|
||
static_cast<unsigned long long>(m_cbMaxUs.load(std::memory_order_relaxed)),
|
||
static_cast<unsigned long long>(cbDelta),
|
||
static_cast<unsigned long long>(excDelta[2]),
|
||
static_cast<unsigned long long>(excDelta[3]),
|
||
static_cast<unsigned long long>(excDelta[4]));
|
||
|
||
m_diagLastPushCount = pushCount;
|
||
m_diagLastQueueDropCount = queueDropCount;
|
||
m_diagLastFrameWaitTimeoutCount = frameWaitTimeoutCount;
|
||
m_diagLastCbCount = cbCount;
|
||
m_diagLastCbAccumUs = cbAccumUs;
|
||
lastLogTime = now;
|
||
}
|
||
|
||
// 内部：交付线程（只调回调，不做数据处理）
|
||
// 从 m_deliveryQueue 取帧 → 调 PointCloudCallback（PushFrame memcpy 在此发生）
|
||
// 与 SDK packet callback 并行：交付帧 N 的同时，解析帧 N+1
|
||
// ============================================================
|
||
void CRsLidarDevice::deliveryThread()
|
||
{
|
||
while (!m_bStopDelivery)
|
||
{
|
||
auto frame = m_deliveryQueue.popWait(500000);
|
||
if (!frame) continue; // 超时 → 检查 m_bStopDelivery
|
||
|
||
// 取回调（持锁时间极短）
|
||
PointCloudCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_pointCloudCallback;
|
||
}
|
||
|
||
if (frame->cloudData.empty())
|
||
{
|
||
m_frameFreeQueue.push(frame);
|
||
continue;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
auto t0 = std::chrono::steady_clock::now();
|
||
cb(frame->cloudData, frame->info);
|
||
auto t1 = std::chrono::steady_clock::now();
|
||
|
||
// 累计回调耗时统计
|
||
uint64_t us = static_cast<uint64_t>(
|
||
std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count());
|
||
m_cbAccumUs.fetch_add(us, std::memory_order_relaxed);
|
||
m_cbCount.fetch_add(1, std::memory_order_relaxed);
|
||
|
||
uint64_t prevMax = m_cbMaxUs.load(std::memory_order_relaxed);
|
||
while (us > prevMax &&
|
||
!m_cbMaxUs.compare_exchange_weak(prevMax, us, std::memory_order_relaxed))
|
||
{}
|
||
}
|
||
|
||
// 归还交付帧到空闲池（清空 cloudData 元数据，保留 pointPool capacity）
|
||
m_deliveredFrameCount.fetch_add(1, std::memory_order_relaxed);
|
||
frame->cloudData.clear();
|
||
m_frameFreeQueue.push(frame);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：空闲/就绪队列回调（rs_driver 内部线程）
|
||
// ============================================================
|
||
CRsLidarDevice::SdkCloudPtr CRsLidarDevice::getFreeCloud()
|
||
{
|
||
auto msg = m_freeQueue.pop();
|
||
if (msg) return msg;
|
||
return std::make_shared<SdkCloudMsg>();
|
||
}
|
||
|
||
void CRsLidarDevice::putStuffedCloud(SdkCloudPtr msg)
|
||
{
|
||
if (msg)
|
||
{
|
||
msg->points.clear();
|
||
m_freeQueue.push(msg);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：Packet → RsPacketInfo
|
||
// ============================================================
|
||
void CRsLidarDevice::resetPacketFrameBuilder()
|
||
{
|
||
if (m_activeFrame)
|
||
{
|
||
m_activeFrame->cloudData.clear();
|
||
m_frameFreeQueue.push(m_activeFrame);
|
||
}
|
||
m_activeFrame.reset();
|
||
m_activeFrameHasData = false;
|
||
m_activeFrameAllValid = true;
|
||
m_activeFrameStartedAtBoundary = false;
|
||
resetActiveFrameTracking();
|
||
m_rsem4FrameSynced = false;
|
||
m_hasRsem4PrevSeq = false;
|
||
m_prevRsem4Seq = 0;
|
||
m_hasRsem4PrevColumn = false;
|
||
m_prevRsem4Column = 0;
|
||
m_rsem4HasPartialPacket = false;
|
||
m_rsem4PartialSeq = 0;
|
||
m_rsem4PartialLen = 0;
|
||
m_diagPacketTick = 0;
|
||
}
|
||
|
||
void CRsLidarDevice::initializeRsem4DefaultAngles()
|
||
{
|
||
m_rsem4YawOffset.fill(0);
|
||
m_rsem4SurfacePitchOffset.fill(0);
|
||
for (size_t i = 0; i < m_rsem4PitchAngle.size(); ++i)
|
||
m_rsem4PitchAngle[i] = static_cast<int16_t>(-1300 + static_cast<int>(i) * 5);
|
||
m_rsem4AnglesReady = false;
|
||
updateRsem4PitchTrig();
|
||
}
|
||
|
||
void CRsLidarDevice::updateRsem4PitchTrig()
|
||
{
|
||
for (size_t surface = 0; surface < kRsem4SurfaceCount; ++surface)
|
||
for (size_t i = 0; i < m_rsem4PitchAngle.size(); ++i)
|
||
{
|
||
const int pitch = static_cast<int>(m_rsem4PitchAngle[i]) +
|
||
static_cast<int>(m_rsem4SurfacePitchOffset[surface]);
|
||
m_rsem4PitchSin[surface][i] = SinDeg01(pitch);
|
||
m_rsem4PitchCos[surface][i] = CosDeg01(pitch);
|
||
}
|
||
}
|
||
|
||
DeliveryFramePtr CRsLidarDevice::acquireFrameForBuild()
|
||
{
|
||
while (!m_bStopProcess.load(std::memory_order_relaxed))
|
||
{
|
||
const auto t0 = std::chrono::steady_clock::now();
|
||
auto frame = m_frameFreeQueue.popWait(100000);
|
||
const auto t1 = std::chrono::steady_clock::now();
|
||
const uint64_t waitUs = ElapsedUs(t0, t1);
|
||
m_frameWaitAccumUs.fetch_add(waitUs, std::memory_order_relaxed);
|
||
m_frameWaitCount.fetch_add(1, std::memory_order_relaxed);
|
||
UpdateMax(m_frameWaitMaxUs, waitUs);
|
||
if (frame)
|
||
{
|
||
resetDeliveryFrame(frame);
|
||
return frame;
|
||
}
|
||
m_frameWaitTimeoutCount.fetch_add(1, std::memory_order_relaxed);
|
||
}
|
||
return DeliveryFramePtr();
|
||
}
|
||
|
||
void CRsLidarDevice::resetDeliveryFrame(const DeliveryFramePtr& frame)
|
||
{
|
||
if (!frame)
|
||
return;
|
||
|
||
const size_t pointCount =
|
||
static_cast<size_t>(kRsem4ReserveColumns) * static_cast<size_t>(kRsem4DefaultRings);
|
||
if (frame->pointPool.size() != pointCount)
|
||
frame->pointPool.resize(pointCount);
|
||
|
||
frame->cloudData.clear();
|
||
frame->cloudData.reserve(kRsem4ReserveColumns);
|
||
std::memset(frame->pointPool.data(), 0, frame->pointPool.size() * sizeof(SVzNLPointXYZI));
|
||
|
||
for (uint32_t col = 0; col < kRsem4ReserveColumns; ++col)
|
||
{
|
||
SVzNLPointXYZI* linePts = frame->pointPool.data() +
|
||
static_cast<size_t>(col) * static_cast<size_t>(kRsem4DefaultRings);
|
||
|
||
SVzLaserLineData ld{};
|
||
ld.p3DPoint = linePts;
|
||
ld.nPointCount = static_cast<int>(kRsem4DefaultRings);
|
||
ld.llFrameIdx = col;
|
||
ld.llTimeStamp = col;
|
||
ld.nEncodeNo = static_cast<int>(col);
|
||
ld.bEndOnceScan = (col + 1 == kRsem4ReserveColumns) ? VzTrue : VzFalse;
|
||
frame->cloudData.emplace_back(keResultDataType_PointXYZI, ld);
|
||
}
|
||
|
||
frame->info.height = kRsem4ReserveColumns;
|
||
frame->info.width = kRsem4DefaultRings;
|
||
frame->info.isDense = false;
|
||
}
|
||
|
||
void CRsLidarDevice::submitActiveFrame()
|
||
{
|
||
if (!m_activeFrame)
|
||
return;
|
||
|
||
auto frame = m_activeFrame;
|
||
m_activeFrame.reset();
|
||
|
||
const bool completeFrame =
|
||
m_activeFrameHasData &&
|
||
m_activeFrameStartedAtBoundary &&
|
||
!m_activeFramePacketLoss &&
|
||
m_activeCompleteColumns == kRsem4ReserveColumns;
|
||
|
||
if (!completeFrame)
|
||
{
|
||
frame->cloudData.clear();
|
||
m_frameFreeQueue.push(frame);
|
||
if (m_activeFrameStartedAtBoundary && m_activeFrameHasData)
|
||
m_droppedFrameCount.fetch_add(1, std::memory_order_relaxed);
|
||
m_activeFrameStartedAtBoundary = false;
|
||
resetActiveFrameTracking();
|
||
return;
|
||
}
|
||
|
||
frame->info.height = kRsem4ReserveColumns;
|
||
frame->info.width = kRsem4DefaultRings;
|
||
frame->info.isDense = m_activeFrameAllValid;
|
||
|
||
const uint64_t parseUs = ElapsedUs(m_activeFrameStartTime, std::chrono::steady_clock::now());
|
||
m_parseAccumUs.fetch_add(parseUs, std::memory_order_relaxed);
|
||
m_parseCount.fetch_add(1, std::memory_order_relaxed);
|
||
UpdateMax(m_parseMaxUs, parseUs);
|
||
|
||
while (!m_bStopDelivery.load(std::memory_order_relaxed))
|
||
{
|
||
const size_t depth = m_deliveryQueue.push(frame);
|
||
if (depth > 0)
|
||
{
|
||
UpdateMaxSize(m_deliveryQueuePeak, depth);
|
||
m_totalPushCount.fetch_add(1, std::memory_order_relaxed);
|
||
break;
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||
}
|
||
|
||
if (m_bStopDelivery.load(std::memory_order_relaxed))
|
||
m_frameFreeQueue.push(frame);
|
||
|
||
m_activeFrameHasData = false;
|
||
m_activeFrameAllValid = true;
|
||
m_activeFrameStartedAtBoundary = false;
|
||
resetActiveFrameTracking();
|
||
}
|
||
|
||
void CRsLidarDevice::resetActiveFrameTracking()
|
||
{
|
||
m_activeFramePacketLoss = false;
|
||
m_activeCompleteColumns = 0;
|
||
m_activeColumnCompleteMask = 0;
|
||
m_activeColumnMask.fill(0);
|
||
}
|
||
|
||
bool CRsLidarDevice::beginActiveFrame(bool startedAtBoundary)
|
||
{
|
||
if (m_activeFrame)
|
||
return true;
|
||
|
||
m_activeFrame = acquireFrameForBuild();
|
||
if (!m_activeFrame)
|
||
return false;
|
||
|
||
m_activeFrameStartTime = std::chrono::steady_clock::now();
|
||
m_activeFrameStartedAtBoundary = startedAtBoundary;
|
||
m_activeFrameHasData = false;
|
||
m_activeFrameAllValid = true;
|
||
resetActiveFrameTracking();
|
||
return true;
|
||
}
|
||
|
||
bool CRsLidarDevice::prepareRsem4FrameForColumn(uint32_t column)
|
||
{
|
||
if (column >= kRsem4ReserveColumns)
|
||
return false;
|
||
|
||
const bool columnWrap = m_hasRsem4PrevColumn && (column + 10 < m_prevRsem4Column);
|
||
const bool columnStart = (column == 0);
|
||
|
||
if (columnWrap && m_activeFrameHasData)
|
||
submitActiveFrame();
|
||
|
||
m_hasRsem4PrevColumn = true;
|
||
m_prevRsem4Column = column;
|
||
|
||
if (!m_activeFrame)
|
||
{
|
||
if (!columnStart)
|
||
return false;
|
||
return beginActiveFrame(true);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void CRsLidarDevice::markRsem4ColumnSegment(uint32_t column, uint8_t segmentMask, uint8_t completeMask)
|
||
{
|
||
if (!m_activeFrame || column >= kRsem4ReserveColumns || segmentMask == 0)
|
||
return;
|
||
|
||
if (m_activeColumnCompleteMask == 0)
|
||
{
|
||
m_activeColumnCompleteMask = completeMask;
|
||
}
|
||
else if (m_activeColumnCompleteMask != completeMask)
|
||
{
|
||
m_activeFramePacketLoss = true;
|
||
return;
|
||
}
|
||
|
||
uint8_t& mask = m_activeColumnMask[column];
|
||
const bool wasComplete = ((mask & completeMask) == completeMask);
|
||
mask = static_cast<uint8_t>(mask | segmentMask);
|
||
const bool isComplete = ((mask & completeMask) == completeMask);
|
||
if (!wasComplete && isComplete)
|
||
++m_activeCompleteColumns;
|
||
}
|
||
|
||
void CRsLidarDevice::fillRsem4Point(uint32_t column, uint32_t ring, float distance,
|
||
uint8_t intensity, uint8_t surfaceIndex,
|
||
float cosYaw, float sinYaw)
|
||
{
|
||
if (!m_activeFrame || column >= kRsem4ReserveColumns || ring >= kRsem4DefaultRings)
|
||
return;
|
||
|
||
const size_t pointIndex =
|
||
static_cast<size_t>(column) * static_cast<size_t>(kRsem4DefaultRings) +
|
||
static_cast<size_t>(kRsem4DefaultRings - 1 - ring);
|
||
if (pointIndex >= m_activeFrame->pointPool.size())
|
||
return;
|
||
|
||
auto& dst = m_activeFrame->pointPool[pointIndex];
|
||
|
||
if (surfaceIndex >= kRsem4SurfaceCount || distance < m_rsem4MinDistance || distance > m_rsem4MaxDistance)
|
||
{
|
||
SetZeroPoint(dst);
|
||
m_activeFrameAllValid = false;
|
||
return;
|
||
}
|
||
|
||
const float sinPitch = m_rsem4PitchSin[surfaceIndex][ring];
|
||
const float cosPitch = m_rsem4PitchCos[surfaceIndex][ring];
|
||
|
||
const float x = distance * cosPitch * cosYaw;
|
||
const float y = distance * cosPitch * sinYaw;
|
||
const float z = distance * sinPitch;
|
||
|
||
dst.fData[0] = -y * 1000.0f;
|
||
dst.fData[1] = -z * 1000.0f;
|
||
dst.fData[2] = x * 1000.0f;
|
||
dst.fData_c[0] = static_cast<float>(intensity);
|
||
}
|
||
|
||
void CRsLidarDevice::processRsem4Difop(const uint8_t* data, size_t size)
|
||
{
|
||
if (!data)
|
||
return;
|
||
|
||
auto applyAngles = [this](const int8_t* yaw, const int16_t* pitch, const int16_t* surface) {
|
||
for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
|
||
m_rsem4YawOffset[i] = yaw[i];
|
||
for (size_t i = 0; i < kRsem4DefaultRings; ++i)
|
||
m_rsem4PitchAngle[i] = SwapI16(pitch[i]);
|
||
for (size_t i = 0; i < kRsem4SurfaceCount; ++i)
|
||
m_rsem4SurfacePitchOffset[i] = SwapI16(surface[i]);
|
||
m_rsem4AnglesReady = true;
|
||
updateRsem4PitchTrig();
|
||
};
|
||
|
||
if (size == sizeof(Rsem4DifopPkt))
|
||
{
|
||
const auto* pkt = reinterpret_cast<const Rsem4DifopPkt*>(data);
|
||
applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
|
||
}
|
||
else if (size == sizeof(Rsem4Difop2Pkt))
|
||
{
|
||
const auto* pkt = reinterpret_cast<const Rsem4Difop2Pkt*>(data);
|
||
applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
|
||
}
|
||
else if (size == sizeof(Rsem4Difop0624Pkt))
|
||
{
|
||
const auto* pkt = reinterpret_cast<const Rsem4Difop0624Pkt*>(data);
|
||
applyAngles(pkt->yawOffset, pkt->pitchAngle, pkt->surfacePitchOffset);
|
||
}
|
||
}
|
||
|
||
bool CRsLidarDevice::processRsem4Packet(const Packet& pkt)
|
||
{
|
||
const uint8_t* data = pkt.buf_.empty() ? nullptr : pkt.buf_.data();
|
||
const size_t size = pkt.buf_.size();
|
||
if (!data || size < 4)
|
||
return false;
|
||
|
||
if (pkt.is_difop)
|
||
{
|
||
processRsem4Difop(data, size);
|
||
return false;
|
||
}
|
||
|
||
return processRsem4Msop(data, size);
|
||
}
|
||
|
||
bool CRsLidarDevice::processRsem4Msop(const uint8_t* data, size_t size)
|
||
{
|
||
if (IsCompleteRsem4Packet(data, size))
|
||
return processRsem4CompleteMsop(data, size);
|
||
|
||
if (size < sizeof(Rsem4MsopHeader2))
|
||
return false;
|
||
|
||
const auto& header2 = *reinterpret_cast<const Rsem4MsopHeader2*>(data);
|
||
const uint16_t seq = ReadU16BE(header2.pktSeq);
|
||
if (!m_rsem4HasPartialPacket || seq != m_rsem4PartialSeq)
|
||
{
|
||
m_rsem4HasPartialPacket = false;
|
||
m_rsem4PartialLen = 0;
|
||
if (m_activeFrame)
|
||
m_activeFramePacketLoss = true;
|
||
return false;
|
||
}
|
||
|
||
const size_t payloadOffset = sizeof(Rsem4MsopHeader2);
|
||
const size_t payloadLen = size - payloadOffset;
|
||
if (m_rsem4PartialLen + payloadLen > m_rsem4PartialPacket.size())
|
||
{
|
||
m_rsem4HasPartialPacket = false;
|
||
m_rsem4PartialLen = 0;
|
||
if (m_activeFrame)
|
||
m_activeFramePacketLoss = true;
|
||
return false;
|
||
}
|
||
|
||
std::memcpy(m_rsem4PartialPacket.data() + m_rsem4PartialLen, data + payloadOffset, payloadLen);
|
||
m_rsem4PartialLen += payloadLen;
|
||
m_rsem4HasPartialPacket = false;
|
||
return processRsem4CompressedMsop(m_rsem4PartialPacket.data(), m_rsem4PartialLen);
|
||
}
|
||
|
||
bool CRsLidarDevice::processRsem4CompleteMsop(const uint8_t* data, size_t size)
|
||
{
|
||
if (size < sizeof(Rsem4MsopHeader))
|
||
return false;
|
||
|
||
const auto& header = *reinterpret_cast<const Rsem4MsopHeader*>(data);
|
||
const uint8_t packMode = header.packMode & 0x03;
|
||
if (packMode == 0x03)
|
||
{
|
||
const uint16_t compressedSeq = ReadU16BE(header.pktSeq);
|
||
|
||
const uint8_t splitPackNum = header.packMode >> 4;
|
||
if (splitPackNum == 0)
|
||
return processRsem4CompressedMsop(data, size);
|
||
|
||
if (size > m_rsem4PartialPacket.size())
|
||
{
|
||
if (m_activeFrame)
|
||
m_activeFramePacketLoss = true;
|
||
return false;
|
||
}
|
||
std::memcpy(m_rsem4PartialPacket.data(), data, size);
|
||
m_rsem4PartialLen = size;
|
||
m_rsem4PartialSeq = compressedSeq;
|
||
m_rsem4HasPartialPacket = true;
|
||
return false;
|
||
}
|
||
|
||
if (size < sizeof(Rsem4MsopPkt))
|
||
return false;
|
||
|
||
const auto& pkt = *reinterpret_cast<const Rsem4MsopPkt*>(data);
|
||
const uint16_t pktSeqRaw = ReadU16BE(pkt.header.pktSeq);
|
||
const uint16_t pktSeq = (pktSeqRaw > 0) ? static_cast<uint16_t>(pktSeqRaw - 1) : 0;
|
||
if (!m_rsem4FrameSynced)
|
||
{
|
||
m_rsem4FrameSynced = true;
|
||
m_hasRsem4PrevSeq = false;
|
||
m_hasRsem4PrevColumn = false;
|
||
}
|
||
|
||
m_prevRsem4Seq = pktSeq;
|
||
m_hasRsem4PrevSeq = true;
|
||
|
||
const uint16_t columnRaw = ReadU16BE(pkt.header.columnNum);
|
||
const uint32_t column = (columnRaw < kRsem4ReserveColumns)
|
||
? static_cast<uint32_t>(columnRaw)
|
||
: static_cast<uint32_t>((pktSeq / 2) % kRsem4ReserveColumns);
|
||
if (!prepareRsem4FrameForColumn(column))
|
||
return false;
|
||
m_activeFrameHasData = true;
|
||
|
||
const uint8_t surfaceIndex = (pkt.header.surfaceId > 0) ? static_cast<uint8_t>(pkt.header.surfaceId - 1) : 0;
|
||
const int16_t yawBase = SwapI16(pkt.header.yawAngle);
|
||
|
||
float yawCos[kRsem4VecselsPerColumn];
|
||
float yawSin[kRsem4VecselsPerColumn];
|
||
for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
|
||
{
|
||
const int yaw = static_cast<int>(yawBase) + static_cast<int>(m_rsem4YawOffset[i]);
|
||
yawCos[i] = CosDeg01(yaw);
|
||
yawSin[i] = SinDeg01(yaw);
|
||
}
|
||
|
||
const bool dualReturn = (pkt.header.returnMode == 0x00);
|
||
const uint8_t segmentModulo = dualReturn ? 4 : 2;
|
||
const uint8_t segmentMask = static_cast<uint8_t>(1U << (pktSeq % segmentModulo));
|
||
const uint8_t completeMask = dualReturn ? 0x0F : 0x03;
|
||
for (uint32_t blk = 0; blk < kRsem4BlocksPerPacket; ++blk)
|
||
{
|
||
uint32_t ring = 0;
|
||
if (dualReturn)
|
||
{
|
||
if ((blk & 1U) != 0)
|
||
continue;
|
||
ring = (blk / 2) + (pktSeq % 4) * (kRsem4HalfRings / 2);
|
||
}
|
||
else
|
||
{
|
||
ring = blk + (pktSeq % 2) * kRsem4HalfRings;
|
||
}
|
||
if (ring >= kRsem4DefaultRings)
|
||
continue;
|
||
|
||
const auto& channel = pkt.blocks[blk].channel[0];
|
||
const float distance = static_cast<float>(ReadU16BE(channel.distance)) * kRsem4DistanceResolution;
|
||
const size_t vecsel = ring / kRsem4PixelsPerVcsel;
|
||
fillRsem4Point(column, ring, distance, channel.intensity, surfaceIndex, yawCos[vecsel], yawSin[vecsel]);
|
||
}
|
||
markRsem4ColumnSegment(column, segmentMask, completeMask);
|
||
|
||
return true;
|
||
}
|
||
|
||
bool CRsLidarDevice::processRsem4CompressedMsop(const uint8_t* data, size_t size)
|
||
{
|
||
if (size < sizeof(Rsem4MsopHeader) + kRsem4MsopTailLen)
|
||
return false;
|
||
|
||
const auto& header = *reinterpret_cast<const Rsem4MsopHeader*>(data);
|
||
const uint16_t pktSeq = ReadU16BE(header.pktSeq);
|
||
if (!m_rsem4FrameSynced)
|
||
{
|
||
m_rsem4FrameSynced = true;
|
||
m_hasRsem4PrevSeq = false;
|
||
m_hasRsem4PrevColumn = false;
|
||
}
|
||
|
||
m_prevRsem4Seq = pktSeq;
|
||
m_hasRsem4PrevSeq = true;
|
||
|
||
uint16_t decoded[kRsem4DefaultRings * 2] = {};
|
||
const int dataLen = static_cast<int>((size - sizeof(Rsem4MsopHeader) - kRsem4MsopTailLen) / 2);
|
||
CompressAlgo::RLenc_unpack_optimize(
|
||
reinterpret_cast<const uint16_t*>(data + sizeof(Rsem4MsopHeader)),
|
||
decoded,
|
||
dataLen,
|
||
16);
|
||
|
||
const uint16_t* radius = decoded;
|
||
const uint16_t* identity = decoded + kRsem4DefaultRings;
|
||
|
||
const uint16_t columnRaw = ReadU16BE(header.columnNum);
|
||
const uint32_t column = (columnRaw < kRsem4ReserveColumns)
|
||
? static_cast<uint32_t>(columnRaw)
|
||
: static_cast<uint32_t>(pktSeq % kRsem4ReserveColumns);
|
||
if (!prepareRsem4FrameForColumn(column))
|
||
return false;
|
||
m_activeFrameHasData = true;
|
||
|
||
const uint8_t surfaceIndex = (header.surfaceId > 0) ? static_cast<uint8_t>(header.surfaceId - 1) : 0;
|
||
const int16_t yawBase = SwapI16(header.yawAngle);
|
||
|
||
float yawCos[kRsem4VecselsPerColumn];
|
||
float yawSin[kRsem4VecselsPerColumn];
|
||
for (size_t i = 0; i < kRsem4VecselsPerColumn; ++i)
|
||
{
|
||
const int yaw = static_cast<int>(yawBase) + static_cast<int>(m_rsem4YawOffset[i]);
|
||
yawCos[i] = CosDeg01(yaw);
|
||
yawSin[i] = SinDeg01(yaw);
|
||
}
|
||
|
||
for (uint32_t ring = 0; ring < kRsem4DefaultRings; ++ring)
|
||
{
|
||
const float distance = static_cast<float>(radius[ring]) * kRsem4DistanceResolution;
|
||
const uint8_t intensity = static_cast<uint8_t>(identity[ring] & 0xFF);
|
||
const size_t vecsel = ring / kRsem4PixelsPerVcsel;
|
||
fillRsem4Point(column, ring, distance, intensity, surfaceIndex, yawCos[vecsel], yawSin[vecsel]);
|
||
}
|
||
markRsem4ColumnSegment(column, 0x01, 0x01);
|
||
|
||
return true;
|
||
}
|
||
|
||
void CRsLidarDevice::onPacket(const Packet& pkt)
|
||
{
|
||
if ((++m_diagPacketTick & 0x3FFU) == 0)
|
||
emitDiagnosticLogIfDue(m_diagLastLogTime);
|
||
if (m_param.lidar_type == LidarType::RSEM4)
|
||
processRsem4Packet(pkt);
|
||
|
||
if (!m_hasPacketCallback.load(std::memory_order_acquire))
|
||
return;
|
||
|
||
PacketCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_packetCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
RsPacketInfo info;
|
||
info.timestamp = pkt.timestamp;
|
||
info.seq = pkt.seq;
|
||
info.is_difop = pkt.is_difop;
|
||
info.is_frame_begin = pkt.is_frame_begin;
|
||
info.frameId = pkt.frame_id;
|
||
info.dataSize = static_cast<uint32_t>(pkt.buf_.size());
|
||
cb(info);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 监控接口
|
||
// ============================================================
|
||
uint64_t CRsLidarDevice::GetDroppedFrameCount() const
|
||
{
|
||
return m_droppedFrameCount.load();
|
||
}
|
||
|
||
size_t CRsLidarDevice::GetStuffedQueueDepth() const
|
||
{
|
||
return m_deliveryQueue.size();
|
||
}
|
||
|
||
uint64_t CRsLidarDevice::GetExceptionCount(int errCode) const
|
||
{
|
||
size_t bucket = errCodeToBucket(errCode);
|
||
return m_exceptionCounts[bucket].load(std::memory_order_relaxed);
|
||
}
|
||
|
||
RsCallbackStats CRsLidarDevice::GetCallbackLatencyStats() const
|
||
{
|
||
RsCallbackStats stats;
|
||
stats.count = m_cbCount.load(std::memory_order_relaxed);
|
||
stats.maxUs = m_cbMaxUs.load(std::memory_order_relaxed);
|
||
uint64_t acc = m_cbAccumUs.load(std::memory_order_relaxed);
|
||
stats.avgUs = (stats.count > 0) ? (acc / stats.count) : 0;
|
||
return stats;
|
||
}
|
||
|
||
size_t CRsLidarDevice::GetStuffedQueuePeak() const
|
||
{
|
||
return m_deliveryQueuePeak.load(std::memory_order_relaxed);
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：Error → RsExceptionInfo（按 ErrCode 分桶计数后转发）
|
||
// ============================================================
|
||
void CRsLidarDevice::onException(const Error& code)
|
||
{
|
||
// 分桶累加（用于诊断日志和 GetExceptionCount 查询）
|
||
size_t bucket = errCodeToBucket(static_cast<int>(code.error_code));
|
||
m_exceptionCounts[bucket].fetch_add(1, std::memory_order_relaxed);
|
||
|
||
ExceptionCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_callbackMutex);
|
||
cb = m_exceptionCallback;
|
||
}
|
||
|
||
if (cb)
|
||
{
|
||
RsExceptionInfo info;
|
||
info.code = static_cast<int>(code.error_code);
|
||
info.message = code.toString();
|
||
cb(info);
|
||
}
|
||
}
|