介绍

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_noneserial_.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：

0. Deta-10的原始坐标系模式
1. 单独imu的坐标系模式

调用的ahrs_driver节点会发布sensor_msgs/Imu格式的imu topic。

std_msgs/Header headeruint32 seqtime stampstring frame_id
geometry_msgs/Quaternion orientationfloat64 xfloat64 yfloat64 zfloat64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocityfloat64 xfloat64 yfloat64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_accelerationfloat64 xfloat64 yfloat64 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