增加可以控制IMU板D0D1D2D3四个引脚高低电平的服务

serial_imu_hat_6dof/CMakeLists.txt

@@ -37,8 +37,6 @@ find_package(catkin REQUIRED COMPONENTS
 ##     find_package(catkin REQUIRED COMPONENTS ...)
 ##   * add "message_runtime" and every package in MSG_DEP_SET to
 ##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
 ##   * uncomment the generate_messages entry below
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
@@ -46,7 +44,6 @@ find_package(catkin REQUIRED COMPONENTS
 # add_message_files(
 #   FILES
 #   Message1.msg
-#   Message2.msg
 # )
 
 ## Generate services in the 'srv' folder
@@ -54,15 +51,9 @@ add_service_files(
   FILES
   setYawZero.srv
   setBaudRate.srv
+  setPinOutHL.srv
 )
 
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
@@ -87,7 +78,6 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 # generate_dynamic_reconfigure_options(
 #   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
 # )
 
 ###################################
@@ -100,7 +90,7 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS include
 #  LIBRARIES serial_imu_hat_6dof
 #  CATKIN_DEPENDS dynamic_reconfigure roscpp sensor_msgs
 #  DEPENDS system_lib

serial_imu_hat_6dof/cfg/param.yaml

@@ -1,4 +1,4 @@
-##########################################################
+###########################################################
 # Copyright: 2016-2021 www.corvin.cn ROS小课堂
 # Description:使用串口与IMU模块进行通信时的配置信息.
 # 可以配置的各参数如下:
@@ -25,8 +25,10 @@
 #   20210318:增加yaw角度归零的话题配置参数yaw_zero_topic.
 #     增加使用服务方式将yaw归零的名称yaw_zero_service.
 #   20210319:增加修改串口波特率的配置参数baud_update_srv.
-##########################################################
-imu_dev: /dev/ttyAMA0
+#   20210321:增加可以设置D0-D4共4个引脚输出数字高低电平的
+#     服务配置参数pin_outHL_srv.
+###########################################################
+imu_dev: /dev/ttyUSB0
 baud_rate: 57600
 pub_hz: 30
 
@@ -37,4 +39,4 @@ yaw_pub_topic: yaw_data
 yaw_zero_topic: yaw_zero_topic
 yaw_zero_service: yaw_zero_service
 baud_update_srv: baud_update_service
-
+pin_outHL_srv: pin_outHL_service

serial_imu_hat_6dof/include/imu_data.h

@@ -5,6 +5,7 @@
  * History:
  *   20210318:init this file.
  *   20210319:增加修改串口通信波特率的函数.
+ *   20210321:增加控制IMU引脚数字高低电平的函数.
  *******************************************************/
 #ifndef _IMU_DATA_H_
 #define _IMU_DATA_H_
@@ -21,4 +22,5 @@ float getQuat(int flag);
 
 int makeYawZero(void);
 int setIMUBaudRate(const int flag);
+int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3);
 #endif

serial_imu_hat_6dof/src/proc_serial_data.cpp

@@ -1,6 +1,6 @@
-/*****************************************************************
+/*******************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口方式读取IMU模块信息.
+ * Description:使用串口方式读取和控制IMU模块信息.
  * Author: corvin
  * History:
  *   20200401:init this file.
@@ -8,6 +8,7 @@
  *     这样才能进行/32768.0*16.0运算.
  *   20210318:增加可以将yaw角度归零的函数yawZeroCmd().
  *   20210319:增加可以修改串口通信波特率的函数setIMUBaudRate().
+ *   20210321:增加控制引脚输出数字高低电平的函数setIMUPinOutHL().
 ******************************************************************/
 #include<ros/ros.h>
 #include<stdio.h>
@@ -296,6 +297,57 @@ int setIMUBaudRate(const int flag)
     return 0;
 }
 
+/*********************************************************************
+ * Description:要想控制引脚输出数字高低电平,其实就是发送固定的控制命令,
+ *   这里的高低电平的控制字节是命令中的第4个字节,0x03表明是输出低电平,
+ *   要想输出高电平就是将该字节改为0x02即可.剩下就是依次发送这4个引脚
+ *   的控制命令就可以了.
+ ********************************************************************/
+int setIMUPinOutHL(const int d0,const int d1,const int d2,const int d3)
+{
+    int ret = -1;
+    unsigned char pinD0CtlCmd[5] = {0xFF, 0xAA, 0x0E, 0x03, 0x00};
+    unsigned char pinD1CtlCmd[5] = {0xFF, 0xAA, 0x0F, 0x03, 0x00};
+    unsigned char pinD2CtlCmd[5] = {0xFF, 0xAA, 0x10, 0x03, 0x00};
+    unsigned char pinD3CtlCmd[5] = {0xFF, 0xAA, 0x11, 0x03, 0x00};
+
+    if(d0 == 1)
+        pinD0CtlCmd[3] = 0x02;
+    if(d1 == 1)
+        pinD1CtlCmd[3] = 0x02;
+    if(d2 == 1)
+        pinD2CtlCmd[3] = 0x02;
+    if(d3 == 1)
+        pinD3CtlCmd[3] = 0x02;
+
+    ret = send_data(port_fd, pinD0CtlCmd, sizeof(pinD0CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D0 control cmd error !");
+        return -1;
+    }
+    ret = send_data(port_fd, pinD1CtlCmd, sizeof(pinD1CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D1 control cmd error !");
+        return -2;
+    }
+    ret = send_data(port_fd, pinD2CtlCmd, sizeof(pinD2CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D2 control cmd error !");
+        return -3;
+    }
+    ret = send_data(port_fd, pinD3CtlCmd, sizeof(pinD3CtlCmd));
+    if(ret != 0)
+    {
+        ROS_ERROR("Send pin D3 control cmd error !");
+        return -4;
+    }
+
+    return 0;
+}
+
 /*****************************************************************
  * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
  *   两个输入参数的意义如下:

serial_imu_hat_6dof/src/serial_imu_node.cpp

@@ -1,8 +1,8 @@
 /****************************************************************
  * Copyright:2016-2021 www.corvin.cn ROS小课堂
- * Description:使用串口来读取IMU模块的数据,并通过ROS话题
- *   topic将数据发布出来.芯片中默认读取出来的加速度数据
- *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
+ * Description:使用串口来读取IMU模块的数据,并通过ROS话题topic将
+ *   数据发布出来.芯片中默认读取出来的加速度数据单位是g,需要将其
+ *   转换为ROS中加速度规定的m/s2才能发布.
  * Author: corvin
  * History:
  *   20200403: init this file.
@@ -15,15 +15,18 @@
  *     增加使用service方式可将yaw角度归零.
  *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
  *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
+ *   20210321:增加可以设置D0,D1,D2,D3共4个IO引脚输出数字高低电平
+ *     的服务,这里的高电平为3.3V.
 ****************************************************************/
 #include <ros/ros.h>
 #include <tf/tf.h>
-#include "imu_data.h"
+#include <imu_data.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Empty.h>
 #include "serial_imu_hat_6dof/setYawZero.h"
 #include "serial_imu_hat_6dof/setBaudRate.h"
+#include "serial_imu_hat_6dof/setPinOutHL.h"
 
 
 /**********************************************************
@@ -64,6 +67,19 @@ bool setBaudService(serial_imu_hat_6dof::setBaudRate::Request &req,
     return true;
 }
 
+/********************************************************************
+ * Description:控制IMU板的D0,D1,D2,D3共4个引脚的输出数字高低电平信号,
+ *   这里的服务request一次性控制4个引脚的状态,对应引脚0,则输出低电平,
+ *   请求为1,则输出高电平.根据反馈状态status,可以得知控制的结果,如果
+ *   反馈0,表明控制成功,负值的话控制失败.这里的高电平为3.3V.
+ ********************************************************************/
+bool pinOutHLService(serial_imu_hat_6dof::setPinOutHL::Request &req,
+                     serial_imu_hat_6dof::setPinOutHL::Response &res)
+{
+    res.status = setIMUPinOutHL(req.D0, req.D1, req.D2, req.D3);
+    return true;
+}
+
 int main(int argc, char **argv)
 {
     float yaw, pitch, roll;
@@ -77,6 +93,7 @@ int main(int argc, char **argv)
     std::string yaw_zero_topic;
     std::string yaw_zero_service;
     std::string baud_update_service;
+    std::string pin_outHL_service;
 
     int pub_hz = 0;
     float degree2Rad = 3.1415926/180.0;
@@ -94,8 +111,9 @@ int main(int argc, char **argv)
     ros::param::get("~pub_temp_topic", temp_topic);
     ros::param::get("~yaw_zero_topic", yaw_zero_topic);
     ros::param::get("~yaw_zero_service", yaw_zero_service);
-    ros::param::get("~yaw_pub_topic",  yaw_pub_topic);
-    ros::param::get("~baud_update_srv", baud_update_service);
+    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
+    ros::param::get("~baud_update_srv",  baud_update_service);
+    ros::param::get("~pin_outHL_srv",    pin_outHL_service);
 
     //初始化imu模块串口,根据设备号和波特率进行连接
     int ret = initSerialPort(imu_dev.c_str(), baud);
@@ -107,9 +125,10 @@ int main(int argc, char **argv)
     }
     ROS_INFO("IMU module is working...");
 
-    //定义两个服务,分别是更新波特率和yaw角度归零
+    //定义三个服务,分别是更新波特率,yaw角度归零和控制引脚输出高低电平
     ros::ServiceServer baudSrv = handle.advertiseService(baud_update_service, setBaudService);
     ros::ServiceServer yawSrv  = handle.advertiseService(yaw_zero_service, yawZeroService);
+    ros::ServiceServer pinSrv  = handle.advertiseService(pin_outHL_service, pinOutHLService);
 
     ros::Subscriber yaw_zero_sub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
     ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);

serial_imu_hat_6dof/srv/setPinOutHL.srv

@@ -0,0 +1,6 @@
+int32 D0
+int32 D1
+int32 D2
+int32 D3
+---
+int32 status