文章目录
- 介绍
- FDILink通讯协议
- 数据帧组成
- 数据包
- 数据处理
- 打开串口
- 在头文件中定义参数
- 串口读取
- 代码运用
- 依赖：
- 使用：
- 源码
介绍
DETA100系列 是一个提供 GNSS/INS & AHRS 系统的模组，在最苛刻的条件下提供准确的位置、速度、加速度和姿态数据。它结合了温度校准的加速度计，陀螺仪，磁力计与一个双天线RTK、GNSS接收器。这些是耦合在一个复杂的融合算法，以提供准确和可靠的导航和方向。同时DETA100系列 支持辅助设备的数据接入，如里程计、光流计、RTCM 数据等。
FDILink通讯协议
数据帧组成
A: 指令类别
B：载荷的字节数。
C：流水号， 每发送一个数据帧数值加一，用于检测数据帧丢包。
D：帧头CRC8校验，计算帧头部分 起始标志 + 指令类别 + 数据长度 + 流水序号。
E：数据CRC16校验，计算载荷数据的CRC16校验。
数据包
以双天线为例
数据处理
打开串口
try
{
serial_.setPort(serial_port_);
serial_.setBaudrate(serial_baud_);
serial_.setFlowcontrol(serial::flowcontrol_none);
serial_.setParity(serial::parity_none); //default is parity_none
serial_.setStopbits(serial::stopbits_one);
serial_.setBytesize(serial::eightbits);
serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
serial_.setTimeout(time_out);
serial_.open();
}
catch (serial::IOException &e)
{
ROS_ERROR_STREAM("Unable to open port ");
exit(0);
}
在头文件中定义参数
进行指令和字节数赋值
以双天线消息为例
#define TYPE_GNSS 0x78 //指令ID
#define GNSS_DUAL_ANTENNA_DATA_LEN 0x86 //字节数长度 134 = 133+1
定义双天线消息的数据结构
#pragma pack(1)
struct Gnss_Dual_Antenna_data_Packet_t
{
uint32_t Microseconds;
double RoverRtkRefPosN;
double RoverRtkRefPosE;
double RoverRtkRefPosD;
double MBRtkRefPosN;
double MBRtkRefPosE;
double MBRtkRefPosD;
double RoverLat;
double RoverLon;
float RoverAlt;
float Rover_hAcc;
float Rover_vAcc;
double MBLat;
double MBLon;
float MBAlt;
float MBhAcc;
float MBvAcc;
double RoverRtkPosLength;
float RoverRtkAccuracyLength;
double RoverRtkPosHeading;
float RoverRtkAccuracyHeading;
uint8_t MBfixtype;
uint8_t Roverfixtype;
};
#pragma pack()
串口读取
else if (head_type[0] == TYPE_GNSS)
{
Gnss_Dual_Antenna_data_frame_.frame.header.header_start = check_head[0];
Gnss_Dual_Antenna_data_frame_.frame.header.data_type = head_type[0];
Gnss_Dual_Antenna_data_frame_.frame.header.data_size = check_len[0];
Gnss_Dual_Antenna_data_frame_.frame.header.serial_num = check_sn[0];
Gnss_Dual_Antenna_data_frame_.frame.header.header_crc8 = head_crc8[0];
Gnss_Dual_Antenna_data_frame_.frame.header.header_crc16_h = head_crc16_H[0];
Gnss_Dual_Antenna_data_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(Gnss_Dual_Antenna_data_frame_.read_buf.frame_header, 4);
if (CRC8 != Gnss_Dual_Antenna_data_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = Gnss_Dual_Antenna_data_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
else if (head_type[0] == TYPE_GNSS)
{
uint16_t head_crc16_l = Gnss_Dual_Antenna_data_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = Gnss_Dual_Antenna_data_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(Gnss_Dual_Antenna_data_frame_.read_buf.read_msg, (GNSS_DUAL_ANTENNA_DATA_LEN + 1)); //134+1
// if (if_debug_){
// for (size_t i = 0; i < (GNSS_DUAL_ANTENNA_DATA_LEN + 1); i++)
// {
// std::cout << std::hex << (int)Gnss_Dual_Antenna_data_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// std::cout << "frame_end: " << std::hex << (int)Gnss_Dual_Antenna_data_frame_.frame.frame_end<< std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(Gnss_Dual_Antenna_data_frame_.frame.data.data_buff, GNSS_DUAL_ANTENNA_DATA_LEN);
if (if_debug_){
std::cout << "CRC16: " << std::hex << (int)CRC16 << std::dec << std::endl;
std::cout << "head_crc16: " << std::hex << (int)head_crc16 << std::dec << std::endl;
std::cout << "head_crc16_h: " << std::hex << (int)head_crc16_h << std::dec << std::endl;
std::cout << "head_crc16_l: " << std::hex << (int)head_crc16_l << std::dec << std::endl;
bool if_right = ((int)head_crc16 == (int)CRC16);
std::cout << "if_right: " << if_right << std::endl;
}
if (head_crc16 != CRC16)
{
ROS_WARN("check crc16 faild(gnss).");
continue;
}
else if(Gnss_Dual_Antenna_data_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.gnss");
continue;
}
}
代码运用
依赖：
sudo apt install ros-melodic-serial
使用：
ahrs_driver.launch
<launch>
<node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
<!-- 串口设备，可通过rules.d配置固定 -->
<param name="port" value="/dev/ttyUSB0"/>
<!-- <param name="port" value="/dev/ttyTHS1"/> -->
<!-- 波特率 -->
<param name="baud" value="921600"/>
<!-- 发布的imu话题名 -->
<param name="imu_topic" value="/imu"/>
<!-- 发布的imu话题中的frame_id -->
<param name="imu_frame" value="imu"/>
<!-- 地磁北的yaw角 --> # 二维指北的朝向，北为0，逆时针增加，0~2π的取值范围。
<param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
<!-- 发布的数据基于不同设备有不同的坐标系 -->
<param name="device_type" value="1"/> <!-- 0: origin_data, 1: for single imu or ucar in ROS, 2:for Xiao in ROS -->
</node>
</launch>
其中device_type：
- Deta-10的原始坐标系模式
- 单独imu的坐标系模式
调用的ahrs_driver节点会发布sensor_msgs/Imu格式的imu topic。
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance
也会发布geometry_msgs/Pose2D格式的二维指北角话题，话题名默认为/mag_pose_2d。
float64 x
float64 y
float64 theta # 指北角
源码
链接：https://pan.baidu.com/s/1xG-Hmpuv_GSkeDP47lfApA
提取码：armd
