Initial commit
This commit is contained in:
@@ -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
|
||||
@@ -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
|
||||
@@ -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_
|
||||
Reference in New Issue
Block a user