增加iic操作的代码

rasp_imu_hat_6dof/CMakeLists.txt

@@ -28,9 +28,10 @@ catkin_package(
     CATKIN_DEPENDS roscpp
     )
 
-add_executable(imu_pub_node src/imu_data.cpp src/imu_node.cpp)
+add_executable(imu_pub_node src/imu_data.cpp src/imu_node.cpp src/iic_dev.cpp)
 target_link_libraries(imu_pub_node
     ${catkin_LIBRARIES}
+    libbcm2835.a
     )
 
 include_directories(

rasp_imu_hat_6dof/include/iic_dev.h

@@ -0,0 +1,48 @@
+#ifndef _I2CDEV_H_
+#define _I2CDEV_H_
+
+#ifndef __cplusplus
+#error A C++ compiler is required!
+#endif
+
+#include <bcm2835.h>
+#include <math.h> // required for BMP180
+#include <stdlib.h> // required for MPU6060
+#include <string.h> // required for MPU6060
+
+
+#define set_I2C_pins  false
+/* used to boolean for setting RPi I2C pins P1-03 (SDA) and P1-05 (SCL) to alternate function ALT0, which enables those pins for I2C interface.
+   setI2Cpin should be false, if the I2C are already configured in alt mode ... */
+
+#define i2c_baudrate 100000  //100 kHz
+//uint32_t i2c_baudrate = 400000 ; //400 kHz,
+
+class I2Cdev {
+ public:
+        I2Cdev();
+
+        static void initialize();
+        static void enable(bool isEnabled);
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+};
+
+#endif /* _I2CDEV_H_ */
+

rasp_imu_hat_6dof/include/imu_data.h

@@ -18,7 +18,7 @@
 class myIMU
 {
     public:
-        int initIICDevice();
+        int initDev();
 
         float getRollData();
         float getPitchData();
@@ -33,18 +33,17 @@ class myIMU
         float getGyoZData();
 
     private:
-        int iicFD;
-        char accX[2];
-        char accY[2];
-        char accZ[2];
-
-        char gyoX[2];
-        char gyoY[2];
-        char gyoZ[2];
-
-        char roll[2];
-        char pitch[2];
-        char yaw[2];
+        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

rasp_imu_hat_6dof/launch/imu_data_pub.launch

@@ -10,8 +10,11 @@
 <launch>
   <arg name="imu_cfg_file" default="$(find rasp_imu_hat_6dof)/cfg/param.yaml"/>
 
+  <!---
   <node pkg="rasp_imu_hat_6dof" type="imu_node.py" name="imu_node" output="screen">
       <rosparam file="$(arg imu_cfg_file)" command="load"/>
   </node>
+  -->
+  <node pkg="rasp_imu_hat_6dof" type="imu_pub_node" name="imu_pub_node" output="screen" />
 </launch>
 

rasp_imu_hat_6dof/src/iic_dev.cpp

@@ -0,0 +1,233 @@
+#include "iic_dev.h"
+#include <stdio.h>
+
+I2Cdev::I2Cdev() { }
+
+void I2Cdev::initialize() {
+  bcm2835_init();
+  bcm2835_i2c_set_baudrate( i2c_baudrate  );
+}
+
+/** Enable or disable I2C,
+ * @param isEnabled true = enable, false = disable
+ */
+void I2Cdev::enable(bool isEnabled) {
+  if ( set_I2C_pins ){
+    if (isEnabled)
+      bcm2835_i2c_end();
+    else
+      bcm2835_i2c_begin() ;
+  }
+}
+
+char sendBuf[256];
+char recvBuf[256];
+
+
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  *data = recvBuf[1] & (1 << bitNum);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
+  // 01101001 read byte
+  // 76543210 bit numbers
+  //    xxx   args: bitStart=4, length=3
+  //    010   masked
+  //   -> 010 shifted
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  uint8_t b = (uint8_t) recvBuf[0];
+  if (response == BCM2835_I2C_REASON_OK) {
+    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+    b &= mask;
+    b >>= (bitStart - length + 1);
+    *data = b;
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  data[0] = (uint8_t) recvBuf[0];
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length);
+  int i ;
+  for (i = 0; i < length ; i++) {
+    data[i] = (uint8_t) recvBuf[i];
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  //first reading registery value
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
+  if ( response == BCM2835_I2C_REASON_OK ) {
+    uint8_t b = recvBuf[0] ;
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    sendBuf[1] = b ;
+    response = bcm2835_i2c_write(sendBuf, 2);
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+  //      010 value to write
+  // 76543210 bit numbers
+  //    xxx   args: bitStart=4, length=3
+  // 00011100 mask byte
+  // 10101111 original value (sample)
+  // 10100011 original & ~mask
+  // 10101011 masked | value
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  //first reading registery value
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
+  if ( response == BCM2835_I2C_REASON_OK ) {
+    uint8_t b = recvBuf[0];
+    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+    data <<= (bitStart - length + 1); // shift data into correct position
+    data &= mask; // zero all non-important bits in data
+    b &= ~(mask); // zero all important bits in existing byte
+    b |= data; // combine data with existing byte
+    sendBuf[1] = b ;
+    response = bcm2835_i2c_write(sendBuf, 2);
+    }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  sendBuf[1] = data;
+  uint8_t response = bcm2835_i2c_write(sendBuf, 2);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 2 );
+  data[0] = (recvBuf[0] << 8) | recvBuf[1] ;
+  return  response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length*2 );
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    data[i] = (recvBuf[i*2] << 8) | recvBuf[i*2+1] ;
+  }
+  return  response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  sendBuf[1] = (uint8_t) (data >> 8); //MSByte
+  sendBuf[2] = (uint8_t) (data >> 0); //LSByte
+  uint8_t response = bcm2835_i2c_write(sendBuf, 3);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    sendBuf[i+1] = data[i] ;
+  }
+  uint8_t response = bcm2835_i2c_write(sendBuf, 1+length);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    sendBuf[1+2*i] = (uint8_t) (data[i] >> 8); //MSByte
+    sendBuf[2+2*i] = (uint8_t) (data[i] >> 0); //LSByte
+  }
+  uint8_t response = bcm2835_i2c_write(sendBuf, 1+2*length);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+

rasp_imu_hat_6dof/src/imu_data.cpp

@@ -1,68 +1,86 @@
 #include "imu_data.h"
+#include "iic_dev.h"
 #include <ros/ros.h>
 #include <unistd.h>
 #include <fcntl.h>
 #include <stdio.h>
 #include <linux/i2c-dev.h>
 #include <sys/ioctl.h>
+#include <bcm2835.h>
+#include <math.h>
 
-int myIMU::initIICDevice()
+int myIMU::initDev()
 {
-    int fd;
-    fd = open("/dev/i2c-1", O_RDWR);
-    if(fd < 0)
-    {
-        ROS_ERROR_STREAM("Open IIC device error !");
-        return -1;
-    }
-    iicFD = fd;
-    ROS_INFO("init iicFD = %d", iicFD);
-    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
-    {
-        ROS_ERROR_STREAM("IIC device ioctl error !");
-    }
-
-    return 0;
+//I2Cdev::initialize();
 }
 
 float myIMU::getAccXData()
 {
     float data = 0;
-    ROS_INFO("iicFD = %d", iicFD);
-
-    if(read(iicFD, &accX, 2))
+#if 0
+    int len = 0;
+    len = I2Cdev::readBytes(I2C_DEV_ADDR, ACC_X_REG, 2, accX);
+    if(len > 0)
     {
         data = (((short)accX[1]<<8)|accX[0])/32768.0*16.0;
     }
+    else
     {
         ROS_ERROR_STREAM("Read accX data error !");
     }
+#endif
     return data;
 }
 
 float myIMU::getAccYData()
 {
     float data = 0;
-    if(read(iicFD, &accY, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, accY, 2))
     {
         data = (((short)accY[1]<<8)|accY[0])/32768.0*16.0;
     }
+    else
     {
         ROS_ERROR_STREAM("Read accY data error !");
     }
+    close(fd);
     return data;
 }
 
 float myIMU::getAccZData()
 {
     float data = 0;
-    if(read(iicFD, &accZ, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, accZ, 2))
     {
         data = (((short)accZ[1]<<8)|accZ[0])/32768.0*16.0;
     }
+    else
     {
         ROS_ERROR_STREAM("Read accZ data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -70,13 +88,25 @@ float myIMU::getAccZData()
 float myIMU::getGyoXData()
 {
     float data = 0;
-    if(read(iicFD, &gyoX, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, gyoX, 2))
     {
         data = (((short)gyoX[1]<<8)|gyoX[0])/32768.0*2000.0;
     }
     {
         ROS_ERROR_STREAM("Read gyoX data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -84,13 +114,25 @@ float myIMU::getGyoXData()
 float myIMU::getGyoYData()
 {
     float data = 0;
-    if(read(iicFD, &gyoY, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, gyoY, 2))
     {
         data = (((short)gyoY[1]<<8)|gyoY[0])/32768.0*2000.0;
     }
     {
         ROS_ERROR_STREAM("Read gyoY data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -98,13 +140,25 @@ float myIMU::getGyoYData()
 float myIMU::getGyoZData()
 {
     float data = 0;
-    if(read(iicFD, &gyoZ, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, gyoZ, 2))
     {
         data = (((short)gyoZ[1]<<8)|gyoZ[0])/32768.0*2000.0;
     }
     {
         ROS_ERROR_STREAM("Read gyoZ register data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -112,13 +166,25 @@ float myIMU::getGyoZData()
 float myIMU::getRollData()
 {
     float data = 0;
-    if(read(iicFD, &roll, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, &roll, 2))
     {
         data = (((short)roll[1]<<8)|roll[0])/32768.0*180.0;
     }
     {
         ROS_ERROR_STREAM("Read roll register data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -126,13 +192,25 @@ float myIMU::getRollData()
 float myIMU::getPitchData()
 {
     float data = 0;
-    if(read(iicFD, &pitch, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, &pitch, 2))
     {
         data = (((short)pitch[1]<<8)|pitch[0])/32768.0*180.0;
     }
     {
         ROS_ERROR_STREAM("Read pitch register data error !");
     }
+    close(fd);
 
     return data;
 }
@@ -140,13 +218,25 @@ float myIMU::getPitchData()
 float myIMU::getYawData()
 {
     float data = 0;
-    if(read(iicFD, &yaw, 2))
+    int fd = 0;
+    fd = open("/dev/i2c-1", O_RDWR);
+    if(fd < 0)
+    {
+        ROS_ERROR_STREAM("Open IIC device error !");
+        return -1;
+    }
+    if(ioctl(fd, I2C_SLAVE, I2C_DEV_ADDR) < 0)
+    {
+        ROS_ERROR_STREAM("IIC device ioctl error !");
+    }
+    if(read(fd, &yaw, 2))
     {
         data = (((short)yaw[1]<<8)|yaw[0])/32768.0*180.0;
     }
     {
         ROS_ERROR_STREAM("Read yaw register data error !");
     }
+    close(fd);
 
     return data;
 }

rasp_imu_hat_6dof/src/imu_node.cpp

@@ -29,7 +29,7 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "imu_data_pub_node");
     ros::NodeHandle handle;
     myIMU my_imu;
-    my_imu.initIICDevice();
+    my_imu.initDev();
 
     ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>("imu_data", 2);
     ros::Rate loop_rate(10);