├── CMakeLists.txt ├── CMakeLists.txt~ ├── README.md ├── jy901.png ├── launch ├── imu_node.launch └── imu_node.launch~ ├── package.xml ├── package.xml~ └── src ├── JY901.h ├── JY901.h~ ├── imu_decoder.cpp └── imu_decoder.cpp~ /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | message_generation 9 | dynamic_reconfigure 10 | rosconsole 11 | roscpp_serialization 12 | roslib 13 | rostime 14 | sensor_msgs 15 | serial 16 | ) 17 | 18 | 19 | 20 | #add_message_files(FILES 21 | # JY901_frame.msg 22 | # Mile.msg 23 | # Control.msg 24 | #) 25 | 26 | #generate_messages(DEPENDENCIES 27 | # std_msgs 28 | #) 29 | 30 | catkin_package( 31 | INCLUDE_DIRS src 32 | CATKIN_DEPENDS serial roscpp sensor_msgs tf 33 | DEPENDS system_lib 34 | message_runtime 35 | ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | add_executable(imu_node 43 | src/imu_decoder.cpp 44 | ) 45 | 46 | target_link_libraries(imu_node 47 | #serial 48 | ${catkin_LIBRARIES} 49 | ) 50 | 51 | -------------------------------------------------------------------------------- /CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(imu_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | message_generation 9 | dynamic_reconfigure 10 | rosconsole 11 | roscpp_serialization 12 | roslib 13 | rostime 14 | sensor_msgs 15 | serial 16 | ) 17 | 18 | 19 | 20 | #add_message_files(FILES 21 | # JY901_frame.msg 22 | # Mile.msg 23 | # Control.msg 24 | #) 25 | 26 | #generate_messages(DEPENDENCIES 27 | # std_msgs 28 | #) 29 | 30 | catkin_package( 31 | INCLUDE_DIRS src 32 | CATKIN_DEPENDS serial roscpp sensor_msgs tf 33 | DEPENDS system_lib 34 | message_runtime 35 | ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | add_executable(imu_node 43 | src/imu_decoder.cpp 44 | src/JY901.cpp 45 | src/Com.cpp 46 | ) 47 | target_link_libraries(imu_node 48 | #serial 49 | ${catkin_LIBRARIES} 50 | ) 51 | 52 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Screenshot](/jy901.png) 2 | 3 | # JY901 series IMU ros driver package 4 | 5 | ## Build 6 | Need ros serial 7 | ``` 8 | sudo apt-get install ros--serial 9 | ``` 10 | don't forget to adjust CMakeLists.txt and package.xml 11 | 12 | ## I/O 13 | Input: serial port data of imu. 14 | 15 | Output: imu data in ros message type [sensor_msgs::Imu](http://docs.ros.org/jade/api/sensor_msgs/html/msg/Imu.html). 16 | 17 | ## Notes 18 | Orientation in quaternion form. 19 | 20 | Angular velocity in rad/s. 21 | 22 | About transform between Euler and Quaternion form, see [here](http://blog.csdn.net/sun19890716/article/details/52104507). 23 | 24 | -------------------------------------------------------------------------------- /jy901.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GuoLindong/imu_driver/87738c4856ec7fd6c9022bc484b701a021932779/jy901.png -------------------------------------------------------------------------------- /launch/imu_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/imu_node.launch~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_driver 4 | 0.1.0 5 | The imu_driver package 6 | 7 | 8 | 9 | 10 | gld 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | roscpp 20 | rospy 21 | std_msgs 22 | rosconsole 23 | roscpp_serialization 24 | rostime 25 | roslib 26 | roscpp 27 | rospy 28 | 29 | gps_common 30 | serial 31 | nav_msgs 32 | roscpp 33 | sensor_msgs 34 | tf 35 | message_generation 36 | 37 | 38 | gps_common 39 | serial 40 | rosconsole 41 | roscpp_serialization 42 | roslib 43 | nav_msgs 44 | roscpp 45 | sensor_msgs 46 | tf 47 | message_runtime 48 | rostime 49 | dynamic_reconfigure 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /package.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_driver 4 | 0.1.0 5 | The imu_driver package 6 | 7 | 8 | 9 | 10 | gld 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | catkin 19 | roscpp 20 | rospy 21 | std_msgs 22 | rosconsole 23 | roscpp_serialization 24 | rostime 25 | roslib 26 | roscpp 27 | rospy 28 | 29 | gps_common 30 | serial 31 | nav_msgs 32 | roscpp 33 | sensor_msgs 34 | tf 35 | message_generation 36 | 37 | 38 | gps_common 39 | cereal_port 40 | rosconsole 41 | roscpp_serialization 42 | roslib 43 | nav_msgs 44 | roscpp 45 | sensor_msgs 46 | tf 47 | message_runtime 48 | rostime 49 | dynamic_reconfigure 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /src/JY901.h: -------------------------------------------------------------------------------- 1 | #ifndef JY901_h 2 | #define JY901_h 3 | 4 | #define SAVE 0x00 5 | #define CALSW 0x01 6 | #define RSW 0x02 7 | #define RRATE 0x03 8 | #define BAUD 0x04 9 | #define AXOFFSET 0x05 10 | #define AYOFFSET 0x06 11 | #define AZOFFSET 0x07 12 | #define GXOFFSET 0x08 13 | #define GYOFFSET 0x09 14 | #define GZOFFSET 0x0a 15 | #define HXOFFSET 0x0b 16 | #define HYOFFSET 0x0c 17 | #define HZOFFSET 0x0d 18 | #define D0MODE 0x0e 19 | #define D1MODE 0x0f 20 | #define D2MODE 0x10 21 | #define D3MODE 0x11 22 | #define D0PWMH 0x12 23 | #define D1PWMH 0x13 24 | #define D2PWMH 0x14 25 | #define D3PWMH 0x15 26 | #define D0PWMT 0x16 27 | #define D1PWMT 0x17 28 | #define D2PWMT 0x18 29 | #define D3PWMT 0x19 30 | #define IICADDR 0x1a 31 | #define LEDOFF 0x1b 32 | #define GPSBAUD 0x1c 33 | 34 | #define YYMM 0x30 35 | #define DDHH 0x31 36 | #define MMSS 0x32 37 | #define MS 0x33 38 | #define AX 0x34 39 | #define AY 0x35 40 | #define AZ 0x36 41 | #define GX 0x37 42 | #define GY 0x38 43 | #define GZ 0x39 44 | #define HX 0x3a 45 | #define HY 0x3b 46 | #define HZ 0x3c 47 | #define Roll 0x3d 48 | #define Pitch 0x3e 49 | #define Yaw 0x3f 50 | #define TEMP 0x40 51 | #define D0Status 0x41 52 | #define D1Status 0x42 53 | #define D2Status 0x43 54 | #define D3Status 0x44 55 | #define PressureL 0x45 56 | #define PressureH 0x46 57 | #define HeightL 0x47 58 | #define HeightH 0x48 59 | #define LonL 0x49 60 | #define LonH 0x4a 61 | #define LatL 0x4b 62 | #define LatH 0x4c 63 | #define GPSHeight 0x4d 64 | #define GPSYAW 0x4e 65 | #define GPSVL 0x4f 66 | #define GPSVH 0x50 67 | 68 | #define DIO_MODE_AIN 0 69 | #define DIO_MODE_DIN 1 70 | #define DIO_MODE_DOH 2 71 | #define DIO_MODE_DOL 3 72 | #define DIO_MODE_DOPWM 4 73 | #define DIO_MODE_GPS 5 74 | 75 | struct STime 76 | { 77 | unsigned char ucYear; 78 | unsigned char ucMonth; 79 | unsigned char ucDay; 80 | unsigned char ucHour; 81 | unsigned char ucMinute; 82 | unsigned char ucSecond; 83 | unsigned short usMiliSecond; 84 | }; 85 | struct SAcc 86 | { 87 | short a[3]; 88 | short T; 89 | }; 90 | struct SGyro 91 | { 92 | short w[3]; 93 | short T; 94 | }; 95 | struct SAngle 96 | { 97 | short Angle[3]; 98 | short T; 99 | }; 100 | struct SMag 101 | { 102 | short h[3]; 103 | short T; 104 | }; 105 | 106 | struct SDStatus 107 | { 108 | short sDStatus[4]; 109 | }; 110 | 111 | struct SPress 112 | { 113 | long lPressure; 114 | long lAltitude; 115 | }; 116 | 117 | struct SLonLat 118 | { 119 | long lLon; 120 | long lLat; 121 | }; 122 | 123 | struct SGPSV 124 | { 125 | short sGPSHeight; 126 | short sGPSYaw; 127 | long lGPSVelocity; 128 | }; 129 | class CJY901 130 | { 131 | public: 132 | struct STime stcTime; 133 | struct SAcc stcAcc; 134 | struct SGyro stcGyro; 135 | struct SAngle stcAngle; 136 | struct SMag stcMag; 137 | struct SDStatus stcDStatus; 138 | struct SPress stcPress; 139 | struct SLonLat stcLonLat; 140 | struct SGPSV stcGPSV; 141 | 142 | CJY901 (); 143 | void CopeSerialData(char ucData[],unsigned short usLength); 144 | }; 145 | //extern CJY901 JY901; 146 | 147 | void CharToLong(char Dest[],char Source[]) 148 | { 149 | *Dest = Source[3]; 150 | *(Dest+1) = Source[2]; 151 | *(Dest+2) = Source[1]; 152 | *(Dest+3) = Source[0]; 153 | } 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /src/JY901.h~: -------------------------------------------------------------------------------- 1 | #ifndef JY901_h 2 | #define JY901_h 3 | 4 | #define SAVE 0x00 5 | #define CALSW 0x01 6 | #define RSW 0x02 7 | #define RRATE 0x03 8 | #define BAUD 0x04 9 | #define AXOFFSET 0x05 10 | #define AYOFFSET 0x06 11 | #define AZOFFSET 0x07 12 | #define GXOFFSET 0x08 13 | #define GYOFFSET 0x09 14 | #define GZOFFSET 0x0a 15 | #define HXOFFSET 0x0b 16 | #define HYOFFSET 0x0c 17 | #define HZOFFSET 0x0d 18 | #define D0MODE 0x0e 19 | #define D1MODE 0x0f 20 | #define D2MODE 0x10 21 | #define D3MODE 0x11 22 | #define D0PWMH 0x12 23 | #define D1PWMH 0x13 24 | #define D2PWMH 0x14 25 | #define D3PWMH 0x15 26 | #define D0PWMT 0x16 27 | #define D1PWMT 0x17 28 | #define D2PWMT 0x18 29 | #define D3PWMT 0x19 30 | #define IICADDR 0x1a 31 | #define LEDOFF 0x1b 32 | #define GPSBAUD 0x1c 33 | 34 | #define YYMM 0x30 35 | #define DDHH 0x31 36 | #define MMSS 0x32 37 | #define MS 0x33 38 | #define AX 0x34 39 | #define AY 0x35 40 | #define AZ 0x36 41 | #define GX 0x37 42 | #define GY 0x38 43 | #define GZ 0x39 44 | #define HX 0x3a 45 | #define HY 0x3b 46 | #define HZ 0x3c 47 | #define Roll 0x3d 48 | #define Pitch 0x3e 49 | #define Yaw 0x3f 50 | #define TEMP 0x40 51 | #define D0Status 0x41 52 | #define D1Status 0x42 53 | #define D2Status 0x43 54 | #define D3Status 0x44 55 | #define PressureL 0x45 56 | #define PressureH 0x46 57 | #define HeightL 0x47 58 | #define HeightH 0x48 59 | #define LonL 0x49 60 | #define LonH 0x4a 61 | #define LatL 0x4b 62 | #define LatH 0x4c 63 | #define GPSHeight 0x4d 64 | #define GPSYAW 0x4e 65 | #define GPSVL 0x4f 66 | #define GPSVH 0x50 67 | 68 | #define DIO_MODE_AIN 0 69 | #define DIO_MODE_DIN 1 70 | #define DIO_MODE_DOH 2 71 | #define DIO_MODE_DOL 3 72 | #define DIO_MODE_DOPWM 4 73 | #define DIO_MODE_GPS 5 74 | 75 | struct STime 76 | { 77 | unsigned char ucYear; 78 | unsigned char ucMonth; 79 | unsigned char ucDay; 80 | unsigned char ucHour; 81 | unsigned char ucMinute; 82 | unsigned char ucSecond; 83 | unsigned short usMiliSecond; 84 | }; 85 | struct SAcc 86 | { 87 | short a[3]; 88 | short T; 89 | }; 90 | struct SGyro 91 | { 92 | short w[3]; 93 | short T; 94 | }; 95 | struct SAngle 96 | { 97 | short Angle[3]; 98 | short T; 99 | }; 100 | struct SMag 101 | { 102 | short h[3]; 103 | short T; 104 | }; 105 | 106 | struct SDStatus 107 | { 108 | short sDStatus[4]; 109 | }; 110 | 111 | struct SPress 112 | { 113 | long lPressure; 114 | long lAltitude; 115 | }; 116 | 117 | struct SLonLat 118 | { 119 | long lLon; 120 | long lLat; 121 | }; 122 | 123 | struct SGPSV 124 | { 125 | short sGPSHeight; 126 | short sGPSYaw; 127 | long lGPSVelocity; 128 | }; 129 | class CJY901 130 | { 131 | public: 132 | struct STime stcTime; 133 | struct SAcc stcAcc; 134 | struct SGyro stcGyro; 135 | struct SAngle stcAngle; 136 | struct SMag stcMag; 137 | struct SDStatus stcDStatus; 138 | struct SPress stcPress; 139 | struct SLonLat stcLonLat; 140 | struct SGPSV stcGPSV; 141 | 142 | CJY901 (); 143 | void CopeSerialData(char ucData[],unsigned short usLength); 144 | }; 145 | //extern CJY901 JY901; 146 | 147 | void CharToLong(char Dest[],char Source[]) 148 | { 149 | *Dest = Source[3]; 150 | *(Dest+1) = Source[2]; 151 | *(Dest+2) = Source[1]; 152 | *(Dest+3) = Source[0]; 153 | } 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /src/imu_decoder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ros driver node for JY901 imu 3 | * guolindong@gmail.com 4 | * 2018.01.23 5 | */ 6 | 7 | #define Pi 3.14159265359 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "std_msgs/Float64.h" 14 | #include "std_msgs/Int8.h" 15 | #include "std_msgs/Int32.h" 16 | #include "std_msgs/Int8MultiArray.h" 17 | #include "std_msgs/Int32MultiArray.h" 18 | #include "std_msgs/Float64MultiArray.h" 19 | 20 | #include "sensor_msgs/Imu.h" 21 | 22 | #include "JY901.h" 23 | 24 | using namespace std; 25 | 26 | struct STime stcTime; 27 | struct SAcc stcAcc; 28 | struct SGyro stcGyro; 29 | struct SAngle stcAngle; 30 | struct SMag stcMag; 31 | struct SDStatus stcDStatus; 32 | struct SPress stcPress; 33 | struct SLonLat stcLonLat; 34 | struct SGPSV stcGPSV; 35 | 36 | 37 | //convert serial data to jy901 data 38 | void CopeSerialData(std::string str_in) 39 | { 40 | unsigned int str_length = str_in.size(); 41 | static unsigned char chrTemp[2000]; 42 | static unsigned char ucRxCnt = 0; 43 | static unsigned int usRxLength = 0; 44 | 45 | memcpy(chrTemp,str_in.data(),str_length); 46 | usRxLength += str_length; 47 | while (usRxLength >= 11) 48 | { 49 | if (chrTemp[0] != 0x55) 50 | { 51 | usRxLength--; 52 | memcpy(&chrTemp[0],&chrTemp[1],usRxLength); 53 | continue; 54 | } 55 | switch(chrTemp[1]) 56 | { 57 | case 0x50: memcpy(&stcTime,&chrTemp[2],8);break; 58 | case 0x51: memcpy(&stcAcc,&chrTemp[2],8);break; 59 | case 0x52: memcpy(&stcGyro,&chrTemp[2],8);break; 60 | case 0x53: memcpy(&stcAngle,&chrTemp[2],8);break; 61 | case 0x54: memcpy(&stcMag,&chrTemp[2],8);break; 62 | case 0x55: memcpy(&stcDStatus,&chrTemp[2],8);break; 63 | case 0x56: memcpy(&stcPress,&chrTemp[2],8);break; 64 | case 0x57: memcpy(&stcLonLat,&chrTemp[2],8);break; 65 | case 0x58: memcpy(&stcGPSV,&chrTemp[2],8);break; 66 | } 67 | usRxLength -= 11; 68 | memcpy(&chrTemp[0],&chrTemp[11],usRxLength); 69 | } 70 | } 71 | 72 | //EulerToQuaternion, euler in rad 73 | template 74 | vector EulerToQuaternion(T roll, T pitch, T yaw) 75 | { 76 | vector q; 77 | double x, y, z, w; 78 | double a = roll/2.0; 79 | double b = pitch/2.0; 80 | double g = yaw/2.0; 81 | w = cos(a)*cos(b)*cos(g) + sin(a)*sin(b)*sin(g); 82 | x = sin(a)*cos(b)*cos(g) - cos(a)*sin(b)*sin(g); 83 | y = cos(a)*sin(b)*cos(g) + sin(a)*cos(b)*sin(g); 84 | z = cos(a)*cos(b)*sin(g) - sin(a)*sin(b)*cos(g); 85 | q.push_back(w); 86 | q.push_back(x); 87 | q.push_back(y); 88 | q.push_back(z); 89 | return q; 90 | } 91 | 92 | //QuaternionToEuler, euler in rad 93 | template 94 | vector QuaternionToEuler(vector q) 95 | { 96 | vector e; 97 | T roll = atan2(2*(q[0]*q[1]+q[2]*q[3]),1-2*(q[1]*q[1]+q[2]*q[2])); 98 | T pitch = asin(2*(q[0]*q[2]-q[1]*q[3])); 99 | T yaw = atan2(2*(q[0]*q[3]+q[1]*q[2]),1-2*(q[2]*q[2]+q[3]*q[3])); 100 | e.push_back(roll); 101 | e.push_back(pitch); 102 | e.push_back(yaw); 103 | return e; 104 | } 105 | 106 | 107 | int main (int argc, char** argv) 108 | { 109 | //param 110 | serial::Serial serial_port; 111 | std::string port; 112 | int baudrate; 113 | int looprate; 114 | 115 | //ros init 116 | ros::init(argc, argv, "imu_node"); 117 | ros::NodeHandle nh; 118 | ros::NodeHandle pnh("~"); 119 | 120 | //get param from launch file 121 | pnh.param("baudrate", baudrate, 115200); 122 | pnh.param("port", port, "/dev/ttyUSB0"); 123 | pnh.param("looprate", looprate, 100); 124 | 125 | pnh.getParam("baudrate",baudrate); 126 | pnh.getParam("port",port); 127 | pnh.getParam("looprate", looprate); 128 | 129 | //ros pub and sub 130 | ros::Publisher imu_pub = nh.advertise("imu", 1000); 131 | 132 | try 133 | { 134 | serial_port.setPort(port); 135 | serial_port.setBaudrate(baudrate); 136 | serial::Timeout to = serial::Timeout::simpleTimeout(1000); 137 | serial_port.setTimeout(to); 138 | serial_port.open(); 139 | } 140 | catch (serial::IOException& e) 141 | { 142 | ROS_ERROR_STREAM("Unable to open serial port "); 143 | return -1; 144 | } 145 | 146 | //check if serial port is open 147 | if(serial_port.isOpen()) 148 | { 149 | ROS_INFO_STREAM("Serial Port initialized"); 150 | } 151 | else 152 | { 153 | return -1; 154 | } 155 | 156 | //set looprate 157 | ros::Rate loop_rate(looprate); 158 | while(ros::ok()) 159 | { 160 | if(serial_port.available()){ 161 | //convert serial string to JY901 data 162 | CopeSerialData(serial_port.read(serial_port.available())); 163 | 164 | vector quaternion = EulerToQuaternion( 165 | stcAngle.Angle[0]/32768*Pi, 166 | stcAngle.Angle[1]/32768*Pi, 167 | stcAngle.Angle[2]/32768*Pi); 168 | 169 | //vector euler = QuaternionToEuler(quaternion); 170 | //ROS_INFO("%.8f\t%.8f\t%.8f", euler[0],euler[1],euler[2]); 171 | 172 | //imu sensor msg pub 173 | sensor_msgs::Imu imu_msg; 174 | imu_msg.header.stamp = ros::Time::now(); 175 | imu_msg.header.frame_id = "imu_link"; 176 | imu_msg.orientation.w = quaternion[0]; 177 | imu_msg.orientation.x = quaternion[1]; 178 | imu_msg.orientation.y = quaternion[2]; 179 | imu_msg.orientation.z = quaternion[3]; 180 | imu_msg.linear_acceleration.x = (float)stcAcc.a[0]/32768*16.0*9.8; 181 | imu_msg.linear_acceleration.y = (float)stcAcc.a[1]/32768*16.0*9.8; 182 | imu_msg.linear_acceleration.z = (float)stcAcc.a[2]/32768*16.0*9.8; 183 | imu_msg.angular_velocity.x = ((float)stcGyro.w[0]/32768*2000)/180.0*Pi; 184 | imu_msg.angular_velocity.y = ((float)stcGyro.w[1]/32768*2000)/180.0*Pi; 185 | imu_msg.angular_velocity.z = ((float)stcGyro.w[2]/32768*2000)/180.0*Pi; 186 | imu_pub.publish(imu_msg); 187 | } 188 | 189 | ros::spinOnce(); 190 | loop_rate.sleep(); 191 | } 192 | } 193 | -------------------------------------------------------------------------------- /src/imu_decoder.cpp~: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "imu_driver/JY901_frame.h" 4 | #include "imu_driver/JY901.h" 5 | #include "std_msgs/Float64.h" 6 | #include "std_msgs/Int8.h" 7 | #include "std_msgs/Int32.h" 8 | #include "std_msgs/Int8MultiArray.h" 9 | #include "std_msgs/Int32MultiArray.h" 10 | #include "std_msgs/Float64MultiArray.h" 11 | 12 | 13 | cereal::CerealPort serial_port; 14 | ros::Publisher gyro_pub; 15 | imu_driver::JY901_frame imu_msg; 16 | 17 | struct STime stcTime; 18 | struct SAcc stcAcc; 19 | struct SGyro stcGyro; 20 | struct SAngle stcAngle; 21 | struct SMag stcMag; 22 | struct SDStatus stcDStatus; 23 | struct SPress stcPress; 24 | struct SLonLat stcLonLat; 25 | struct SGPSV stcGPSV; 26 | 27 | 28 | void *rcvThread(void *arg) 29 | { 30 | char chrBuffer[200]; 31 | unsigned char chrTemp[200]; 32 | while(1){ 33 | serial_port.readBytes(chrBuffer,11,1000); 34 | memcpy(chrTemp,chrBuffer,11); 35 | if(chrTemp[0]!=0x55) { 36 | ros::Duration(0.001).sleep(); 37 | ROS_INFO("can not find start"); 38 | if(!serial_port.flush()) ROS_INFO("flush failed!"); 39 | } 40 | else{ 41 | //ROS_INFO("%x\t%x\t%x\t%x\t%x\t%x\t%x\t%x",chrTemp[1],chrTemp[2],chrTemp[3],chrTemp[4],chrTemp[5],chrTemp[6],chrTemp[7],chrTemp[8]); 42 | switch(chrTemp[1]){ 43 | case 0x50: //Time 44 | stcTime.ucYear = chrTemp[2]; 45 | stcTime.ucMonth = chrTemp[3]; 46 | stcTime.ucDay = chrTemp[4]; 47 | stcTime.ucHour = chrTemp[5]; 48 | stcTime.ucMinute = chrTemp[6]; 49 | stcTime.ucSecond = chrTemp[7]; 50 | stcTime.usMiliSecond=((unsigned short)chrTemp[9]<<8)|chrTemp[8]; 51 | break; 52 | case 0x51: 53 | stcAcc.a[0] = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 54 | stcAcc.a[1] = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 55 | stcAcc.a[2] = ((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 56 | break; 57 | case 0x52: 58 | stcGyro.w[0] = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 59 | stcGyro.w[1] = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 60 | stcGyro.w[2] = ((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 61 | break; 62 | case 0x53: 63 | stcAngle.Angle[0] = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 64 | stcAngle.Angle[1] = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 65 | stcAngle.Angle[2] = ((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 66 | stcAngle.T = ((unsigned short)chrTemp[9]<<8)|chrTemp[8]; 67 | break; 68 | case 0x54: 69 | stcMag.h[0] = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 70 | stcMag.h[1] = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 71 | stcMag.h[2] = ((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 72 | stcAngle.T = ((unsigned short)chrTemp[9]<<8)|chrTemp[8]; 73 | break; 74 | case 0x55: 75 | stcDStatus.sDStatus[0] = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 76 | stcDStatus.sDStatus[1] = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 77 | stcDStatus.sDStatus[2] = ((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 78 | stcDStatus.sDStatus[3] = ((unsigned short)chrTemp[9]<<8)|chrTemp[8]; 79 | break; 80 | case 0x57: 81 | stcLonLat.lLon = ((unsigned short)chrTemp[5]<<24)|((unsigned short)chrTemp[4]<<16)|((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 82 | stcLonLat.lLat = ((unsigned short)chrTemp[9]<<24)|((unsigned short)chrTemp[8]<<16)|((unsigned short)chrTemp[7]<<8)|chrTemp[6]; 83 | break; 84 | case 0x58: 85 | stcGPSV.sGPSHeight = ((unsigned short)chrTemp[3]<<8)|chrTemp[2]; 86 | stcGPSV.sGPSYaw = ((unsigned short)chrTemp[5]<<8)|chrTemp[4]; 87 | CharToLong((char*)&stcGPSV.lGPSVelocity,(char*)&chrTemp[6]); 88 | break; 89 | } 90 | } 91 | 92 | imu_msg.gyro_x = (float)stcGyro.w[0]/32768*2000; 93 | imu_msg.gyro_y = (float)stcGyro.w[1]/32768*2000; 94 | imu_msg.gyro_z = (float)stcGyro.w[2]/32768*2000; 95 | 96 | imu_msg.acc_x = (float)stcAcc.a[0]/32768*16; 97 | imu_msg.acc_y = (float)stcAcc.a[1]/32768*16; 98 | imu_msg.acc_z = (float)stcAcc.a[2]/32768*16; 99 | 100 | imu_msg.angle_x = (float)stcAngle.Angle[0]/32768*180; 101 | imu_msg.angle_y = (float)stcAngle.Angle[1]/32768*180; 102 | imu_msg.angle_z = (float)stcAngle.Angle[2]/32768*180; 103 | 104 | imu_msg.latitute = (double)stcLonLat.lLon/10000000; 105 | imu_msg.longtitute = (double)stcLonLat.lLat/10000000; 106 | 107 | gyro_pub.publish(imu_msg); 108 | } 109 | } 110 | 111 | 112 | 113 | int main(int argc, char** argv) 114 | { 115 | 116 | ros::init(argc, argv, "imu_node"); 117 | std::string portname; 118 | int baudrate; 119 | ros::NodeHandle pn("~"); 120 | ros::NodeHandle nh; 121 | gyro_pub = nh.advertise("Gyro", 30); 122 | pn.param("port", portname, "/dev/ttyUSB0");//ttyUSB1 123 | pn.param("baudrate", baudrate, 115200); 124 | 125 | try{ serial_port.open((char*)portname.c_str(), baudrate); } 126 | catch(cereal::Exception& e) 127 | { 128 | ROS_INFO("unable to open imu com"); 129 | } 130 | 131 | 132 | //Create receive thread 133 | pthread_t rcvThrID; //receive thread ID 134 | int err; 135 | err = pthread_create(&rcvThrID, NULL, rcvThread, NULL); 136 | if (err != 0) 137 | { 138 | ROS_ERROR("unable to create receive thread"); 139 | return -1; 140 | } 141 | 142 | ros::spin(); 143 | return 0; 144 | 145 | } 146 | 147 | 148 | 149 | --------------------------------------------------------------------------------