21 如何解析 DIFOP 中的配置和状态数据
21.1 概述
ag_driver 的首要目标是解析 MSOP/DIFOP 包,得到点云。其中 MSOP 包含点的坐标数据,DIFOP 包只包含构造点云的一些参数。
对于 MEMS 雷达,MSOP 包就够了,DIFOP 包其实是不需要的。
除构造点云需要的数据之外,DIFOP 还包含了一些雷达的配置和状态数据,ag_driver 似乎是解析这些数据的合适的地方。
遗憾的是,不同类型的雷达,DIFOP 格式一般不同,甚至同一类型的雷达,不同项目也可能定制自己的格式,这样 ag_driver 就很难给出一个通用而又完善的实现。所以ag_driver 只能给出一个折中的方案:它针对 DIFOP 包中若干通用的字段进行解析。使用者可以根据这些范例来解析其他需要的字段。
本文描述如何一步步修改 ag_driver,支持解析新的字段。
21.2 拓展 DIFOP 包的定义
DIFOP 包的定义在雷达解码器的头文件中。
如果客户定制了自己的 DIFOP 包格式,则需要拓展原有的定义、甚至将这个定义替换成自己的。
对于 A0 雷达,这个定义在 decoder_A0.hpp 中。
// decoder_A0.hpp
typedef struct
{
A0DifopHead difop_head;
A0DifopFuncSafety func_safety;
A0DifopNetSafety net_safety;
} A0DifopPkt;
21.3 扩充 DeviceInfo 和 DeviceStatus
ag_driver 将解析后的结果保存在两个结构 DeviceInfo 和 DeviceStatus 中,它们定义在 driver_param.hpp 中。
DIFOP 包中包含配置和状态数据。
DeviceInfo这个结构保存配置数据,这部分一般是不变的。DeviceStatus是状态数据,这部分是变化的。
// driver_param.hpp
struct DeviceInfo
{
uint8_t sn[6];
uint8_t mac[6];
uint8_t version[6];
};
struct DeviceStatus
{
float voltage = 0.0f;
};
Decoder<T_PointCloud> 是所有雷达解码器的基类。它的成员 device_info_ 和 device_status_ 分别保存了配置和状态数据。
template <typename T_PointCloud>
class Decoder
{
public:
......
DeviceInfo device_info_;
DeviceStatus device_status_;
......
};
21.4 解析 DIFOP 包
雷达解码器负责解码 DIFOP 包,并将结果保存到 Decoder<T_PointCloud> 的成员 device_info_ 和 device_status_ 中。
对于 A0,DecoderA0<T_PointCloud> 的成员函数 decodeDifopPkt() 负责解析 DIFOP 包。
这个函数是一个虚拟函数,在基类 Decoder<T_PointCloud> 中定义。
//decoder_A0.hpp
template <typename T_PointCloud>
inline void DecoderA0<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, size_t size)
{
const A0DifopPkt& pkt = *(A0DifopPkt*)packet;
this->echo_mode_ = this->getEchoMode(pkt.difop_head.return_mode);
#ifdef ENABLE_DIFOP_PARSE
// device info
memcpy (this->device_info_.sn, pkt.difop_head.sn, 6);
memcpy (this->device_info_.mac, pkt.difop_head.eth.mac_addr, 6);
memcpy (this->device_info_.top_ver, pkt.difop_head.version.firmware_ver, 6);
// device status
this->device_status_.voltage = ntohs(pkt.func_safety.lidar_status);
#endif
}
21.5 读取配置和状态数据
使用者的代码创建 LidarDriver<T_PointCloud> 的实例,所以可以调用它的成员函数 getDeviceInfo() 和 getDeviceStatus() 得到配置和状态数据。
如前所述,Decoder<T_PointCloud> 的成员 device_info_ 和 device_status_ 保存这两类数据。
这里需要注意的是,使用者的代码需要检查这两个成员的返回值,来确定数据是不是有效的。也就是 DIFOP 是不是解析好了。
template <typename T_PointCloud>
class LidarDriver
{
public:
......
inline bool getDeviceInfo(DeviceInfo& info)
{
return driver_ptr_->getDeviceInfo(info);
}
inline bool getDeviceStatus(DeviceStatus& status)
{
return driver_ptr_->getDeviceStatus(status);
}
......
};
21.6 使能 ENABLE_DIFOP_PARSE
一般的使用者只需要解析点云,所以默认情况下希望免去这些 CPU 资源的消耗。
CMake 编译选项 ENABLE_DIFOP_PARSE 用于指定是否解析 DIFOP 包。如果希望解析,则需要启用这个选项。这个选项定义在 CMakeLists.txt 下。
CMakeLists.txt
option(ENABLE_DIFOP_PARSE "Enable DIFOP Packet Parse" OFF)