houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/fdilink_ahrs/CMakeLists.txt
+50
View File
@@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 2.8)
project(fdilink_ahrs)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
find_package(Eigen3 REQUIRED)
set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
find_package(catkin REQUIRED COMPONENTS
message_generation
# genmsg
# geometry_msgs
roscpp
rospy
sensor_msgs
std_msgs
tf
# std_srvs
nav_msgs
serial
)
include_directories(
include
src
${catkin_INCLUDE_DIRS}
${Eigen3_INCLUDE_DIRS}
)
catkin_package(
CATKIN_DEPENDS message_runtime
# INCLUDE_DIRS include
# LIBRARIES xf_mic_api
# CATKIN_DEPENDS roscpp rospy senor_msgs std_msgs
# DEPENDS system_lib
)
# set(SERIAL_LIB /opt/ros/kinetic/lib/libserial.so)
# set(SERIAL_LIB_PATH /opt/ros/kinetic/lib)
# link_directories(${SERIAL_LIB_PATH})
## ahrs_driver
add_executable(ahrs_driver src/ahrs_driver.cpp)
target_link_libraries(ahrs_driver crc_table ${catkin_LIBRARIES})# ${SERIAL_LIB}
add_executable(imu_tf src/imu_tf.cpp)
target_link_libraries( imu_tf ${catkin_LIBRARIES})# ${SERIAL_LIB}
## crc_table
add_library(crc_table src/crc_table.cpp)
sensors/fdilink_ahrs/README.md
+81
View File
@@ -0,0 +1,81 @@
## fdilink的imu驱动包
Deta-10-ros-v0.0.1
### 依赖：
```bash
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 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 # 指北角
```
### 2020-1-15
维护了文件注释。
### 2020-10-20
添加了`device_type`参数，可以在`ahrs_data.launch`文件中指定设备类型，根据不同设备类型以不同的坐标系发布ROS的imu数据。
其中：
0. Deta-10的原始坐标系模式
1. 单独imu的坐标系模式
sensors/fdilink_ahrs/include/ahrs_driver.h
+97
View File
@@ -0,0 +1,97 @@
#ifndef BASE_DRIVER_H_
#define BASE_DRIVER_H_
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <iostream>
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
#include <fstream>
#include <fdilink_data_struct.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/Pose2D.h>
#include <boost/thread.hpp>
#include <string>
#include <ros/package.h>
#include <crc_table.h>
using namespace std;
namespace FDILink
{
#define FRAME_HEAD 0xfc
#define FRAME_END 0xfd
#define TYPE_IMU 0x40
#define TYPE_AHRS 0x41
#define TYPE_INSGPS 0x42
#define TYPE_GEODETIC_POS 0x5c
#define TYPE_GROUND 0xf0
#define IMU_LEN 0x38 //56
#define AHRS_LEN 0x30 //48
#define INSGPS_LEN 0x48 //80
#define GEODETIC_POS_LEN 0x20 //32
#define PI 3.141592653589793
#define DEG_TO_RAD 0.017453292519943295
class ahrsBringup
{
public:
ahrsBringup();
~ahrsBringup();
void processLoop();
bool checkCS8(int len);
bool checkCS16(int len);
void checkSN(int type);
void magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz);
ros::NodeHandle nh_;
private:
bool if_debug_;
//sum info
int sn_lost_ = 0;
int crc_error_ = 0;
uint8_t read_sn_ = 0;
bool frist_sn_;
int device_type_ = 1;
//serial
serial::Serial serial_; //声明串口对象
std::string serial_port_;
int serial_baud_;
int serial_timeout_;
//data
FDILink::imu_frame_read imu_frame_;
FDILink::ahrs_frame_read ahrs_frame_;
FDILink::insgps_frame_read insgps_frame_;
//FDILink::lanlon_frame_read latlon_frame_;
FDILink::Geodetic_Position_frame_read Geodetic_Position_frame_;
//frame name
string imu_frame_id_;
string insgps_frame_id_;
string latlon_frame_id_;
//topic
string imu_topic_, mag_pose_2d_topic_;
string latlon_topic_;
string Euler_angles_topic_,Magnetic_topic_;
string gps_topic_,twist_topic_,NED_odom_topic_;
//Publisher
ros::Publisher imu_pub_;
ros::Publisher gps_pub_;
ros::Publisher mag_pose_pub_;
ros::Publisher Euler_angles_pub_;
ros::Publisher Magnetic_pub_;
ros::Publisher twist_pub_;
ros::Publisher NED_odom_pub_;
}; //ahrsBringup
} // namespace FDILink
#endif
sensors/fdilink_ahrs/include/crc_table.h
+10
View File
@@ -0,0 +1,10 @@
#ifndef CRC_TABLE_H
#define CRC_TABLE_H
#include <stdint.h>
uint8_t CRC8_Table(uint8_t* p, uint8_t counter);
uint16_t CRC16_Table(uint8_t *p, uint8_t counter);
uint32_t CRC32_Table(uint8_t *p, uint8_t counter);
#endif // CRC_TABLE_H
sensors/fdilink_ahrs/include/fdilink_data_struct.h
+189
View File
@@ -0,0 +1,189 @@
#ifndef FDILINK_DATA_STRUCT_H_
#define FDILINK_DATA_STRUCT_H_
#include <iostream>
namespace FDILink{
#pragma pack(1)
struct fdilink_header
{
uint8_t header_start;
uint8_t data_type;
uint8_t data_size;
uint8_t serial_num;
uint8_t header_crc8;
uint8_t header_crc16_h;
uint8_t header_crc16_l;
};
#pragma pack()
#pragma pack(1)
struct IMUData_Packet_t
{
float gyroscope_x; //unit: rad/s
float gyroscope_y; //unit: rad/s
float gyroscope_z; //unit: rad/s
float accelerometer_x; //m/s^2
float accelerometer_y; //m/s^2
float accelerometer_z; //m/s^2
float magnetometer_x; //mG
float magnetometer_y; //mG
float magnetometer_z; //mG
float imu_temperature; //C
float Pressure; //Pa
float pressure_temperature; //C
int64_t Timestamp; //us
};
#pragma pack()
struct AHRSData_Packet_t
{
float RollSpeed; //unit: rad/s
float PitchSpeed; //unit: rad/s
float HeadingSpeed;//unit: rad/s
float Roll; //unit: rad
float Pitch; //unit: rad
float Heading; //unit: rad
float Qw;//w //Quaternion
float Qx;//x
float Qy;//y
float Qz;//z
int64_t Timestamp; //unit: us
};
#pragma pack(1)
struct INSGPSData_Packet_t
{
float BodyVelocity_X;
float BodyVelocity_Y;
float BodyVelocity_Z;
float BodyAcceleration_X;
float BodyAcceleration_Y;
float BodyAcceleration_Z;
float Location_North;
float Location_East;
float Location_Down;
float Velocity_North;
float Velocity_East;
float Velocity_Down;
float Acceleration_North;
float Acceleration_East;
float Acceleration_Down;
float Pressure_Altitude;
int64_t Timestamp;
};
#pragma pack()
#pragma pack(1)
struct Geodetic_Position_Packet_t
{
double Latitude;
double Longitude;
double Height;
float hAcc;
float vAcc;
};
#pragma pack()
//for IMU=========================
#pragma pack(1)
struct read_imu_struct{
fdilink_header header; //7
union data
{
IMUData_Packet_t data_pack; //56
uint8_t data_buff[56]; //56
}data;
uint8_t frame_end; //1
};
struct read_imu_tmp{
uint8_t frame_header[7];
uint8_t read_msg[57];
};
union imu_frame_read{
struct read_imu_struct frame;
read_imu_tmp read_buf;
uint8_t read_tmp[64];
};
#pragma pack()
//for IMU------------------------
//for AHRS=========================
#pragma pack(1)
struct read_ahrs_struct{
fdilink_header header; //7
union data
{
AHRSData_Packet_t data_pack; //48
uint8_t data_buff[48]; //48
}data;
uint8_t frame_end; //1
};
struct read_ahrs_tmp{
uint8_t frame_header[7];
uint8_t read_msg[49];
};
union ahrs_frame_read{
struct read_ahrs_struct frame;
read_ahrs_tmp read_buf;
uint8_t read_tmp[56];
};
#pragma pack()
//for AHRS------------------------
//for INSGPS=========================
#pragma pack(1)
struct read_insgps_struct{
fdilink_header header; //7
union data
{
INSGPSData_Packet_t data_pack; //72
uint8_t data_buff[72]; //72
}data;
uint8_t frame_end; //1
};
struct read_insgps_tmp{
uint8_t frame_header[7];
uint8_t read_msg[73];
};
union insgps_frame_read{
struct read_insgps_struct frame;
read_insgps_tmp read_buf;
uint8_t read_tmp[80];
};
#pragma pack()
//for INSGPS------------------------
//for Geodetic_Position=========================
#pragma pack(1)
struct read_Geodetic_Position_struct{
fdilink_header header; //7
union data
{
Geodetic_Position_Packet_t data_pack; //40
uint8_t data_buff[32]; //40
}data;
uint8_t frame_end; //1
};
struct read_Geodetic_Position_tmp{
uint8_t frame_header[7];
uint8_t read_msg[33];
};
union Geodetic_Position_frame_read{
struct read_Geodetic_Position_struct frame;
read_Geodetic_Position_tmp read_buf;
uint8_t read_tmp[40];
};
#pragma pack()
}//namespace FDILink
#endif//FDILINK_DATA_STRUCT_H_
sensors/fdilink_ahrs/launch/ahrs_data.launch
+38
View File
@@ -0,0 +1,38 @@
<launch>
<node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
<!-- 是否输出debug信息 -->
<param name="debug" value="false"/>
<!-- 串口设备，可通过rules.d配置固定。
若使用DETA100,则value="/dev/wheeltec_ch340"
若使用WHEELTEC N系列，则不需要改动 -->
<param name="port" value="/dev/ttyUSB0"/>
<!-- 波特率 -->
<param name="baud" value="921600"/>
<!-- 发布的imu话题名 -->
<param name="imu_topic" value="/imu/data"/>
<!-- 发布的imu话题中的frame_id -->
<param name="imu_frame" value="imu"/>
<!-- 地磁北的yaw角 -->
<param name="mag_pose_2d_topic" value="/mag_pose_2d"/>
<!-- 欧拉角 -->
<param name="Euler_angles_pub_" value="/euler_angles"/>
<!-- 磁力计磁场强度 -->
<param name="Magnetic_pub_" value="/magnetic"/>
<!-- gps数据输出话题-->
<param name="gps_topic_" value="/gps/fix"/>
<!-- 机体系速度数据输出话题-->
<param name="twist_topic_" value="/system_speed"/>
<!-- NED系位移和速度数据输出话题-->
<param name="NED_odom_topic_" value="/NED_odometry"/>
<!-- 发布的数据基于不同设备有不同的坐标系-->
<param name="device_type" value="1"/> <!-- 0: origin_data, 1: for single imu or ucar in ROS, 2:for Xiao in ROS -->
</node>
</launch>
sensors/fdilink_ahrs/launch/tf.launch
+11
View File
@@ -0,0 +1,11 @@
<launch>
<node pkg="fdilink_ahrs" name="imu_tf" type="imu_tf" output="screen" >
<param name="imu_topic" value="/imu"/>
<param name="world_frame_id" value="/world"/>
<param name="imu_frame_id" value="/gyro_link"/>
<param name="position_x" value="1"/>
<param name="position_y" value="1"/>
<param name="position_z" value="0"/>
</node>
</launch>
sensors/fdilink_ahrs/package.xml
+54
View File
@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<package format="2">
<name>fdilink_ahrs</name>
<version>0.0.1</version>
<description>The fdilink_ahrs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="jwliang@iflytek.com">iflytek</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>genmsg</buildtool_depend>
<!-- <build_depend>geometry_msgs</build_depend> -->
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>tf</build_depend>
<build_depend>nav_msgs</build_depend>
<!-- <build_export_depend>geometry_msgs</build_export_depend> -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<build_export_depend>tf</build_export_depend>
<!-- <exec_depend>geometry_msgs</exec_depend> -->
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
<!-- <build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend> -->
</package>
sensors/fdilink_ahrs/src/ahrs_driver.cpp
+640
View File
@@ -0,0 +1,640 @@
#include <ahrs_driver.h>
#include <Eigen/Eigen>
namespace FDILink
{
ahrsBringup::ahrsBringup() :frist_sn_(false), serial_timeout_(20)
{
ros::NodeHandle pravite_nh("~");
//topic_name & frame_id 加载参数服务器
pravite_nh.param("debug", if_debug_, false);
pravite_nh.param("device_type", device_type_, 1); // default: single imu
pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
pravite_nh.param("gps_topic_", gps_topic_, std::string("/gps/fix"));
pravite_nh.param("twist_topic_", twist_topic_, std::string("/system_speed"));
pravite_nh.param("NED_odom_topic_", NED_odom_topic_, std::string("/NED_odometry"));
//serial
pravite_nh.param("port", serial_port_, std::string("/dev/ttyTHS1"));
pravite_nh.param("baud", serial_baud_, 115200);
//publisher 创建发布对象
imu_pub_ = nh_.advertise<sensor_msgs::Imu>(imu_topic_.c_str(), 10);
mag_pose_pub_ = nh_.advertise<geometry_msgs::Pose2D>(mag_pose_2d_topic_.c_str(), 10);
Euler_angles_pub_ = nh_.advertise<geometry_msgs::Vector3>(Euler_angles_topic_.c_str(), 10);
Magnetic_pub_ = nh_.advertise<geometry_msgs::Vector3>(Magnetic_topic_.c_str(), 10);
gps_pub_ = nh_.advertise<sensor_msgs::NavSatFix>(gps_topic_.c_str(), 10);
twist_pub_ = nh_.advertise<geometry_msgs::Twist>(twist_topic_.c_str(), 10);
NED_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(NED_odom_topic_.c_str(), 10);
//setp up serial 设置串口参数并打开串口
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);
}
if (serial_.isOpen())
{
ROS_INFO_STREAM("Serial Port initialized");
}
else
{
ROS_ERROR_STREAM("Unable to initial Serial port ");
exit(0);
}
processLoop();
}
ahrsBringup::~ahrsBringup() // 析构函数关闭串口通道
{
if (serial_.isOpen())
serial_.close();
}
void ahrsBringup::processLoop() // 数据处理过程
{
ROS_INFO("ahrsBringup::processLoop: start");
while (ros::ok())
{
if (!serial_.isOpen())
{
ROS_WARN("serial unopen");
}
//check head start 检查起始 数据帧头
uint8_t check_head[1] = {0xff};
size_t head_s = serial_.read(check_head, 1);
if (if_debug_){
if (head_s != 1)
{
ROS_ERROR("Read serial port time out! can't read pack head.");
}
std::cout << std::endl;
std::cout << "check_head: " << std::hex << (int)check_head[0] << std::dec << std::endl;
}
if (check_head[0] != FRAME_HEAD)
{
continue;
}
// check head type 检查数据类型
uint8_t head_type[1] = {0xff};
size_t type_s = serial_.read(head_type, 1);
if (if_debug_){
std::cout << "head_type: " << std::hex << (int)head_type[0] << std::dec << std::endl;
}
if (head_type[0] != TYPE_IMU && head_type[0] != TYPE_AHRS && head_type[0] != TYPE_INSGPS && head_type[0] != TYPE_GEODETIC_POS && head_type[0] != 0x50 && head_type[0] != TYPE_GROUND&& head_type[0] != 0xff)
{
ROS_WARN("head_type error: %02X",head_type[0]);
continue;
}
//check head length 检查对应数据类型的长度是否符合
uint8_t check_len[1] = {0xff};
size_t len_s = serial_.read(check_len, 1);
if (if_debug_){
std::cout << "check_len: "<< std::dec << (int)check_len[0] << std::endl;
}
if (head_type[0] == TYPE_IMU && check_len[0] != IMU_LEN)
{
ROS_WARN("head_len error (imu)");
continue;
}else if (head_type[0] == TYPE_AHRS && check_len[0] != AHRS_LEN)
{
ROS_WARN("head_len error (ahrs)");
continue;
}else if (head_type[0] == TYPE_INSGPS && check_len[0] != INSGPS_LEN)
{
ROS_WARN("head_len error (insgps)");
continue;
}else if (head_type[0] == TYPE_GEODETIC_POS && check_len[0] != GEODETIC_POS_LEN)
{
ROS_WARN("head_len error (GEODETIC_POS)");
continue;
}
else if (head_type[0] == TYPE_GROUND || head_type[0] == 0x50) // 未知数据，防止记录失败
{
uint8_t ground_sn[1];
size_t ground_sn_s = serial_.read(ground_sn, 1);
if (++read_sn_ != ground_sn[0])
{
if ( ground_sn[0] < read_sn_)
{
if(if_debug_){
ROS_WARN("detected sn lost.");
}
sn_lost_ += 256 - (int)(read_sn_ - ground_sn[0]);
read_sn_ = ground_sn[0];
// continue;
}
else
{
if(if_debug_){
ROS_WARN("detected sn lost.");
}
sn_lost_ += (int)(ground_sn[0] - read_sn_);
read_sn_ = ground_sn[0];
// continue;
}
}
uint8_t ground_ignore[500];
size_t ground_ignore_s = serial_.read(ground_ignore, (check_len[0]+4));
continue;
}
//read head sn 检查sn 流水序号
uint8_t check_sn[1] = {0xff};
size_t sn_s = serial_.read(check_sn, 1);
uint8_t head_crc8[1] = {0xff};
size_t crc8_s = serial_.read(head_crc8, 1);
uint8_t head_crc16_H[1] = {0xff};
uint8_t head_crc16_L[1] = {0xff};
size_t crc16_H_s = serial_.read(head_crc16_H, 1);
size_t crc16_L_s = serial_.read(head_crc16_L, 1);
if (if_debug_){
std::cout << "check_sn: " << std::hex << (int)check_sn[0] << std::dec << std::endl;
std::cout << "head_crc8: " << std::hex << (int)head_crc8[0] << std::dec << std::endl;
std::cout << "head_crc16_H: " << std::hex << (int)head_crc16_H[0] << std::dec << std::endl;
std::cout << "head_crc16_L: " << std::hex << (int)head_crc16_L[0] << std::dec << std::endl;
}
// put header & check crc8 & count sn lost
// check crc8 进行crc8数据校验
if (head_type[0] == TYPE_IMU)
{
imu_frame_.frame.header.header_start = check_head[0];
imu_frame_.frame.header.data_type = head_type[0];
imu_frame_.frame.header.data_size = check_len[0];
imu_frame_.frame.header.serial_num = check_sn[0];
imu_frame_.frame.header.header_crc8 = head_crc8[0];
imu_frame_.frame.header.header_crc16_h = head_crc16_H[0];
imu_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(imu_frame_.read_buf.frame_header, 4);
if (CRC8 != imu_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = imu_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
//check sn
ahrsBringup::checkSN(TYPE_IMU);
}
else if (head_type[0] == TYPE_AHRS)
{
ahrs_frame_.frame.header.header_start = check_head[0];
ahrs_frame_.frame.header.data_type = head_type[0];
ahrs_frame_.frame.header.data_size = check_len[0];
ahrs_frame_.frame.header.serial_num = check_sn[0];
ahrs_frame_.frame.header.header_crc8 = head_crc8[0];
ahrs_frame_.frame.header.header_crc16_h = head_crc16_H[0];
ahrs_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(ahrs_frame_.read_buf.frame_header, 4);
if (CRC8 != ahrs_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = ahrs_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
//check sn
ahrsBringup::checkSN(TYPE_AHRS);
}
else if (head_type[0] == TYPE_INSGPS)
{
insgps_frame_.frame.header.header_start = check_head[0];
insgps_frame_.frame.header.data_type = head_type[0];
insgps_frame_.frame.header.data_size = check_len[0];
insgps_frame_.frame.header.serial_num = check_sn[0];
insgps_frame_.frame.header.header_crc8 = head_crc8[0];
insgps_frame_.frame.header.header_crc16_h = head_crc16_H[0];
insgps_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(insgps_frame_.read_buf.frame_header, 4);
if (CRC8 != insgps_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
else if(if_debug_)
{
std::cout << "header_crc8 matched." << std::endl;
}
ahrsBringup::checkSN(TYPE_INSGPS);
}
else if (head_type[0] == TYPE_GEODETIC_POS)
{
Geodetic_Position_frame_.frame.header.header_start = check_head[0];
Geodetic_Position_frame_.frame.header.data_type = head_type[0];
Geodetic_Position_frame_.frame.header.data_size = check_len[0];
Geodetic_Position_frame_.frame.header.serial_num = check_sn[0];
Geodetic_Position_frame_.frame.header.header_crc8 = head_crc8[0];
Geodetic_Position_frame_.frame.header.header_crc16_h = head_crc16_H[0];
Geodetic_Position_frame_.frame.header.header_crc16_l = head_crc16_L[0];
uint8_t CRC8 = CRC8_Table(Geodetic_Position_frame_.read_buf.frame_header, 4);
if (CRC8 != Geodetic_Position_frame_.frame.header.header_crc8)
{
ROS_WARN("header_crc8 error");
continue;
}
if(!frist_sn_){
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num - 1;
frist_sn_ = true;
}
ahrsBringup::checkSN(TYPE_GEODETIC_POS);
}
// check crc16 进行crc16数据校验
if (head_type[0] == TYPE_IMU)
{
uint16_t head_crc16_l = imu_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = imu_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(imu_frame_.read_buf.read_msg, (IMU_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (IMU_LEN + 1); i++)
// {
// std::cout << std::hex << (int)imu_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(imu_frame_.frame.data.data_buff, IMU_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(imu).");
continue;
}
else if(imu_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_AHRS)
{
uint16_t head_crc16_l = ahrs_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = ahrs_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(ahrs_frame_.read_buf.read_msg, (AHRS_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(ahrs_frame_.frame.data.data_buff, AHRS_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(ahrs).");
continue;
}
else if(ahrs_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_INSGPS)
{
uint16_t head_crc16_l = insgps_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = insgps_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(insgps_frame_.read_buf.read_msg, (INSGPS_LEN + 1)); //48+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(insgps_frame_.frame.data.data_buff, INSGPS_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(ahrs).");
continue;
}
else if(insgps_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
else if (head_type[0] == TYPE_GEODETIC_POS)
{
uint16_t head_crc16_l = Geodetic_Position_frame_.frame.header.header_crc16_l;
uint16_t head_crc16_h = Geodetic_Position_frame_.frame.header.header_crc16_h;
uint16_t head_crc16 = head_crc16_l + (head_crc16_h << 8);
size_t data_s = serial_.read(Geodetic_Position_frame_.read_buf.read_msg, (GEODETIC_POS_LEN + 1)); //24+1
// if (if_debug_){
// for (size_t i = 0; i < (AHRS_LEN + 1); i++)
// {
// std::cout << std::hex << (int)ahrs_frame_.read_buf.read_msg[i] << " ";
// }
// std::cout << std::dec << std::endl;
// }
uint16_t CRC16 = CRC16_Table(Geodetic_Position_frame_.frame.data.data_buff, GEODETIC_POS_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(gps).");
continue;
}
else if(Geodetic_Position_frame_.frame.frame_end != FRAME_END)
{
ROS_WARN("check frame end.");
continue;
}
}
//读取IMU数据进行解析，并发布相关话题
if (head_type[0] == TYPE_IMU)
{
// publish imu topic
sensor_msgs::Imu imu_data;
imu_data.header.stamp = ros::Time::now();
imu_data.header.frame_id = imu_frame_id_.c_str();
Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
ahrs_frame_.frame.data.data_pack.Qx,
ahrs_frame_.frame.data.data_pack.Qy,
ahrs_frame_.frame.data.data_pack.Qz);
Eigen::Quaterniond q_r =
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_rr =
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_xiao_rr =
Eigen::AngleAxisd( PI/2, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd( PI, Eigen::Vector3d::UnitX());
if (device_type_ == 0) //未经变换的原始数据
{
imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qx;
imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qy;
imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;
imu_data.angular_velocity.x = ahrs_frame_.frame.data.data_pack.RollSpeed;
imu_data.angular_velocity.y = ahrs_frame_.frame.data.data_pack.PitchSpeed;
imu_data.angular_velocity.z = ahrs_frame_.frame.data.data_pack.HeadingSpeed;
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_y;
imu_data.linear_acceleration.z = imu_frame_.frame.data.data_pack.accelerometer_z;
}
else if (device_type_ == 1) //imu单品ROS标准下的坐标变换
{
Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr;
imu_data.orientation.w = q_out.w();
imu_data.orientation.x = q_out.x();
imu_data.orientation.y = q_out.y();
imu_data.orientation.z = q_out.z();
imu_data.angular_velocity.x = ahrs_frame_.frame.data.data_pack.RollSpeed;
imu_data.angular_velocity.y = -ahrs_frame_.frame.data.data_pack.PitchSpeed;
imu_data.angular_velocity.z = -ahrs_frame_.frame.data.data_pack.HeadingSpeed;
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_x;
imu_data.linear_acceleration.y = -imu_frame_.frame.data.data_pack.accelerometer_y;
imu_data.linear_acceleration.z = -imu_frame_.frame.data.data_pack.accelerometer_z;
}
imu_pub_.publish(imu_data);
}
//读取AHRS数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_AHRS)
{
geometry_msgs::Pose2D pose_2d;
pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
mag_pose_pub_.publish(pose_2d);
//std::cout << "YAW: " << pose_2d.theta << std::endl;
geometry_msgs::Vector3 Euler_angles_2d,Magnetic;
Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
Euler_angles_pub_.publish(Euler_angles_2d);
Magnetic_pub_.publish(Magnetic);
}
//读取gps_pos数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_GEODETIC_POS)
{
sensor_msgs::NavSatFix gps_data;
gps_data.header.stamp = ros::Time::now();
gps_data.header.frame_id = "navsat_link";
gps_data.latitude = Geodetic_Position_frame_.frame.data.data_pack.Latitude / DEG_TO_RAD;
gps_data.longitude = Geodetic_Position_frame_.frame.data.data_pack.Longitude / DEG_TO_RAD;
gps_data.altitude = Geodetic_Position_frame_.frame.data.data_pack.Height;
//std::cout << "lat: " << Geodetic_Position_frame_.frame.data.data_pack.Latitude << std::endl;
//std::cout << "lon: " << Geodetic_Position_frame_.frame.data.data_pack.Longitude << std::endl;
//std::cout << "h: " << Geodetic_Position_frame_.frame.data.data_pack.Height << std::endl;
gps_pub_.publish(gps_data);
}
//读取INSGPS数据进行解析，并发布相关话题
else if (head_type[0] == TYPE_INSGPS)
{
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = ros::Time::now();
// odom_msg.header.frame_id = odom_frame_id; // Odometer TF parent coordinates //里程计TF父坐标
odom_msg.pose.pose.position.x = insgps_frame_.frame.data.data_pack.Location_North; //Position //位置
odom_msg.pose.pose.position.y = insgps_frame_.frame.data.data_pack.Location_East;
odom_msg.pose.pose.position.z = insgps_frame_.frame.data.data_pack.Location_Down;
// odom_msg.child_frame_id = robot_frame_id; // Odometer TF subcoordinates //里程计TF子坐标
odom_msg.twist.twist.linear.x = insgps_frame_.frame.data.data_pack.Velocity_North; //Speed in the X direction //X方向速度
odom_msg.twist.twist.linear.y = insgps_frame_.frame.data.data_pack.Velocity_East; //Speed in the Y direction //Y方向速度
odom_msg.twist.twist.linear.z = insgps_frame_.frame.data.data_pack.Velocity_Down;
NED_odom_pub_.publish(odom_msg);
geometry_msgs::Twist speed_msg;
speed_msg.linear.x = insgps_frame_.frame.data.data_pack.BodyVelocity_X;
speed_msg.linear.y = insgps_frame_.frame.data.data_pack.BodyVelocity_Y;
speed_msg.linear.z = insgps_frame_.frame.data.data_pack.BodyVelocity_Z;
twist_pub_.publish(speed_msg);
// std::cout << "N: " << insgps_frame_.frame.data.data_pack.Location_North << std::endl;
// std::cout << "E: " << insgps_frame_.frame.data.data_pack.Location_East << std::endl;
// std::cout << "D: " << insgps_frame_.frame.data.data_pack.Location_Down << std::endl;
}
}
}
void ahrsBringup::magCalculateYaw(double roll, double pitch, double &magyaw, double magx, double magy, double magz)
{
double temp1 = magy * cos(roll) + magz * sin(roll);
double temp2 = magx * cos(pitch) + magy * sin(pitch) * sin(roll) - magz * sin(pitch) * cos(roll);
magyaw = atan2(-temp1, temp2);
if(magyaw < 0)
{
magyaw = magyaw + 2 * PI;
}
// return magyaw;
}
void ahrsBringup::checkSN(int type)
{
switch (type)
{
case TYPE_IMU:
if (++read_sn_ != imu_frame_.frame.header.serial_num)
{
if ( imu_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - imu_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(imu_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = imu_frame_.frame.header.serial_num;
break;
case TYPE_AHRS:
if (++read_sn_ != ahrs_frame_.frame.header.serial_num)
{
if ( ahrs_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - ahrs_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(ahrs_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = ahrs_frame_.frame.header.serial_num;
break;
case TYPE_INSGPS:
if (++read_sn_ != insgps_frame_.frame.header.serial_num)
{
if ( insgps_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - insgps_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(insgps_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = insgps_frame_.frame.header.serial_num;
break;
case TYPE_GEODETIC_POS:
if (++read_sn_ != Geodetic_Position_frame_.frame.header.serial_num)
{
if ( Geodetic_Position_frame_.frame.header.serial_num < read_sn_)
{
sn_lost_ += 256 - (int)(read_sn_ - Geodetic_Position_frame_.frame.header.serial_num);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
else
{
sn_lost_ += (int)(Geodetic_Position_frame_.frame.header.serial_num - read_sn_);
if(if_debug_){
ROS_WARN("detected sn lost.");
}
}
}
read_sn_ = Geodetic_Position_frame_.frame.header.serial_num;
break;
default:
break;
}
}
} //namespace FDILink
int main(int argc, char **argv)
{
ros::init(argc, argv, "ahrs_bringup");
FDILink::ahrsBringup bp;
return 0;
}
sensors/fdilink_ahrs/src/crc_table.cpp
+159
View File
@@ -0,0 +1,159 @@
#include <stdint.h>
#include <crc_table.h>
static const uint8_t CRC8Table[] = {
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53};
static const uint16_t CRC16Table[256] =
{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
static const uint32_t CRC32Table[] = {
0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL,
0x076dc419L, 0x706af48fL, 0xe963a535L, 0x9e6495a3L,
0x0edb8832L, 0x79dcb8a4L, 0xe0d5e91eL, 0x97d2d988L,
0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L, 0x90bf1d91L,
0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L,
0x136c9856L, 0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL,
0x14015c4fL, 0x63066cd9L, 0xfa0f3d63L, 0x8d080df5L,
0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L, 0xa2677172L,
0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L,
0x32d86ce3L, 0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L,
0x26d930acL, 0x51de003aL, 0xc8d75180L, 0xbfd06116L,
0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L, 0xb8bda50fL,
0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL,
0x76dc4190L, 0x01db7106L, 0x98d220bcL, 0xefd5102aL,
0x71b18589L, 0x06b6b51fL, 0x9fbfe4a5L, 0xe8b8d433L,
0x7807c9a2L, 0x0f00f934L, 0x9609a88eL, 0xe10e9818L,
0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL,
0x6c0695edL, 0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L,
0x65b0d9c6L, 0x12b7e950L, 0x8bbeb8eaL, 0xfcb9887cL,
0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L, 0xfbd44c65L,
0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL,
0x4369e96aL, 0x346ed9fcL, 0xad678846L, 0xda60b8d0L,
0x44042d73L, 0x33031de5L, 0xaa0a4c5fL, 0xdd0d7cc9L,
0x5005713cL, 0x270241aaL, 0xbe0b1010L, 0xc90c2086L,
0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L,
0x59b33d17L, 0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL,
0xedb88320L, 0x9abfb3b6L, 0x03b6e20cL, 0x74b1d29aL,
0xead54739L, 0x9dd277afL, 0x04db2615L, 0x73dc1683L,
0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L,
0xf00f9344L, 0x8708a3d2L, 0x1e01f268L, 0x6906c2feL,
0xf762575dL, 0x806567cbL, 0x196c3671L, 0x6e6b06e7L,
0xfed41b76L, 0x89d32be0L, 0x10da7a5aL, 0x67dd4accL,
0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L,
0xd1bb67f1L, 0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL,
0xd80d2bdaL, 0xaf0a1b4cL, 0x36034af6L, 0x41047a60L,
0xdf60efc3L, 0xa867df55L, 0x316e8eefL, 0x4669be79L,
0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL,
0xc5ba3bbeL, 0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L,
0xc2d7ffa7L, 0xb5d0cf31L, 0x2cd99e8bL, 0x5bdeae1dL,
0x9b64c2b0L, 0xec63f226L, 0x756aa39cL, 0x026d930aL,
0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L,
0x92d28e9bL, 0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L,
0x86d3d2d4L, 0xf1d4e242L, 0x68ddb3f8L, 0x1fda836eL,
0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L, 0x18b74777L,
0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L,
0xa00ae278L, 0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L,
0xa7672661L, 0xd06016f7L, 0x4969474dL, 0x3e6e77dbL,
0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L, 0x37d83bf0L,
0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L,
0xbad03605L, 0xcdd70693L, 0x54de5729L, 0x23d967bfL,
0xb3667a2eL, 0xc4614ab8L, 0x5d681b02L, 0x2a6f2b94L,
0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL, 0x2d02ef8dL
};
uint8_t CRC8_Table(uint8_t* p, uint8_t counter)
{
uint8_t crc8 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
uint8_t new_index = crc8 ^ value;
crc8 = CRC8Table[new_index];
}
return (crc8);
}
uint16_t CRC16_Table(uint8_t *p, uint8_t counter)
{
uint16_t crc16 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
crc16 = CRC16Table[((crc16 >> 8) ^ value) & 0xff] ^ (crc16 << 8);
}
return (crc16);
}
uint32_t CRC32_Table(uint8_t *p, uint8_t counter)
{
uint16_t crc32 = 0;
for (int i = 0; i < counter; i++)
{
uint8_t value = p[i];
crc32 = CRC16Table[((crc32 >> 8) ^ value) & 0xff] ^ (crc32 << 8);
}
return (crc32);
}
sensors/fdilink_ahrs/src/imu_tf.cpp
+53
View File
@@ -0,0 +1,53 @@
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <string>
/* 参考ROS wiki
* http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
* */
int position_x ;
int position_y ;
int position_z ;
std::string imu_frame_id, world_frame_id;
void ImuCallback(const sensor_msgs::ImuConstPtr& imu_data) {
static tf::TransformBroadcaster br;//广播器
tf::Transform transform;
transform.setOrigin(tf::Vector3(position_x, position_y, position_z));//设置平移部分
//从IMU消息包中获取四元数数据
tf::Quaternion q;
q.setX(imu_data->orientation.x);
q.setY(imu_data->orientation.y);
q.setZ(imu_data->orientation.z);
q.setW(imu_data->orientation.w);
q.normalized();//归一化
transform.setRotation(q);//设置旋转部分
//广播出去
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), world_frame_id, imu_frame_id));
}
int main (int argc, char ** argv) {
ros::init(argc, argv, "imu_data_to_tf");
ros::NodeHandle node;
std::string imu_topic;
node.param("/imu_tf/imu_topic", imu_topic, std::string("/imu"));
node.param("/imu_tf/position_x", position_x, 0);
node.param("/imu_tf/position_y", position_y, 0);
node.param("/imu_tf/position_z", position_z, 0);
node.param("/imu_tf/world_frame_id", world_frame_id, std::string("/world"));
node.param("/imu_tf/imu_frame_id", imu_frame_id, std::string("/imu"));
ros::Subscriber sub = node.subscribe(imu_topic.c_str(), 10, &ImuCallback);
ros::spin();
return 0;
}