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 将解析后的结果保存在两个结构 DeviceInfoDeviceStatus 中,它们定义在 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)