向imu发送命令的时间减小优化,这样控制更迅速

serial_6dof_imu/src/proc_serial_data.cpp

@@ -34,11 +34,11 @@ int closeSerialPort(void)
 }
 
 /*****************************************************************
- * Description:向IMU模块发送解锁命令,发送完命令需要延迟250ms.
+ * Description:向IMU模块发送解锁命令,发送完命令需要延迟10ms.
  ****************************************************************/
 static int send_unlockCmd(int fd)
 {
-    int ret = -1;
+    int ret = 0;
     unsigned char unLockCmd[5] = {0xFF, 0xAA, 0x69, 0x88, 0xB5};
     ret = write(fd, unLockCmd, sizeof(unLockCmd));
     if(ret != sizeof(unLockCmd))
@@ -46,26 +46,24 @@ static int send_unlockCmd(int fd)
         ROS_ERROR("Send IMU module unlock cmd error !");
         return -1;
     }
-    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
-
+    ros::Duration(0.01).sleep(); //delay 10ms才能发送其他配置命令
     return 0;
 }
 
 /*****************************************************************
- * Description:向IMU模块发送保存命令,发送完命令需要延迟250ms.
+ * Description:向IMU模块发送保存命令,发送完命令需要延迟100ms.
  *****************************************************************/
 static int send_saveCmd(int fd)
 {
-    int ret = -1;
+    int ret = 0;
     unsigned char saveCmd[5]   = {0xFF, 0xAA, 0x00, 0x00, 0x00};
     ret = write(fd, saveCmd, sizeof(saveCmd));
     if(ret != sizeof(saveCmd))
     {
         ROS_ERROR("Send IMU module save cmd error !");
-        return -3;
+        return -1;
     }
-    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
-
+    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
     return 0;
 }
 
@@ -75,7 +73,7 @@ static int send_saveCmd(int fd)
  *****************************************************************/
 static int send_data(int fd, unsigned char *send_buffer, int length)
 {
-    int ret = -1;
+    int ret = 0;
 
     #if 0 //打印发送给IMU模块的数据,用于调试
     printf("Send %d byte to IMU:", length);
@@ -85,23 +83,18 @@ static int send_data(int fd, unsigned char *send_buffer, int length)
     }
     printf("\n");
     #endif
-    send_unlockCmd(fd);//发送解锁命令
+    send_unlockCmd(fd); //发送解锁命令
 
-    //发送控制命令,需要延时250ms
+    //发送完控制命令,需要延时100ms
     ret = write(fd, send_buffer, length*sizeof(unsigned char));
     if(ret != length)
     {
         ROS_ERROR("Send IMU module control cmd error !");
         return -2;
     }
-    ros::Duration(0.25).sleep(); //delay 250ms才能发送其他配置命令
-
-    //发送两遍解锁保存命令
-    send_unlockCmd(fd);  //发送解锁命令
-    send_saveCmd(fd);    //发送保存命令
-    send_unlockCmd(fd);  //发送解锁命令
-    send_saveCmd(fd);    //发送保存命令
+    ros::Duration(0.1).sleep(); //delay 100ms才能发送其他配置命令
 
+    send_saveCmd(fd);  //发送保存命令
     return 0;
 }