Enable I2C interface.

diff --git a/src/bno055_node.cpp b/src/bno055_node.cpp
index 024d863..2dad386 100644 (file)
--- a/src/bno055_node.cpp
+++ b/src/bno055_node.cpp
@@ -20,7 +20,7 @@
 *******************************************************************************/
 
 #include <bno055_tty.h>
-//#include <bno055_i2c.h>
+#include <bno055_i2c.h>
 #include <bno055_units.h>
 
 #include <ros/node_handle.h>
@@ -103,7 +103,7 @@ Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false)
        else
        {
                u8 address=std::stoi(addressStr,NULL,0);        
-//             if(BNO055_i2c_init(device.c_str(),address,&bno055_))
+               if(BNO055_i2c_init(device.c_str(),address,&bno055_))
                {
                        perror("Error initializing I2C interface");
                        exit(-1);
@@ -168,7 +168,7 @@ Bno055Node::~Bno055Node(void)
        gravityPub_.shutdown();
 
        if(uart_) BNO055_tty_close(&bno055_);
-//     else BNO055_i2c_close(&bno055_);
+       else BNO055_i2c_close(&bno055_);
 }
 
 void Bno055Node::publish(void) const