更新串口读取imu模块代码

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_data.py

@@ -9,7 +9,6 @@
 # History:
 #    20191031: Initial this file.
 #    20191209： Add get imu chip temperature function-get_temp().
-
 import smbus
 import rospy
 import numpy as np
@@ -28,6 +27,9 @@ class MyIMU(object):
             ax_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x34, 2)
             ay_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x35, 2)
             az_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x36, 2)
+            #rospy.loginfo("Read accX:" + str(ax_tmp))
+            #rospy.loginfo("Read accY:" + str(ay_tmp))
+            #rospy.loginfo("Read accZ:" + str(az_tmp))
 
             gx_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x37, 2)
             gy_tmp  = self.i2c.read_i2c_block_data(self.addr, 0x38, 2)

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/CMakeLists.txt

@@ -128,12 +128,12 @@ include_directories(
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/proc_serial_data.cpp src/serial_imu_node.cpp)
+add_executable(serial_imu_node src/proc_serial_data.cpp src/serial_imu_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -146,9 +146,9 @@ add_executable(${PROJECT_NAME}_node src/proc_serial_data.cpp src/serial_imu_node
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+target_link_libraries(serial_imu_node
+  ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/include/imu_data.h

@@ -1,45 +1,22 @@
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
 
-#define ACC_X_REG  0x34
-#define ACC_Y_REG  0x35
-#define ACC_Z_REG  0x36
+int initSerialPort(const char* path, const int baud, const int dataBits,
+        const char* parity, const int stopBit);
 
-#define GYO_X_REG  0x37
-#define GYO_Y_REG  0x38
-#define GYO_Z_REG  0x39
+int getImuData();
+int closeSerialPort();
 
-#define ROLL_REG   0x3d
-#define PITCH_REG  0x3e
-#define YAW_REG    0x3f
+float getAccX();
+float getAccY();
+float getAccZ();
 
-class myIMU
-{
-    public:
-        float getRollData();
-        float getPitchData();
-        float getYawData();
+float getAngularX();
+float getAngularY();
+float getAngularZ();
 
-        float getAccXData();
-        float getAccYData();
-        float getAccZData();
+float getAngleX();
+float getAngleY();
+float getAngleZ();
 
-        float getGyoXData();
-        float getGyoYData();
-        float getGyoZData();
-
-    private:
-        unsigned char accX[2];
-        unsigned char accY[2];
-        unsigned char accZ[2];
-
-        unsigned char gyoX[2];
-        unsigned char gyoY[2];
-        unsigned char gyoZ[2];
-
-        unsigned char roll[2];
-        unsigned char pitch[2];
-        unsigned char yaw[2];
-};
 #endif
-

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/launch/serial_imu_hat.launch

@@ -6,9 +6,9 @@
     20200401: init this file.
 -->
 <launch>
-  <arg name="imu_cfg_file" default="$(find serial_imu_hat_6dof/cfg/param.yaml)" />
+  <arg name="imu_cfg_file" default="$(find serial_imu_hat_6dof)/cfg/param.yaml" />
 
-  <node pkg="serial_imu_hat_6dof" type="" name="" output="screen">
+  <node pkg="serial_imu_hat_6dof" type="serial_imu_node" name="serial_imu_node" output="screen">
     <rosparam file="$(arg imu_cfg_file)" command="load" />
   </node>
 </launch>

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -5,6 +5,7 @@
  * History:
  *   20200401:init this file.
  *************************************************************************/
+#include<ros/ros.h>
 #include<stdio.h>
 #include<stdlib.h>
 #include<fcntl.h>
@@ -14,39 +15,33 @@
 #include<string.h>
 #include<sys/time.h>
 #include<sys/types.h>
-#include<errno.h>
 
-
-#define  BAUD  9600 //9600 default baund for JY61P
+char r_buf[512];
+int port_fd = 0;
+float acce[3],angular[3],angle[3],quater[4];
 
 /**********************************************
- * Description:打开串口,输入设备号参数即可.
+ * Description:打开串口,输入各参数即可.
  *********************************************/
-int open_serialPort(const char *path)
+int openSerialPort(const char *path, int baud, int dataBits,
+   const char* parity, int stopBit)
 {
-    int fd = open(path, O_RDWR|O_NOCTTY);
+    int fd = 0;
+    struct termios terminfo;
+    bzero(&terminfo, sizeof(terminfo));
+
+    fd = open(path, O_RDWR|O_NOCTTY);
     if (-1 == fd)
     {
-        return(-1);
+        ROS_ERROR("Open imu dev error:%s", path);
+        return -1;
     }
 
     if(isatty(STDIN_FILENO) == 0)
     {
-        return(-2);
+        ROS_ERROR("IMU dev isatty error !");
+        return -2;
     }
-    else
-    {
-        return fd;
-    }
-}
-
-/************************************************************************
- * Description:配置可以与串口进行正常数据通信的参数.
- ***********************************************************************/
-int set_serialPort(int fd, int baud, int dataBits, char parity, int stopBits)
-{
-    struct termios terminfo;
-    bzero(&terminfo, sizeof(terminfo));
 
     /*设置串口通信波特率-bps*/
     switch(baud)
@@ -96,12 +91,12 @@ int set_serialPort(int fd, int baud, int dataBits, char parity, int stopBits)
            break;
 
         default:
-           printf("Set dataBits error !\n");
-           return -1;
+           ROS_ERROR("set dataBit error !");
+           return -3;
     }
 
     //设置奇偶校验位
-    switch(parity)
+    switch(*parity)
     {
         case 'o': //设置奇校验
         case 'O':
@@ -123,12 +118,12 @@ int set_serialPort(int fd, int baud, int dataBits, char parity, int stopBits)
            break;
 
         default:
-          printf("set parity error !\n");
-          return -2;
+           ROS_ERROR("set parity error !");
+          return -4;
     }
 
     //设置停止位
-    switch(stopBits)
+    switch(stopBit)
     {
         case 1:
             terminfo.c_cflag &= ~CSTOPB;
@@ -139,8 +134,8 @@ int set_serialPort(int fd, int baud, int dataBits, char parity, int stopBits)
             break;
 
         default:
-            printf("set stopBits error !\n");
-            return -3;
+            ROS_ERROR("set stopBit error !");
+            return -5;
     }
 
     terminfo.c_cc[VTIME] = 10;
@@ -149,19 +144,19 @@ int set_serialPort(int fd, int baud, int dataBits, char parity, int stopBits)
 
     if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
     {
-      printf("config serialPort params error !\n");
-      return -4;
+        ROS_ERROR("set imu serial port attr error !");
+        return -6;
     }
 
-    return 0;
+    return fd;
 }
 
 /**************************************
  * Description:关闭串口文件描述符.
  *************************************/
-int close_serialPort(int fd)
+int closeSerialPort()
 {
-    int ret = close(fd);
+    int ret = close(port_fd);
     return ret;
 }
 
@@ -184,15 +179,15 @@ int recv_data(int fd, char* recv_buffer, int len)
     return length;
 }
 
-float a[3], w[3], Angle[3], q[4];
+
 /************************************************************
  * Description:根据串口数据协议来解析有效数据,
  ***********************************************************/
 void parse_serialData(char chr)
 {
-    static char chrBuf[100];
+    static unsigned char chrBuf[100];
     static unsigned char chrCnt = 0;
-    signed short sData[4]; //存储8字节的有效信息
+    signed short sData[4]; //save 8 Byte valid info
     unsigned char i = 0;
     char frameSum = 0;
 
@@ -217,88 +212,118 @@ void parse_serialData(char chr)
         return;
     }
 
+    //for(i=0;i<11; i++)
+    //  printf("0x%x ",chrBuf[i]);
+    //printf("\n");
+
     memcpy(&sData[0], &chrBuf[2], 8);
     switch(chrBuf[1])
     {
         case 0x51: //加速度输出
-            for(i=0; i<3; i++)
-            {
-                a[i] = (float)sData[i]/32768.0*16.0;
-            }
-            printf("a:%6.4f %6.4f %6.4f ", a[0], a[1], a[2]);
+            //ROS_INFO("AccX:%d %d",chrBuf[2], chrBuf[3]);
+            //ROS_INFO("AccY:%d %d",chrBuf[4], chrBuf[5]);
+            acce[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*16.0;
+            acce[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*16.0;
+            acce[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*16.0;
             break;
 
         case 0x52: //角速度输出
-            for(i=0; i<3; i++)
-            {
-                w[i] = (float)sData[i]/32768.0*2000.0;
-            }
-            printf("w:%7.4f %7.4f %7.4f ", w[0], w[1], w[2]);
+            angular[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*2000.0;
+            angular[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*2000.0;
+            angular[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*2000.0;
             break;
 
         case 0x53: //角度输出, roll, pitch, yaw
-            for (i=0; i<3; i++)
-            {
-                Angle[i] = (float)sData[i]/32768.0*180.0;
-            }
-            printf("A:%7.4f %7.4f %7.4f ", Angle[0], Angle[1], Angle[2]);
+            angle[0] = (short)(((short)chrBuf[3]<<8)|chrBuf[2])/32768.0*180.0;
+            angle[1] = (short)(((short)chrBuf[5]<<8)|chrBuf[4])/32768.0*180.0;
+            angle[2] = (short)(((short)chrBuf[7]<<8)|chrBuf[6])/32768.0*180.0;
             break;
 
         case 0x59: //四元素输出
-            for (i=0; i<4; i++)
-            {
-                q[i] = (float)sData[i]/32768.0;
-            }
-            printf("q:%6.4f %6.4f %6.4f %6.4f\n", q[0], q[1], q[2], q[3]);
+            quater[0] = (((short)chrBuf[3]<<8)|chrBuf[2])/32768.0;
+            quater[1] = (((short)chrBuf[5]<<8)|chrBuf[4])/32768.0;
+            quater[2] = (((short)chrBuf[7]<<8)|chrBuf[6])/32768.0;
+            quater[3] = (((short)chrBuf[9]<<8)|chrBuf[8])/32768.0;
             break;
     }
     chrCnt = 0;
 }
 
-int main(void)
+/*****************************************************************
+ * Description:根据串口配置信息来配置打开串口,准备进行数据通信
+ ****************************************************************/
+int initSerialPort(const char* path, const int baud, const int dataBits,
+          const char* parity, const int stopBit)
 {
-    int ret = 0;
-    int port_fd = 0;
-    char r_buf[512];
     bzero(r_buf, 512);
-    std::string imu_dev;
-
-    port_fd = open_serialPort("/dev/ttyUSB0");
+    port_fd = openSerialPort(path, baud, dataBits, parity, stopBit);
     if(port_fd < 0)
     {
-        printf("open_serialPort error !\n");
-        exit(EXIT_FAILURE);
+        ROS_ERROR("openSerialPort error !");
+        return -1;
     }
 
-    if(set_serialPort(port_fd, BAUD, 8, 'N', 1) < 0)
-    {
-        printf("set serialPort failed !\n");
-        exit(EXIT_FAILURE);
-    }
+    return 0;
+}
 
-    while(1)
+float getAccX()
+{
+    return acce[0];
+}
+float getAccY()
+{
+    return acce[1];
+}
+float getAccZ()
+{
+    return acce[2];
+}
+
+float getAngularX()
+{
+    return angular[0];
+}
+
+float getAngularY()
+{
+    return angular[1];
+}
+
+float getAngularZ()
+{
+    return angular[2];
+}
+
+float getAngleX()
+{
+    return angle[0];
+}
+float getAngleY()
+{
+    return angle[1];
+}
+float getAngleZ()
+{
+    return angle[2];
+}
+
+int getImuData()
+{
+    int ret = 0;
+    ret = recv_data(port_fd, r_buf,  44);
+    if(ret == -1)
     {
-        ret = recv_data(port_fd, r_buf,  44);
-        if(ret == -1)
-        {
-            printf("read serial port data failed !\n");
-            exit(EXIT_FAILURE);
-        }
-
-        for (int i=0; i<ret; i++)
-        {
-            parse_serialData(r_buf[i]);
-        }
-        usleep(1000);
+        ROS_ERROR("read serial port data failed !\n");
+        closeSerialPort();
+
+        return -1;
     }
 
-    ret = close_serialPort(port_fd);
-    if(ret == -1)
+    for (int i=0; i<ret; i++)
     {
-        printf("close serialPort error !\n");
-        exit(EXIT_FAILURE);
+        parse_serialData(r_buf[i]);
     }
 
-    exit(EXIT_SUCCESS);
+    return 0;
 }
 

src/rasp_imu_hat_6dof/serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,71 +1,83 @@
 #include <ros/ros.h>
+#include <tf/tf.h>
 #include "imu_data.h"
-#include <Eigen/Eigen>
-#include <Eigen/Geometry>
-#include <Eigen/Core>
 #include <sensor_msgs/Imu.h>
 
 
-Eigen::Quaterniond euler2Quater(float roll, float yaw, float pitch)
-{
-    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
-    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
-    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());
-
-    Eigen::Quaterniond q = rollAngle * yawAngle * pitchAngle;
-    cout << "Euler2Quaternion result is:" <<endl;
-    cout << "x = " << q.x() <<endl;
-    cout << "y = " << q.y() <<endl;
-    cout << "z = " << q.z() <<endl;
-    cout << "w = " << q.w() <<endl<<endl;
-
-    return q;
-}
-
 int main(int argc, char **argv)
 {
+    float yaw, pitch, roll;
+    tf::Quaternion q;
     std::string imu_dev;
-    myImu my_imu;
+    int baud = 0;
+    int dataBit = 0;
+    std::string parity;
+    int stopBit = 0;
+
+    std::string link_name;
+    std::string imu_topic;
+    std::string temp_topic;
+    int pub_hz;
+    float degree2Rad = 3.1415926/180.0;
+    float acc_factor= 9.806/256.0;
 
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
 
-    imu_dev = handle.getParam("~imu_dev", "imu_dev");
-    rospy.loginfo("imu_dev:%s", imu_dev);
+    ros::param::get("~imu_dev", imu_dev);
+    ros::param::get("~baud_rate", baud);
+    ros::param::get("~data_bits", dataBit);
+    ros::param::get("~parity", parity);
+    ros::param::get("~stop_bits", stopBit);
 
-    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>("imu_data", 2);
-    ros::Rate loop_rate(10);
-    sensor_msgs::Imu imu_msg;
-    float yaw_tmp,pitch_tmp,roll_tmp;
+    ros::param::get("~link_name", link_name);
+    ros::param::get("~pub_data_topic", imu_topic);
+    ros::param::get("~pub_temp_topic", temp_topic);
+    ros::param::get("~pub_hz", pub_hz);
+
+    int ret = initSerialPort(imu_dev.c_str(), baud, dataBit, parity.c_str(), stopBit);
+    if(ret < 0)
+    {
+        ROS_ERROR("init serial port error !");
+        return -1;
+    }
+
+    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 2);
+    ros::Rate loop_rate(pub_hz);
 
+    sensor_msgs::Imu imu_msg;
+    imu_msg.header.frame_id = link_name;
     while(ros::ok())
     {
-        imu_msg.header.stamp = ros::Time::now();
-        imu_msg.header.frame_id = "base_link";
+        if(getImuData() < 0)
+            break;
 
-        roll_tmp  = my_imu.getRollData();
-        pitch_tmp = my_imu.getPitchData();
-        yaw_tmp   = my_imu.getYawData();
+        imu_msg.header.stamp = ros::Time::now();
+        roll  = getAngleX();
+        pitch = getAngleY();
+        yaw   = getAngleZ();
+        if(yaw >= 180.0)
+            yaw -= 360.0;
 
-        Eigen::Quaterniond q = euler2Quater(roll_tmp, pitch_tmp, yaw_tmp);
-        imu_msg.orientation.x = q.x();
-        imu_msg.orientation.y = q.y();
-        imu_msg.orientation.z = q.z();
-        imu_msg.orientation.w = q.w();
+        q.setRPY(roll, pitch, yaw);
+        imu_msg.orientation.x = q[0];
+        imu_msg.orientation.y = q[1];
+        imu_msg.orientation.z = q[2];
+        imu_msg.orientation.w = q[3];
         imu_msg.orientation_covariance = {0.0025, 0, 0,
                                           0, 0.0025, 0,
                                           0, 0, 0.0025};
 
-        imu_msg.angular_velocity.x = my_imu.getGyoXData();
-        imu_msg.angular_velocity.y = my_imu.getGyoYData();
-        imu_msg.angular_velocity.z = my_imu.getGyoZData();
+        imu_msg.angular_velocity.x = getAngularX()*degree2Rad;
+        imu_msg.angular_velocity.y = getAngularY()*degree2Rad;
+        imu_msg.angular_velocity.z = getAngularZ()*degree2Rad;
         imu_msg.angular_velocity_covariance = {0.02, 0, 0,
                                                0, 0.02, 0,
                                                0, 0, 0.02};
 
-        imu_msg.linear_acceleration.x = my_imu.getAccXData();
-        imu_msg.linear_acceleration.y = my_imu.getAccYData();
-        imu_msg.linear_acceleration.z = my_imu.getAccZData();
+        imu_msg.linear_acceleration.x = getAccX()*acc_factor;
+        imu_msg.linear_acceleration.y = getAccY()*acc_factor;
+        imu_msg.linear_acceleration.z = getAccZ()*acc_factor;
         imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
                                                    0, 0.04, 0,
                                                    0, 0, 0.04};
@@ -75,5 +87,8 @@ int main(int argc, char **argv)
         loop_rate.sleep();
     }
 
+    closeSerialPort();
+
     return 0;
 }
+