104 lines
3.0 KiB
Markdown
104 lines
3.0 KiB
Markdown
# RSEMX 平台通信协议pitch补偿变更代码说明
|
||
|
||
**文件路径**: [src/rs_driver/driver/decoder/decoder_RSEMX.hpp](../src/rs_driver/driver/decoder/decoder_RSEMX.hpp)
|
||
|
||
以下为 RSEMX 平台通信协议pitch补偿代码变更细节，主要涉及协议头定义的更新以及 `pitch_offset`的计算。
|
||
|
||
---
|
||
|
||
## 变更点 1：协议头结构体更新
|
||
|
||
**位置**: `RSEMXMsopHeader` 结构体定义
|
||
|
||
**变更描述**:
|
||
将原有的 `version_protocol` 字段替换为 `pitch_offset`。
|
||
|
||
**代码**:
|
||
|
||
```cpp
|
||
typedef struct
|
||
{
|
||
uint8_t id[4];
|
||
uint16_t pkt_seq;
|
||
uint16_t pitch_offset; // [修改] 原为 uint16_t version_protocol，现改为 pitch_offset
|
||
uint8_t return_mode;
|
||
uint8_t time_mode;
|
||
RSTimestampUTC timestamp;
|
||
uint8_t fram_sync;
|
||
uint8_t frame_rate;
|
||
uint16_t column_num;
|
||
int16_t yaw_angle;
|
||
uint8_t pack_mode;
|
||
uint8_t surface_id;
|
||
uint16_t temperature;
|
||
uint8_t lidar_type;
|
||
uint8_t reserved;
|
||
} RSEMXMsopHeader; // 32-bytes
|
||
```
|
||
|
||
## 变更点 2：解码逻辑更新
|
||
|
||
**位置** : `DecoderRSEMX<T_PointCloud>::decodeMsopPkt` 函数
|
||
|
||
**变更描述** :
|
||
|
||
1. **定义 `pitch_offset`系数**。
|
||
2. **计算偏移量** : 对 header 中的 `pitch_offset` 进行大小端转换、类型转换及系数乘法。
|
||
3. **应用offset** : 在计算最终 `pitch` 值时减去偏移量（注意系数对齐）。
|
||
|
||
**代码** :
|
||
|
||
```C++
|
||
template <typename T_PointCloud>
|
||
inline bool DecoderRSEMX<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size_t size)
|
||
{
|
||
const RSEMXMsopPkt& pkt = *(RSEMXMsopPkt*)packet;
|
||
bool ret = false;
|
||
|
||
constexpr float SCALE_FACTOR = 503.975f / 65536.0f;
|
||
constexpr float KELVIN_OFFSET = 273.15f;
|
||
// [新增] 定义 pitch_offset 系数
|
||
constexpr float PITCH_OFFSET_SCALE = 1.0f / 32768.0f;
|
||
/*
|
||
...
|
||
时间戳、包序号、分帧等正常进行解析，略
|
||
...
|
||
*/
|
||
const int yaw_base = static_cast<int>(RS_SWAP_INT16(pkt.header.yaw_angle));
|
||
|
||
// [新增] 计算 pitch_offset
|
||
// 逻辑：先大小端转换(ntohs) -> 转为int16_t -> 乘系数
|
||
const float pitch_offset = static_cast<int16_t>(ntohs(pkt.header.pitch_offset)) * PITCH_OFFSET_SCALE;
|
||
uint16_t seq_mod = (is_dual_mode && (pkt_seq % 2 != 0)) ? EMX_PIXELS_PER_COLUMN : 0;
|
||
for (uint16_t blk = 0; blk < blocks_per_pkt; ++blk)
|
||
{
|
||
/*
|
||
...
|
||
通道正常计算，略
|
||
...
|
||
*/
|
||
int yaw = yaw_base ;
|
||
yaw = static_cast<int>(yaw * yaw_factor);
|
||
/*
|
||
[修改] 计算最终 pitch
|
||
说明：用原始 pitch 减去 pitch_offset。
|
||
注意：此处乘 100 是因为 pitch_angle_ 存储的是 0.01 度为单位的数值（用于查表法）。
|
||
如果用户代码未使用查表法且 pitch 为 float 直出，则不需要乘 100。
|
||
*/
|
||
const int pitch = this->pitch_angle_[real_chan] - pitch_offset * 100;
|
||
|
||
const float cos_pitch = COS(pitch);
|
||
const float sin_pitch = SIN(pitch);
|
||
const float cos_yaw = COS(yaw);
|
||
const float sin_yaw = SIN(yaw);
|
||
/*
|
||
...
|
||
点云坐标正常进行解析，略
|
||
*/
|
||
}
|
||
this->prev_point_ts_ = pkt_ts;
|
||
this->prev_pkt_ts_ = pkt_ts;
|
||
return ret;
|
||
}
|
||
```
|