Add mode to Bno055 constructor.

diff --git a/include/bno055.hpp b/include/bno055.hpp
index c566e31..305117d 100644 (file)
--- a/include/bno055.hpp
+++ b/include/bno055.hpp
@@ -44,18 +44,24 @@ class Bno055: public Imu
        struct bno055_t bno055_;        ///< BNO055_driver handle.
        char interface_;                ///< Interface type: 'i' for I2C, 't" for UART.
        
-        /** @brief Initialize Bno055
+        /** @brief Initialize Bno055.
+               @param mode Operation mode.
        */
-       void init(void);
+       void init(uint8_t mode);
        
        public:
        /** @brief Constructor of Bno055 for I2C interface.
+               @param device I2C device name. Something like /dev/i2c-0.
+               @param devAddr Address of BNO055 in I2C bus.
+               @param mode Operation mode.
        */
-       Bno055(const char *device,uint8_t devAddr);
+       Bno055(const char *device,uint8_t devAddr,uint8_t mode);
        
        /** @brief Constructor of Bno055 for UART interface.
+               @param device UART device name. Something like/dev/ttyS0, /dev/ttyUSB0.
+               @param Operation mode.
        */
-       Bno055(const char *device);
+       Bno055(const char *device,uint8_t mode);
        
        /** @brief Destructor of Bno055.
        */

diff --git a/lib/bno055.cpp b/lib/bno055.cpp
index aeeba64..f583642 100644 (file)
--- a/lib/bno055.cpp
+++ b/lib/bno055.cpp
@@ -28,24 +28,24 @@
 namespace imufusion
 {
 
-Bno055::Bno055(const char *device,uint8_t devAddr)
+Bno055::Bno055(const char *device,uint8_t devAddr,uint8_t mode)
 {
        interface_='i';
        if(BNO055_i2c_init(device,devAddr,&bno055_))
                throw Exception("Error initializing I2C interface.");
-       init();
+       init(mode);
 }
 
-Bno055::Bno055(const char *device)
+Bno055::Bno055(const char *device,uint8_t mode)
 {
        interface_='t';
        if(BNO055_tty_init(device,&bno055_))
                throw Exception("Error initializing TTY interface.");
-       init();
+       init(mode);
 }
 
 
-void Bno055::init(void)
+void Bno055::init(uint8_t mode)
 {
        if(bno055_init(&bno055_) != BNO055_SUCCESS)
                throw Exception("Error initializing bno055.");
@@ -81,12 +81,8 @@ void Bno055::init(void)
        if(bno055_set_data_output_format(0x01) != BNO055_SUCCESS)
                throw Exception("Error setting data output format.");
 
-//     Use internal fusion data for evaluation
-//     if(bno055_set_operation_mode(BNO055_OPERATION_MODE_AMG) != BNO055_SUCCESS)
-//             throw Exception("Error setting operation mode to AMG.");
-
-       if(bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF) != BNO055_SUCCESS)
-               throw Exception("Error setting operation mode to NDOF.");
+       if(bno055_set_operation_mode(mode) != BNO055_SUCCESS)
+               throw Exception("Error setting operation mode.");
 }
 
 Bno055::~Bno055(void)

diff --git a/lib/ekf.cpp b/lib/ekf.cpp
index e757879..a688340 100644 (file)
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -20,7 +20,7 @@
 *******************************************************************************/
 
 #include <ekf.hpp>
-#include <iostream>
+
 namespace imufusion
 {
 

diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp
index d89677b..d7161a9 100644 (file)
--- a/test/ekf_test.cpp
+++ b/test/ekf_test.cpp
@@ -51,13 +51,13 @@ int main(int argc,char *argv[])
                                {
                                        /* I2C interface */
                                        uint8_t devAddr=strtol(optarg,NULL,0);
-                                       imu=new imufusion::Bno055(argv[optind],devAddr);
+                                       imu=new imufusion::Bno055(argv[optind],devAddr,BNO055_OPERATION_MODE_NDOF); // Use this mode for debug, use AMG for production
                                        break;
                                        }
                                case 't':
                                {
                                        /* TTY interface */
-                                       imu=new imufusion::Bno055(argv[optind]);
+                                       imu=new imufusion::Bno055(argv[optind],BNO055_OPERATION_MODE_NDOF);
                                        break;
                                }
                                default: