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;
|
|||
|
|
}
|
|||
|
|
```
|