From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 16 Aug 2019 21:39:44 +0000 (-0300)
Subject: Add mode to Bno055 constructor.
X-Git-Tag: f1~6
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=2c49c408e58a0d7caf60ad18ea454646c5de46b5;p=imufusion.git

Add mode to Bno055 constructor.
---

diff --git a/include/bno055.hpp b/include/bno055.hpp
index c566e31..305117d 100644
--- 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
--- 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
--- 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
--- 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: