精简一些获取角速度,加速度,四元素的函数

README.md

@@ -11,6 +11,15 @@
 * 发布yaw角度的话题名: /yaw_data
 * 可将yaw角度归零的话题名: /yaw_zero_topic
 * 可将yaw角度归零的服务名: /yaw_zero_service
+* 可修改串口波特率的服务: /baud_update_service
+如果想使用话题方式,将yaw角度归零,那就可以执行如下命令：
+`rostopic pub -1 /yaw_zero_topic std_msgs/Empty "{}"`
+如果想使用服务调用方式,将yaw角度归零,那执行如下命令：
+`rosservice call /yaw_zero_service "{}"`
+如果想修改串口通信的波特率,那就可以执行如下命令:
+`rosservice call /baud_update_service "index: 1"`
+这里需要注意index后面的数字,１表示更新波特率为9600,2表示为57600,3表示115200,
+当更新完波特率后,需要修改本地配置文件中串口波特率为新的,否则会无法正常获取imu数据.
 
 ----
 ## 使用IIC发布IMU数据

serial_imu_hat_6dof/cfg/param.yaml

@@ -8,12 +8,12 @@
 #     默认是给树莓派蓝牙使用的,所以要想使用该硬件串口,
 #     就需要在raspi-config中关闭串口登录功能,打开硬件
 #     串口功能,这同时也需要将树莓派的蓝牙给禁用掉.
-#  baud_rate:串口通信波特率,旧模块的波特率为9600,
-#     新版本的波特率为57600,如果串口没数据可以修改.
+#  baud_rate:串口通信波特率,默认为57600,如果查看发布的
+#  　　话题中没IMU数据可以尝试修改波特率.
 #  pub_data_topic:发布imu数据的ROS话题名.
 #  pub_temp_topic:发布imu模块温度的ROS话题名.
 #  link_name:imu模块的link名.
-#  pub_hz:话题发布的数据频率,默认设置为20Hz.
+#  pub_hz:话题发布的数据频率,默认设置为30Hz.
 #  yaw_zero_topic:通过往该话题中发消息即可将yaw角度归零.
 #  yaw_pub_topic:将yaw信息通过该话题发送出来.
 # Author: corvin
@@ -26,9 +26,9 @@
 #     增加使用服务方式将yaw归零的名称yaw_zero_service.
 #   20210319:增加修改串口波特率的配置参数baud_update_srv.
 ##########################################################
-imu_dev: /dev/ttyUSB0
+imu_dev: /dev/ttyAMA0
 baud_rate: 57600
-pub_hz: 20
+pub_hz: 30
 
 link_name: base_imu_link
 pub_data_topic: imu_data

serial_imu_hat_6dof/include/imu_data.h

@@ -14,17 +14,10 @@ int initSerialPort(const char* path, const int baud);
 int getImuData();
 int closeSerialPort();
 
-float getAccX();
-float getAccY();
-float getAccZ();
-
-float getAngularX();
-float getAngularY();
-float getAngularZ();
-
-float getAngleX();
-float getAngleY();
-float getAngleZ();
+float getAcc(int flag);
+float getAngular(int flag);
+float getAngle(int flag);
+float getQuat(int flag);
 
 int makeYawZero(void);
 int setIMUBaudRate(const int flag);

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -22,7 +22,7 @@
 
 static unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
 static int port_fd = -1; //串口打开时的文件描述符
-float acce[3],angular[3],angle[3],quater[4];
+static float acce[3],angular[3],angle[3],quater[4];
 
 /******************************************************
  * Description:打开IMU模块串口,输入两个参数即可连接.
@@ -237,6 +237,7 @@ void parse_serialData(unsigned char chr)
             quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
             quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
             quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
+            //printf("%f %f %f %f\n", quater[0], quater[1], quater[2], quater[3]);
             break;
     }
     chrCnt = 0;
@@ -314,59 +315,56 @@ int initSerialPort(const char* path, const int baud)
 
     return 0;
 }
-
-float getAccX()
-{
-    return acce[0];
-}
-float getAccY()
-{
-    return acce[1];
-}
-float getAccZ()
+/*****************************************
+ * Description:得到三轴加速度信息,输入
+ * 　参数可以为0,1,2分别代表x,y,z轴的
+ * 　加速度信息.
+ *****************************************/
+float getAcc(int flag)
 {
-    return acce[2];
+    return acce[flag];
 }
 
-float getAngularX()
+/******************************************
+ * Description:得到角速度信息,输入参数可
+ * 　以为0,1,2,分别代表x,y,z三轴的角速度
+ *   信息.
+ *****************************************/
+float getAngular(int flag)
 {
-    return angular[0];
+    return angular[flag];
 }
 
-float getAngularY()
+/*******************************************
+ * Description:获取yaw,pitch,roll,输入参数0,
+ * 1,2可以分别获取到roll,pitch,yaw的数据.
+ *******************************************/
+float getAngle(int flag)
 {
-    return angular[1];
+    return angle[flag];
 }
 
-float getAngularZ()
+/********************************************
+ * Description:输入参数0,1,2,3可以分别获取到
+ * 　四元素的q0,q1,q2,q3.但是这里对应的ros中
+ * 　的imu_msg.orientation.w,x,y,z的顺序,
+ * 　这里的q0是对应w参数,1,2,3对应x,y,z.
+ ********************************************/
+float getQuat(int flag)
 {
-    return angular[2];
+    return quater[flag];
 }
 
-float getAngleX()
-{
-    return angle[0];
-}
-float getAngleY()
-{
-    return angle[1];
-}
-float getAngleZ()
-{
-    return angle[2];
-}
-
-/********************************************************
+/*************************************************************
  * Description:从串口读取数据,然后解析出各有效数据段,这里
- *   一次性从串口中读取100个字节,然后需要从这些字节中进行
- *   解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
- *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
- *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
- ********************************************************/
+ *  一次性从串口中读取100个字节,然后需要从这些字节中进行
+ *  解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
+ *  有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
+ *  角度输出(0x55 0x53),四元素输出(0x55 0x59).
+ ***********************************************************/
 int getImuData()
 {
     int data_len = 0;
-
     bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零
 
     //开始从串口中读取数据,并将其保存到r_buf缓冲区中

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -105,7 +105,7 @@ int main(int argc, char **argv)
         closeSerialPort();
         return -1;
     }
-    ROS_INFO("IMU module is working... ");
+    ROS_INFO("IMU module is working...");
 
     //定义两个服务,分别是更新波特率和yaw角度归零
     ros::ServiceServer baudSrv = handle.advertiseService(baud_update_service, setBaudService);
@@ -125,9 +125,9 @@ int main(int argc, char **argv)
             break;
 
         imu_msg.header.stamp = ros::Time::now();
-        roll  = getAngleX()*degree2Rad;
-        pitch = getAngleY()*degree2Rad;
-        yaw   = getAngleZ()*degree2Rad;
+        roll  = getAngle(0)*degree2Rad;
+        pitch = getAngle(1)*degree2Rad;
+        yaw   = getAngle(2)*degree2Rad;
         if(yaw >= 3.1415926)
             yaw -= 6.2831852;
 
@@ -136,22 +136,25 @@ int main(int argc, char **argv)
         yaw_pub.publish(yaw_msg);
 
         //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
-        imu_msg.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+        imu_msg.orientation.x = getQuat(1);
+        imu_msg.orientation.y = getQuat(2);
+        imu_msg.orientation.z = getQuat(3);
+        imu_msg.orientation.w = getQuat(0);
         imu_msg.orientation_covariance = {0.0025, 0, 0,
                                           0, 0.0025, 0,
                                           0, 0, 0.0025};
 
-        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.x = getAngular(0)*degree2Rad;
+        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
+        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
         imu_msg.angular_velocity_covariance = {0.02, 0, 0,
                                                0, 0.02, 0,
                                                0, 0, 0.02};
 
-        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAccX(), getAccY(), getAccZ());
-        imu_msg.linear_acceleration.x = -getAccX()*acc_factor;
-        imu_msg.linear_acceleration.y = -getAccY()*acc_factor;
-        imu_msg.linear_acceleration.z = -getAccZ()*acc_factor;
+        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
+        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
+        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
+        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
         imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
                                                    0, 0.04, 0,
                                                    0, 0, 0.04};