参考:ARS_408毫米波雷达数据解析学习记录
感谢博主的分享(https://blog.csdn.net/weixin_49401384)
雷达can消息解析(通用can解析思路)
- socketcan学习
- can总线
- 毫米波雷达can协议解读
- RadarCfg
- RadarState
代码基本是关于c++类和类的实现。在域控制器上实际使用毫米波雷达真的tkl,但是也比一般的要难不少,可以说是进阶版的配置了。域控是高级版(其实也高不了多少)的orin,架构为ARM64,这也是比较烦的地方。搭载了计算用的英伟达的orin和英飞凌的TC297。有两个can接口(可以通过串口线并联多个设备)。还有RS232。主要就是对这两个模块进行配置了。
第一次配置,要面对许多未知的恐惧,这个过程还是挺煎熬的,更重要的是我还有其他项目,导致一时间我这的不知道如何是好。这里就要感谢域控和感知组的师兄师姐,本身确实不是多大的问题,但我初次接触还是感觉特别吃力。
实际配置的过程和遇到的问题:
首先删除了anaconda。这东西可以有,但是最好只是用他的虚拟环境,不用在bash环境下使用它的python3库,这样比较好编译(很大一部分编译问题都是因为路径不对)
(当然,后来用的解决方案是通过默认不进入conda环境来进行编译)
进入su,
rm -rf /home/nvidia/anaconda3
然后去bashrc里面删除conda的initial
其次用毫米波雷达连接域控时需要一根转接线,刚开始的时候借了一根来用,但是一直没出现can信号,(我都要崩溃了,第一次使用,我还以为自己操作不对)
后来进过师兄一顿测试,原来是因为接线是23交叉的,而不是DB直连线
又买了一根直连线,终于能收到信号了,也成功通过socketcan对其进行了解析。
这基本上是最简单不过的内容了,但是要真正学会can和毫米波雷达,这还远远不够,所以接下来从原理上进行分解,争取用1天左右的时间解决毫米波雷达的各种原理学习。
socketcan学习
毫米波雷达通过can通信传输数据,can总线的学习是第一步,接下来主要是说明如何利用socketcan接口进行can总线数据的收发。
can总线
什么是can,我在之前几乎没有听说过can消息和CAN通讯。
- CAN是控制器局域网络(controller area network)的简称,由德国博世开发,并最终成了国际标准。
- CAN是一种串行通信协议,一根总线下可以有好多的信号,但最终执行机构只选定其中的一种起作用。
- CAN 总线规范从最初的CAN 1.0 规范(标准格式)发展CAN 2.0 规范,目前应用的CAN器件大多符合CAN2.0规范。(这个在传感器里面也是可以选的)
- 当CAN总线上的节点发送数据时,以报文形式广播给网络中的所有节点,总线上的所有节点都不使用节点地址等系统配置信息,只根据每组报文开头的11位标识符(CAN2.0A规范)解释数据的含义来决定是否接收。
ifconfig -a
eth0设备是以太网接口,can0和can1设备是两个CAN总线接口。
要成功接收can消息,必须先配置好波特率:
ip link set can0 up type can bitrate 500000
注意:真正在域控上使用时改波特率得先关闭can设备:
sudo ifconfig can0 down
或者是(更推荐):sudo ip link set can0 down
sudo ip link set can0 up type can bitrate 500000
然后再打开设备:
sudo ifconfig can0 up
这样基本就能通过 candump can0看到数据了
然后是一些其他常用的命令:
ip -details link show can0
ip link set can0 down //关闭can0网络
ip link set can0 up //打开can0网络
candump can0 //接收can0数据
canconfig can0 bitrate + 波特率
canconfig can0 start //启动can0设备
canconfig can0 ctrlmode loopback on //回环测试
canconfig can0 restart //重启can0设备
canconfig can0 stop //停止can0设备
canecho can0 //查看can0设备总线状态
cansend can0 --identifer=ID+数据 //发送数据;
candump can0 --filter=ID:mask //使用滤波器接受ID匹配的数据
接下来就到了最难的socketcan解析了:
自定义can接口用到的头文件:
#include <cstdint>
#include <errno.h> //错误号头文件,包含系统中各种出错号。
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h> //Linux 标准头文件,定义了各种符号常数和类型
#include <string.h>
#include <net/if.h> //用来配置和获取ip地址,掩码,MTU等接口信息的
#include <sys/types.h> //类型头文件,定义了基本的系统数据类型。
#include <sys/socket.h> //定义socket的头文件
#include <sys/ioctl.h> //包含设备I/O通道管理的函数
#include <linux/can.h> //包含了SocketCAN中大部分的数据结构和函数
#include <linux/can/raw.h>
声明命名空间和类的属性和成员:
namespace socket_can{
class SocketCAN{
public:
//公共成员函数:可以从类外部访问和调用。其他类的对象和函数可以使用这些公共成员函数来与类进行交互。
//私有成员函数:只能在类内部访问和调用。只有类的其他成员函数可以使用私有成员函数,外部无法直接调用。
SocketCAN(const char * ifname);
SocketCAN(const char * ifname, long timeout);
~SocketCAN();
bool is_connected(); //用于确定是否建立连接
bool write(uint32_t frame_id, uint8_t dlc, uint8_t * data); //写入数据
bool read(uint32_t * can_id, uint8_t * dlc, uint8_t * data); //获取数据
private:
void init();
const char * ifname_; //用于指定网络设备的名称
int socket_; //用于接收创建的socket返回的描述符
bool connected_;
long timeout_;
};
}
类的实现:
#include <socket_can/socket_can.hpp>
namespace socket_can{
SocketCAN::SocketCAN(const char * ifname) :
ifname_(ifname), //指定网络设备的名称
connected_(false), //创建socket默认设置为未连接状态
timeout_(1000l) //设置默认的套接字超时时间
{
init(); //默认调用SocketCAN类的初始化函数
}
SocketCAN::SocketCAN(const char * ifname, long timeout) :
ifname_(ifname),
connected_(false),
timeout_(timeout) //从外部获取套接字超时时间
{
init();
}
SocketCAN::~SocketCAN(){
if (close(socket_) < 0) {
perror("Closing: ");
printf("Error: %d", errno); //如果未成功关闭socket描述符对应的操作空间,则输出错误信息
}
}
void SocketCAN::init(){
if ((socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { //其中:PF_CAN为所用的协议族;SOCK_RAW为所用的套接字类,这里采用的是原始套接字;CAN_RAW是指原始CAN协议
perror("Error while opening socket"); //创建socket套接字,并对返回的描述符进行判断,失败(<0)则输出错误信息并返回
return;
}
struct ifreq ifr{}; //定义ifeq结构体,用于配置和获取接口信息
strcpy(ifr.ifr_name, ifname_); //将从外部获取网络设备名称拷贝给ifr.ifr_name
ioctl(socket_, SIOCGIFINDEX, &ifr); //获取网络设备接口地址
struct sockaddr_can addr{}; //通用地址结构
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex; //设置CAN协议
printf("%s at index %d\n", ifname_, ifr.ifr_ifindex);
if (bind(socket_, (struct sockaddr *) &addr, sizeof(addr)) < 0) { //将刚生成的套接字与网络地址进行绑定,并对bind()的返回值进行判断,失败则输出错误信息并返回
perror("Error in socket bind");
return;
}
int error = 0; //用于保存错误代码
socklen_t len = sizeof (error);
int retval = getsockopt (socket_, SOL_SOCKET, SO_ERROR, &error, &len); //获取socket_的状态
if (retval != 0) {
/* there was a problem getting the error code */
printf("Error getting socket error code: %s\n", strerror(retval));
return;
}
if (error != 0) {
/* socket has a non zero error status */
printf("Socket error: %s\n", strerror(error)); //将error中保存的错误代码输出
return;
}
struct timeval timeout{}; //设置超时时间
timeout.tv_sec = (timeout_ / 1000);
timeout.tv_usec = (timeout_ % 1000) * 1000;
if (setsockopt(socket_, SOL_SOCKET, SO_RCVTIMEO, (char *) & timeout, sizeof(timeout)) < 0) { //SO_RCVTIMEO参数表示设置socket接收超时时间
perror("Setting timeout failed");
}
connected_ = true;
}
bool SocketCAN::is_connected(){ //获取套接字连接状态
return connected_;
}
bool SocketCAN::write(uint32_t can_id, uint8_t dlc, uint8_t * data){ //数据写入函数
struct can_frame frame{};
frame.can_id = can_id;
frame.can_dlc = dlc;
memcpy(frame.data, data, dlc * sizeof(uint8_t)); //将要写入的数据保存到can_frame结构体中
auto num_bytes = ::write(socket_, &frame, sizeof(struct can_frame)); //获取写入的字节数
return num_bytes > 0;
}
bool SocketCAN::read(uint32_t * can_id, uint8_t * dlc, uint8_t * data){
struct can_frame frame{};
auto num_bytes = ::read(socket_, &frame, sizeof(struct can_frame)); //获取读到的字节数
if (num_bytes != sizeof(struct can_frame)) {
return false; 如果返回的bytes不等于帧长度,则读取失败,并返回 false
}
(* can_id) = frame.can_id;
(* dlc) = frame.can_dlc;
memcpy(data, frame.data, sizeof(frame.data)); //将读取的数据保存到传出参数中
return true;
}
}
分析这段类的实现。前面的部分主要是两个构造函数和一个析构函数,
构造函数:
-
SocketCAN(const char * ifname)是一个构造函数,用于根据指定的网络设备名称创建一个SocketCAN对象。
-
初始化了类的成员变量ifname_为传入的ifname参数。
-
将connected_初始化为false,表示还未建立连接。
-
将timeout_初始化为默认值1000l,表示超时时间为1秒。
-
调用私有成员函数init()进行初始化操作。
-
SocketCAN(const char * ifname, long timeout)是另一个构造函数,功能与上述构造函数类似,但可以传入额外的超时时间参数timeout。
-
初始化了类的成员变量ifname_为传入的ifname参数。
-
将connected_初始化为false。
-
将timeout_初始化为传入的timeout参数。
-
调用私有成员函数init()进行初始化操作。
析构函数:
~SocketCAN()是析构函数,用于进行对象销毁前的清理工作。 -
使用close()函数关闭socket_描述符对应的操作空间。
-
如果关闭操作失败,则输出错误信息。
这里不懂什么是构造和析构的:
构造函数和析构函数是面向对象编程中的特殊成员函数,用于对象的创建和销毁。
1. 构造函数 (Constructor):
- 构造函数是在创建对象时自动调用的特殊成员函数。
- 构造函数的名称与类的名称相同,没有返回类型(包括void)。
- 构造函数可以有参数,用于初始化对象的成员变量。
- 构造函数在对象创建时被调用,常用于执行对象的初始化工作。
- 如果没有显式定义构造函数,编译器会自动生成一个默认构造函数。
- 可以定义多个构造函数,以便根据参数的不同选择合适的构造函数进行对象的初始化。
2. 析构函数 (Destructor):
- 析构函数是在对象被销毁时自动调用的特殊成员函数。
- 析构函数的名称与类的名称相同,前面加上一个波浪号(~)作为前缀,没有参数和返回类型(包括void)。
- 析构函数在对象被销毁时被调用,常用于进行对象的清理工作,如释放资源、关闭文件等。
- 如果没有显式定义析构函数,编译器会自动生成一个默认析构函数。
- 通常情况下,析构函数不需要手动调用,而是由编译器自动调用。
- 一个类只能有一个析构函数,不能重载。
构造函数和析构函数都是类的特殊成员函数,它们的命名和使用方式是固定的,没有返回值,且构造函数在对象创建时调用,析构函数在对象销毁时调用。构造函数用于对象的初始化,析构函数用于对象的清理。通过构造函数和析构函数,可以实现对对象的自动初始化和清理,提高代码的可靠性和可维护性。
我们继续看。
后半部分的主函数,这段代码是对SocketCAN类中的私有成员函数init()以及公共成员函数is_connected()、write()、read()的具体实现。
第一个是void SocketCAN::init()
init()函数:
- 使用socket()函数创建一个原始套接字,其中使用PF_CAN作为协议族,SOCK_RAW作为套接字类,表示使用原始CAN协议。
- 如果创建套接字失败,则输出错误信息并返回。
- 使用ioctl()函数获取网络设备接口地址。
- 创建并初始化通用地址结构addr,将协议族设置为AF_CAN,接口索引设置为获取到的接口地址。
- 使用bind()函数将套接字与网络地址绑定,如果绑定失败,则输出错误信息并返回。
- 使用getsockopt()函数获取套接字的状态,检查错误码。
- 设置套接字的接收超时时间。
- 将connected_标记为true,表示已建立连接。
- is_connected()函数:
返回connected_,表示套接字的连接状态。
- write()函数:
创建一个can_frame结构体对象frame,设置帧ID和数据长度。
使用memcpy()函数将数据拷贝到frame.data中。
使用::write()函数将frame写入套接字,获取写入的字节数。
如果写入字节数大于0,返回true;否则返回false。
- read()函数:
创建一个can_frame结构体对象frame。
使用::read()函数从套接字中读取frame,获取读取的字节数。
如果读取的字节数不等于can_frame的长度,则读取失败,返回false。
将帧ID、数据长度和数据拷贝到传出参数中。
返回true表示读取成功。
总结:SocketCAN类的使用(只需知道原理)
//创建类对象及其初始化
socket_can::SocketCAN can_("can0");
//类的构造函数中已经完成了socket的创建以及与本地网络地址绑定的工作,若无异常则说明连接建立成功
/*数据获取
bool SocketCAN::read(uint32_t * can_id, uint8_t * dlc, uint8_t * data)
自定义数据读取函数的参数均为指针类型,因此在调用前需要创建对应类型的变量
为确保数据读取成功,还需要对read()函数的返回值进行判断
*/
uint32_t frame_id;
uint8_t dlc;
uint8_t data[8] = {0};
bool read_status = can_.read(&frame_id, &dlc, data);
if (!read_status) {
return false;
}
/*数据写入
bool SocketCAN::write(uint32_t can_id, uint8_t dlc, uint8_t * data)
*/
uint8_t raw_data[8]
can_.write(frame_id, 8, raw_data)
到这里,socketcan(套接字can)的内容就解析完了
毫米波雷达can协议解读
雷达的关键ID及数据
- socketcan学习
- can总线
- 毫米波雷达can协议解读
- RadarCfg
- RadarState
毫米波雷达的输入和输出部分的信息中,主要是1个输入信息(RadarCfg 0x200)和输出:RadarState 0x201和Cluster List(0x600、0x701、0x702) 和Object List(0x60A-0x60E)。
对应关系如下:
RadarCfg
id号0x200
定义union共用体变量用于存储数据
namespace radar_cfg {
typedef union radar_cfg {
struct { // ":"表示机构内位域的定义,即该变量占几个bit空间
//truct //创建自定义的数据结构,可以包含不同类型的数据成员,这些成员可以按需组织在一起。
//uint64_t 无符号整数类型,表示一个占据 64 位(8 字节)内存空间的无符号整数。
//位和字节,这是最基础的东西
uint64_t RadarCfg_MaxDistance_valid:1;
uint64_t RadarCfg_SensorID_valid:1;
uint64_t RadarCfg_RadarPower_valid:1;
uint64_t RadarCfg_OutputType_valid:1;
uint64_t RadarCfg_SendQuality_valid:1;
uint64_t RadarCfg_SendExtInfo_valid:1;
uint64_t RadarCfg_SortIndex_valid:1;
uint64_t RadarCfg_StoreInNVM_valid:1;
uint64_t RadarCfg_MaxDistance1:8;
uint64_t Reserved:6;
uint64_t RadarCfg_MaxDistance2:2;
uint64_t Reserved2:8;
uint64_t RadarCfg_SensorID:3;
uint64_t RadarCfg_OutputType:2;
uint64_t RadarCfg_RadarPower:3;
uint64_t RadarCfg_CtrlRelay_valid:1;
uint64_t RadarCfg_CtrlRelay:1;
uint64_t RadarCfg_SendQuality:1;
uint64_t RadarCfg_SendExtInfo:1;
uint64_t RadarCfg_SortIndex:3;
uint64_t RadarCfg_StoreInNVM:1;
uint64_t RadarCfg_RCS_Threshold_valid:1;
uint64_t RadarCfg_RCS_Threshold:3;
uint64_t Reserved3:4;
uint64_t Reserved4:8;
} data = {};
uint8_t raw_data[8];
} radar_cfg;
}
定义radar_cfg类
namespace radar_cfg {
class RadarCfg {
public:
RadarCfg();
~RadarCfg();
bool set_max_distance(uint64_t distance, bool valid = true); //最大距离
bool set_sensor_id(int id, bool valid = true); //传感器ID
bool set_radar_power(int power, bool valid = true); //雷达发射功率
bool set_output_type(int output_type, bool valid = true); //数据输出模式
void set_send_quality(bool quality, bool valid = true); //quality信息
void set_send_ext_info(bool send_ext, bool valid = true); //Extended信息
bool set_sort_index(int sort_index, bool valid = true); //为object list选择排序索引(clusters list默认按照距离进行排列)
void set_ctrl_relay_cfg(bool ctrl_relay, bool valid = true); //Relay control message
void set_store_in_nvm(bool store_in_nvm, bool valid = true); //将配置信息保存到non-volatile memory(NVM),以便传感器启动时读取和配置
bool set_rcs_threshold(int rcs_threshold, bool valid = true); //cluster检测的灵敏度设置,标准(0x0)或高灵敏度(0x1)
radar_cfg *get_radar_cfg();
private:
radar_cfg radar_cfg_msg;
};
}
radar_cfg类的实现:
namespace radar_cfg {
RadarCfg::RadarCfg() {
radar_cfg_msg.data.RadarCfg_MaxDistance_valid = 0;
radar_cfg_msg.data.RadarCfg_SensorID_valid = 0;
radar_cfg_msg.data.RadarCfg_RadarPower_valid = 0;
radar_cfg_msg.data.RadarCfg_OutputType_valid = 0;
radar_cfg_msg.data.RadarCfg_SendQuality_valid = 0;
radar_cfg_msg.data.RadarCfg_SendExtInfo_valid = 0;
radar_cfg_msg.data.RadarCfg_SortIndex_valid = 0;
radar_cfg_msg.data.RadarCfg_CtrlRelay_valid = 0;
radar_cfg_msg.data.RadarCfg_StoreInNVM_valid = 0;
radar_cfg_msg.data.RadarCfg_RCS_Threshold_valid = 0;
}
RadarCfg::~RadarCfg() {
}
bool RadarCfg::set_max_distance(uint64_t distance, bool valid) {
if (distance < 90 || distance > 1000) {
return false; //Extended Range ARS408:196m~1200m ARS404:90m~1000m
}
distance /= 2;
radar_cfg_msg.data.RadarCfg_MaxDistance1 = distance >> 2; // 二进制值按位右移2位,表示取数据的去掉2位后的高位部分
radar_cfg_msg.data.RadarCfg_MaxDistance2 = distance & 0b11; //取数据的最后2位地位部分
radar_cfg_msg.data.RadarCfg_MaxDistance_valid = static_cast<uint64_t>(valid);
return true;
}
bool RadarCfg::set_sensor_id(int id, bool valid) {
if (id < 0 || id > 7) {
return false;
}
radar_cfg_msg.data.RadarCfg_SensorID = static_cast<uint64_t>(id);
radar_cfg_msg.data.RadarCfg_SensorID_valid = static_cast<uint64_t>(valid);
return true;
}
bool RadarCfg::set_radar_power(int power, bool valid) {
if (power < 0 || power > 3) {
return false; //0x0: Standard 0x1: -3dB Tx gain 0x2: -6dB Tx gain 0x3: -9dB Tx gain
}
radar_cfg_msg.data.RadarCfg_RadarPower = static_cast<uint64_t>(power);
radar_cfg_msg.data.RadarCfg_RadarPower_valid = static_cast<uint64_t>(valid);
return true;
}
bool RadarCfg::set_output_type(int output_type, bool valid) {
if (output_type < 0 || output_type > 2) { //Clusters (0x2) or Objects (0x1) or “Stand By” (0x0)
return false;
}
radar_cfg_msg.data.RadarCfg_OutputType = static_cast<uint64_t>(output_type);
radar_cfg_msg.data.RadarCfg_OutputType_valid = static_cast<uint64_t>(valid);
return true;
}
void RadarCfg::set_send_quality(bool quality, bool valid) {
radar_cfg_msg.data.RadarCfg_SendQuality = static_cast<uint64_t>(quality);
radar_cfg_msg.data.RadarCfg_SendQuality_valid = static_cast<uint64_t>(valid); //Cluster or object quality information (message 0x60C or 0x702) is sent if true
}
void RadarCfg::set_send_ext_info(bool send_ext, bool valid) {
radar_cfg_msg.data.RadarCfg_SendExtInfo = static_cast<uint64_t>(send_ext);
radar_cfg_msg.data.RadarCfg_SendExtInfo_valid = static_cast<uint64_t>(valid); //Extended information (message 0x60D) is sent for objects if true (if clusters are selected as output type this value is ignored)
}
bool RadarCfg::set_sort_index(int sort_index, bool valid) {
if (sort_index < 0 || sort_index > 2) {
return false; //Selects the sorting index for the object list (ignored for clusters as they are always sorted by range)
}
radar_cfg_msg.data.RadarCfg_SortIndex = static_cast<uint64_t>(sort_index);
radar_cfg_msg.data.RadarCfg_SortIndex_valid = static_cast<uint64_t>(valid);
return true;
}
void RadarCfg::set_ctrl_relay_cfg(bool ctrl_relay, bool valid) {
radar_cfg_msg.data.RadarCfg_CtrlRelay = static_cast<uint64_t>(ctrl_relay);
radar_cfg_msg.data.RadarCfg_CtrlRelay_valid = static_cast<uint64_t>(valid); //Relay control message (0x8) is sent if true and the collision detection is activated
}
void RadarCfg::set_store_in_nvm(bool store_in_nvm, bool valid) {
radar_cfg_msg.data.RadarCfg_StoreInNVM = static_cast<uint64_t>(store_in_nvm);
radar_cfg_msg.data.RadarCfg_StoreInNVM_valid = static_cast<uint64_t>(valid); //Stores the current configuration to non-volatile memory to be read and set at sensor startup
}
bool RadarCfg::set_rcs_threshold(int rcs_threshold, bool valid) {
if (rcs_threshold != 0 && rcs_threshold != 1) {
return false; //Sets the sensitivity of the cluster detection to standard (0x0) or high sensitivity (0x1)
}
radar_cfg_msg.data.RadarCfg_RCS_Threshold = static_cast<uint64_t>(rcs_threshold);
radar_cfg_msg.data.RadarCfg_RCS_Threshold_valid = static_cast<uint64_t>(valid);
return true;
}
radar_cfg *RadarCfg::get_radar_cfg() {
return &radar_cfg_msg;
}
}
RadarState
```cpp
namespace radar_state {
typedef union radar_state {
struct {
uint64_t Reserved:6;
uint64_t RadarState_NVMReadStatus:1;
uint64_t RadarState_NVMwriteStatus:1;
uint64_t RadarState_MaxDistanceCfg1:8;
uint64_t Reserved2:1;
uint64_t RadarState_Voltage_Error:1;
uint64_t RadarState_Temporary_Error:1;
uint64_t RadarState_Temperature_Error:1;
uint64_t RadarState_Interference:1;
uint64_t RadarState_Persistent_Error:1;
uint64_t RadarState_MaxDistanceCfg2:2;
uint64_t RadarState_RadarPowerCfg1:2;
uint64_t Reserved3:6;
uint64_t RadarState_SensorID:3;
uint64_t Reserved4:1;
uint64_t RadarState_SortIndex:3;
uint64_t RadarState_RadarPowerCfg2:1;
uint64_t Reserved5:1;
uint64_t RadarState_CtrlRelayCfg:1;
uint64_t RadarState_OutputTypeCfg:2;
uint64_t RadarState_SendQualityCfg:1;
uint64_t RadarState_SendExtInfoCfg:1;
uint64_t RadarState_MotionRxState:2;
uint64_t Reserved6:8;
uint64_t Reserved7:2;
uint64_t RadarState_RCS_Threshold:3;
uint64_t Reserved8:3;
} data = {};
uint8_t raw_data[8];
} radar_state;
}
- RadarState类的实现
namespace radar_state {
RadarState::RadarState() {
}
RadarState::~RadarState() {
}
bool RadarState::get_read_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_NVMReadStatus);
}
bool RadarState::get_write_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_NVMwriteStatus);
}
uint64_t RadarState::get_max_distance() {
return (radar_state_msg.data.RadarState_MaxDistanceCfg1 << 2 |
radar_state_msg.data.RadarState_MaxDistanceCfg2) * 2;
} //数据在Union的存储位置是二进制位中的22-23位(cfg2),8-15位(cfg1),取数据时是cfg1和cfg2的拼接结果*2
bool RadarState::get_persistent_error_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_Persistent_Error);
}
bool RadarState::get_interference_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_Interference);
}
bool RadarState::get_temperature_error_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_Temperature_Error);
}
bool RadarState::get_temporary_error_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_Temporary_Error);
}
bool RadarState::get_voltage_error_status() {
return static_cast<bool>(radar_state_msg.data.RadarState_Voltage_Error);
}
int RadarState::get_sensor_id() {
return static_cast<int>(radar_state_msg.data.RadarState_SensorID);
}
int RadarState::get_sort_index() {
return static_cast<int>(radar_state_msg.data.RadarState_SortIndex);
}
int RadarState::get_radar_power_cfg() {
return static_cast<int>((radar_state_msg.data.RadarState_RadarPowerCfg1 << 1)
| radar_state_msg.data.RadarState_RadarPowerCfg2);
}
bool RadarState::get_ctrl_relay_cfg() {
return static_cast<bool>(radar_state_msg.data.RadarState_CtrlRelayCfg);
}
int RadarState::get_output_type_cfg() {
return static_cast<int>(radar_state_msg.data.RadarState_OutputTypeCfg);
}
bool RadarState::get_send_quality_cfg() {
return static_cast<bool>(radar_state_msg.data.RadarState_SendQualityCfg);
}
bool RadarState::get_ext_info_cfg() {
return static_cast<bool>(radar_state_msg.data.RadarState_SendExtInfoCfg);
}
int RadarState::get_motion_rx_state() {
return static_cast<int>(radar_state_msg.data.RadarState_MotionRxState);
}
int RadarState::get_rcs_threshold() {
return static_cast<int>(radar_state_msg.data.RadarState_RCS_Threshold);
}
radar_state *RadarState::get_radar_state() {
return &radar_state_msg;
}
}
后面也基本都一样,定义存储的union,定义类,类实现三步。
搞定所有信息后,再定义接口类:
ars_40X_can.hpp文件
#ifndef ARS_40X_ARS_40X_HPP
#define ARS_40X_ARS_40X_HPP
#include <socket_can/socket_can.hpp> //使用SocketCAN进行CAN总线通信
#include "ars_40X/cluster_list.hpp" //解析雷达的cluster模式数据
#include "ars_40X/motion_input_signals.hpp" //雷达的运动输入信号
#include "ars_40X/object_list.hpp" //解析雷达的object模式数据
#include "ars_40X/radar_cfg.hpp" //雷达配置信息
#include "ars_40X/radar_state.hpp" //雷达状态信息
#include <cstdint>
#include <string>
#include <iostream>
namespace ars_40X {
typedef enum can_messages { //参照传感器 CAN messages表(传感器ID为 0)
RadarCfg = 0x200,
RadarState = 0x201,
FilterCfg = 0x202,
FilterState_Header = 0x203,
FilterState_Cfg = 0x204,
CollDetCfg = 0x400,
CollDetRegionCfg = 0x401,
CollDetState = 0x408,
CollDetRegionState = 0x402,
SpeedInformation = 0x300,
YawRateInformation = 0x301,
Cluster_0_Status = 0x600,
Cluster_1_General = 0x701,
Cluster_2_Quality = 0x702,
Object_0_Status = 0x60A,
Object_1_General = 0x60B,
Object_2_Quality = 0x60C,
Object_3_Extended = 0x60D,
Object_4_Warning = 0x60E,
VersionID = 0x700,
CollDetRelayCtrl = 0x8,
} can_messages;
class ARS_40X_CAN {
public:
ARS_40X_CAN();
ARS_40X_CAN(std::string port); //用于接收传入的端口号,该端口号主要用于socket的初始化
~ARS_40X_CAN();
virtual bool receive_radar_data(); //虚函数,用于继承使用的该类的对象进行重写使用
virtual bool send_radar_data(uint32_t frame_id);
cluster_list::Cluster_0_Status *get_cluster_0_status();
cluster_list::Cluster_1_General *get_cluster_1_general();
cluster_list::Cluster_2_Quality *get_cluster_2_quality();
motion_input_signals::SpeedInformation *get_speed_information();
motion_input_signals::YawRateInformation *get_yaw_rate_information();
object_list::Object_0_Status *get_object_0_status();
object_list::Object_1_General *get_object_1_general();
object_list::Object_2_Quality *get_object_2_quality();
object_list::Object_3_Extended *get_object_3_extended();
radar_state::RadarState *get_radar_state();
radar_cfg::RadarCfg *get_radar_cfg(); //各部分协议的状态返回函数
virtual void send_cluster_0_status() {};
virtual void send_cluster_1_general() {};
virtual void send_cluster_2_quality() {};
virtual void send_object_0_status() {};
virtual void send_object_1_general() {};
virtual void send_object_2_quality() {};
virtual void send_object_3_extended() {};
virtual void send_radar_state() {}; //虚函数,用于各部分数据解析之后的发送操作
private:
socket_can::SocketCAN can_; //构建SocketCAN类对象
cluster_list::Cluster_0_Status cluster_0_status_;
cluster_list::Cluster_1_General cluster_1_general_;
cluster_list::Cluster_2_Quality cluster_2_quality_;
motion_input_signals::SpeedInformation speed_information_;
motion_input_signals::YawRateInformation yaw_rate_information_;
object_list::Object_0_Status object_0_status_;
object_list::Object_1_General object_1_general_;
object_list::Object_2_Quality object_2_quality_;
object_list::Object_3_Extended object_3_extended_;
radar_state::RadarState radar_state_;
radar_cfg::RadarCfg radar_cfg_; //构建各部分协议的类对象
};
}
#endif //ARS_40X_ARS_40X_HPP
接口类实现:
#include "ars_40X/ars_40X_can.hpp"
namespace ars_40X {
ARS_40X_CAN::ARS_40X_CAN() :
can_("can1") { //默认构造函数,指定默认的CAN口设备
}
ARS_40X_CAN::ARS_40X_CAN(std::string port) :
can_(port.c_str()) { //带参构造函数,从函数调用出获取需要绑定的设备端口号
}
ARS_40X_CAN::~ARS_40X_CAN() {
}
bool ARS_40X_CAN::receive_radar_data() {
uint32_t frame_id;
uint8_t dlc;
uint8_t data[8] = {0}; //定义用于接收CAN口数据的变量
bool read_status = can_.read(&frame_id, &dlc, data); //读取CAN口数据,并对读取状态进行判断
if (!read_status) {
return false;
}
switch (frame_id) { //根据获取的数据的frame_id来对其CAN messages类型进行判断,并交由相应解析部分处理
case RadarState:memcpy(radar_state_.get_radar_state()->raw_data, data, dlc);
send_radar_state();
break; //memcpy()为内存拷贝函数,表示将数据从data中拷贝dlc个字节到raw_data中。之后通过调用send()函数将数据解析后得到的结果发送出去
case Cluster_0_Status:memcpy(cluster_0_status_.get_cluster_0_status()->raw_data, data, dlc);
send_cluster_0_status();
break;
case Cluster_1_General:memcpy(cluster_1_general_.get_cluster_1_general()->raw_data, data, dlc);
send_cluster_1_general();
break;
case Cluster_2_Quality:memcpy(cluster_2_quality_.get_cluster_2_quality()->raw_data, data, dlc);
send_cluster_2_quality();
break;
case Object_0_Status:memcpy(object_0_status_.get_object_0_status()->raw_data, data, dlc);
send_object_0_status();
break;
case Object_1_General:memcpy(object_1_general_.get_object_1_general()->raw_data, data, dlc);
send_object_1_general();
break;
case Object_2_Quality:memcpy(object_2_quality_.get_object_2_quality()->raw_data, data, dlc);
send_object_2_quality();
break;
case Object_3_Extended:memcpy(object_3_extended_.get_object_3_extended()->raw_data, data, dlc);
send_object_3_extended();
break;
default: {
#if DEBUG
printf("Unidentified Message: %d\n", frame_id); //如果frame_id不属于以上CAN messages类型,则输出错误信息。
#endif
break;
}
}
return true;
}
bool ARS_40X_CAN::send_radar_data(uint32_t frame_id) { //向雷达发送数据的函数
switch (frame_id) {
case RadarCfg:can_.write(frame_id, 8, radar_cfg_.get_radar_cfg()->raw_data);
break; //发送雷达配置信息
case SpeedInformation:
can_.write(frame_id,
2,
speed_information_.get_speed_information()->raw_data);
break; //发送雷达速度信息
case YawRateInformation:
can_.write(frame_id,
2,
yaw_rate_information_.get_yaw_rate_information()->raw_data);
break; //发送雷达角速度信息
#if DEBUG
default: printf("Frame ID not supported\n"); //发送不符合要求信息时的错误输出
#endif
}
return true;
}
cluster_list::Cluster_0_Status *ARS_40X_CAN::get_cluster_0_status() {
return &cluster_0_status_; //返回类对象,便于函数调用
}
cluster_list::Cluster_1_General *ARS_40X_CAN::get_cluster_1_general() {
return &cluster_1_general_;
}
cluster_list::Cluster_2_Quality *ARS_40X_CAN::get_cluster_2_quality() {
return &cluster_2_quality_;
}
motion_input_signals::SpeedInformation *ARS_40X_CAN::get_speed_information() {
return &speed_information_;
}
motion_input_signals::YawRateInformation *ARS_40X_CAN::get_yaw_rate_information() {
return &yaw_rate_information_;
}
object_list::Object_0_Status *ARS_40X_CAN::get_object_0_status() {
return &object_0_status_;
}
object_list::Object_1_General *ARS_40X_CAN::get_object_1_general() {
return &object_1_general_;
}
object_list::Object_2_Quality *ARS_40X_CAN::get_object_2_quality() {
return &object_2_quality_;
}
object_list::Object_3_Extended *ARS_40X_CAN::get_object_3_extended() {
return &object_3_extended_;
}
radar_state::RadarState *ARS_40X_CAN::get_radar_state() {
return &radar_state_;
}
radar_cfg::RadarCfg *ARS_40X_CAN::get_radar_cfg() {
return &radar_cfg_;
}
}