--- /dev/null
+/******************************************************************************
+ IMU Fusion Library
+ BNO080 Abstraction
+ Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+/** @file bno080.hpp
+ @brief BNO080 abstraction.
+ @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+
+#ifndef __IMUFUSION_BNO080_H__
+#define __IMUFUSION_BNO080_H__
+
+#include <stdint.h>
+
+#include <thread>
+
+extern "C"
+{
+#include <sh2.h>
+}
+
+#include <imu.hpp>
+
+namespace imufusion
+{
+
+/** @brief BNO080 Abstraction.
+*/
+class Bno080: public Imu
+{
+ private:
+ sh2_Hal_t sh2Hal_; ///< SH2 handle.
+ char interface_; ///< Interface type: 'i' for I2C, 's' for SPI, 't' for UART.
+
+ Eigen::Vector3d accel_; ///< Accelerometer reading.
+ Eigen::Vector3d mag_; ///< Magnetometer reading.
+ Eigen::Vector3d gyro_; ///< Gyroscope reading.
+
+ Eigen::Quaterniond quaternion_; ///< Orientation estimate.
+ Eigen::Vector3d linearAccel_; ///< Linear acceleration estimate.
+ Eigen::Vector3d gravity_; ///< Gravity estimate.
+
+ std::thread serviceThread_; ///< Handle for SH2 internal processing thread.
+ bool runService_; ///< Flag to control SH2 interl processing flag.
+
+ /** @brief Initialize Bno080.
+ @param sensors List of sensors to use.
+ @param nsensors Number of sensors.
+ */
+ void init(const int sensors[],int nsensors);
+
+ /** @brief Sensor callback
+ @param cookie A value that will be passed to the sensor
+ callback function, typically, a pointer to the Bno080
+ object.
+ @param event SH2 sensor event.
+ */
+ friend void sensorCallback(void *cookie,sh2_SensorEvent_t *event);
+
+ /** @brief SH2 service thread
+ */
+ void serviceThread(void);
+
+ public:
+ /** @brief Constructor of Bno080 for I2C interface.
+ @param device I2C device name. Something like /dev/i2c-0.
+ @param devAddr Address of BNO080 in I2C bus.
+ @param sensors List of sensors to use.
+ @param nsensors Number of sensors.
+ */
+ Bno080(const char *device,uint8_t devAddr,const int sensors[],int nsensors);
+
+ /** @brief Constructor of Bno080 for UART interface.
+ @param device UART device name. Something like/dev/ttyS0, /dev/ttyUSB0.
+ @param sensors List of sensors to use.
+ @param nsensors Number of sensors.
+ */
+ Bno080(const char *device,const int sensors[],int nsensors);
+
+ /** @brief Destructor of Bno080.
+ */
+ ~Bno080(void);
+
+ /** @brief Accelerometer readings.
+ @return raw accelerometer readings.
+ */
+ Eigen::Vector3d accel(void);
+
+ /** @brief Magnetometer readings.
+ @return raw magnetometer readings.
+ */
+ Eigen::Vector3d mag(void);
+
+ /** @brief Gyroscope readings.
+ @return raw gyroscope readings.
+ */
+ Eigen::Vector3d gyro(void);
+
+ /** @brief Euler angles obtained through internal fusion.
+ Euler angles around ZYX are equivalent to Roll-Pitch-Yaw.
+ @return Euler angles.
+ */
+ Eigen::EulerAnglesZYXd euler(void);
+
+ /** @brief Unitary quaternion describing orientation obtained through internal fusion.
+ @return unitary quaternion.
+ */
+ Eigen::Quaterniond quaternion(void);
+
+ /** @brief Linear acceleration obtained through internal fusion.
+ @return linear acceleration.
+ */
+ Eigen::Vector3d linearAccel(void);
+
+ /** @brief Gravity vector obtained through internal fusion.
+ @return gravity vector.
+ */
+ Eigen::Vector3d gravity(void);
+
+ // Force 16-byte aligment of fixed-size vectorizable Eigen types,
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}
+#endif
@return Euler angles
*/
Eigen::EulerAnglesZYXd euler(void);
+
+ // Force 16-byte aligment of fixed-size vectorizable Eigen types,
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
TARGET=libimufusion.a
-SRCS=imu.cpp bno055.cpp lsm9ds1.cpp ekf.cpp
+SRCS=imu.cpp bno055.cpp lsm9ds1.cpp bno080.cpp ekf.cpp
-FLAGS=-O2 -Wall -MMD
-INCLUDE=-I. -I../include \
+FLAGS=-O2 -Wall -MMD -std=c++11
+INCLUDE=-I../include \
-I../../bno055/BNO055_driver -I../../bno055/bno055/include \
-I../../lsm9ds1/lsm9ds1_STdC/driver -I../../lsm9ds1/lsm9ds1/include \
+ -I../../bno080/sh2 -I../../bno080/bno080/include \
-I /usr/include/eigen3
LIBDIR=
LIBS=
--- /dev/null
+/******************************************************************************
+ IMU Fusion Library
+ BNO080 Abstraction
+ Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+extern "C"
+{
+#include <sh2.h>
+#include <sh2_err.h>
+#include <sh2_SensorValue.h>
+}
+
+#include <sh2_tty.h>
+//#include <sh2_i2c.h>
+#include <sh2_string.h>
+
+#include <bno080.hpp>
+
+namespace imufusion
+{
+
+Bno080::Bno080(const char *device,uint8_t devAddr,const int sensors[],int nsensors)
+{
+ interface_='i';
+// if(BNO080_i2c_init(device,devAddr,&bno080_))
+ throw Exception("Error initializing I2C interface.");
+ init(sensors,nsensors);
+}
+
+Bno080::Bno080(const char *device,const int sensors[],int nsensors)
+{
+ interface_='t';
+ if(sh2_tty_init(&sh2Hal_,device))
+ throw Exception("Error initializing TTY interface.");
+ init(sensors,nsensors);
+}
+
+static void eventCallback(void *cookie,sh2_AsyncEvent_t *event)
+{
+ // Do nothing
+}
+
+void sensorCallback(void *cookie,sh2_SensorEvent_t *event)
+{
+ Bno080 &bno080=*(Bno080 *)cookie;
+ int status;
+ sh2_SensorValue_t value;
+
+ if((status=sh2_decodeSensorEvent(&value,event)) != SH2_OK)
+ {
+ throw Bno080::Exception("Error decoding sensor event: "+std::string(sh2_strerror(status)));
+ return;
+ }
+
+ switch(value.sensorId)
+ {
+ case SH2_RAW_ACCELEROMETER:
+ case SH2_RAW_MAGNETOMETER:
+ case SH2_RAW_GYROSCOPE:
+ case SH2_TEMPERATURE:
+ case SH2_GAME_ROTATION_VECTOR:
+ case SH2_GEOMAGNETIC_ROTATION_VECTOR:
+ case SH2_GYRO_INTEGRATED_RV:
+ throw Bno080::Exception("Sensor not used for IMU fusion.");
+ break;
+
+ case SH2_ACCELEROMETER:
+ bno080.accel_[0]=value.un.accelerometer.x;
+ bno080.accel_[1]=value.un.accelerometer.y;
+ bno080.accel_[2]=value.un.accelerometer.z;
+ break;
+
+ case SH2_MAGNETIC_FIELD_CALIBRATED:
+ bno080.mag_[0]=value.un.magneticField.x;
+ bno080.mag_[1]=value.un.magneticField.y;
+ bno080.mag_[2]=value.un.magneticField.z;
+ break;
+
+ case SH2_MAGNETIC_FIELD_UNCALIBRATED:
+ bno080.mag_[0]=value.un.magneticFieldUncal.x;
+ bno080.mag_[1]=value.un.magneticFieldUncal.y;
+ bno080.mag_[2]=value.un.magneticFieldUncal.z;
+ break;
+
+ case SH2_GYROSCOPE_CALIBRATED:
+ bno080.gyro_[0]=value.un.gyroscope.x;
+ bno080.gyro_[1]=value.un.gyroscope.y;
+ bno080.gyro_[2]=value.un.gyroscope.z;
+ break;
+
+ case SH2_GYROSCOPE_UNCALIBRATED:
+ bno080.gyro_[0]=value.un.gyroscopeUncal.x;
+ bno080.gyro_[1]=value.un.gyroscopeUncal.y;
+ bno080.gyro_[2]=value.un.gyroscopeUncal.z;
+ break;
+
+ case SH2_ROTATION_VECTOR:
+ bno080.quaternion_.w()=value.un.rotationVector.real;
+ bno080.quaternion_.x()=value.un.rotationVector.i;
+ bno080.quaternion_.y()=value.un.rotationVector.j;
+ bno080.quaternion_.z()=value.un.rotationVector.k;
+ break;
+
+ case SH2_LINEAR_ACCELERATION:
+ bno080.linearAccel_[0]=value.un.linearAcceleration.x;
+ bno080.linearAccel_[1]=value.un.linearAcceleration.y;
+ bno080.linearAccel_[2]=value.un.linearAcceleration.z;
+ break;
+
+ case SH2_GRAVITY:
+ bno080.gravity_[0]=value.un.gravity.x;
+ bno080.gravity_[1]=value.un.gravity.y;
+ bno080.gravity_[2]=value.un.gravity.z;
+ break;
+
+ default:
+ throw Bno080::Exception("Unknown sensor");
+ break;
+ }
+}
+
+void Bno080::serviceThread(void)
+{
+ while(runService_) sh2_service();
+}
+
+void Bno080::init(const int sensors[],int nsensors)
+{
+ int status;
+ if((status=sh2_open(&sh2Hal_,eventCallback,this)) != SH2_OK)
+ throw Exception("Error initializing SH-2:"+std::string(sh2_strerror(status)));
+
+ if((status=sh2_setSensorCallback(sensorCallback,this)) != SH2_OK)
+ throw Exception("Error setting sensor callback: "+std::string(sh2_strerror(status)));
+
+ sh2_SensorConfig_t config;
+ config.changeSensitivityEnabled=false;
+ config.wakeupEnabled=false;
+ config.changeSensitivityRelative=false;
+ config.alwaysOnEnabled=false;
+ config.changeSensitivity=0;
+ config.batchInterval_us=0;
+ config.sensorSpecific=0;
+ config.reportInterval_us=100000;
+
+ for(int i=0;i < nsensors;i++)
+ {
+ if((status=sh2_setSensorConfig(sensors[i],&config)) != SH2_OK)
+ {
+ sh2_SensorMetadata_t metadata;
+ if((status=sh2_getMetadata(sensors[i],&metadata)) != SH2_OK)
+ throw("Error getting metadata for sensor: "+std::string(sh2_strerror(status)));
+
+ if(*(metadata.vendorId) == '\0') strcpy(metadata.vendorId,"Fused");
+
+ throw("Error enabling sensor "+std::string(metadata.vendorId)+": "+std::string(sh2_strerror(status)));
+ }
+ }
+
+ runService_=true;
+ serviceThread_=std::thread(&Bno080::serviceThread,this);
+}
+
+Bno080::~Bno080(void)
+{
+ runService_=false;
+ serviceThread_.join();
+
+ sh2_close();
+
+ switch(interface_)
+ {
+ case 't':
+ sh2_tty_shutdown(&sh2Hal_);
+ break;
+ case 'i':
+// sh2_i2c_close(&sh2Hal_);
+ break;
+ }
+}
+
+Eigen::Vector3d Bno080::accel(void)
+{
+ return accel_;
+}
+
+Eigen::Vector3d Bno080::mag(void)
+{
+ return mag_;
+}
+
+Eigen::Vector3d Bno080::gyro(void)
+{
+ return gyro_;
+}
+
+Eigen::EulerAnglesZYXd Bno080::euler(void)
+{
+ Eigen::Vector3d eulerVector=quaternion_.toRotationMatrix().eulerAngles(0,1,2);
+ Eigen::EulerAnglesZYXd euler(eulerVector(0),eulerVector(1),eulerVector(2));
+ return euler;
+}
+
+Eigen::Quaterniond Bno080::quaternion(void)
+{
+ return quaternion_;
+}
+
+Eigen::Vector3d Bno080::linearAccel(void)
+{
+ return linearAccel_;
+}
+
+Eigen::Vector3d Bno080::gravity(void)
+{
+ return gravity_;
+}
+
+} // namespace
TARGET=ekf_test
SRCS=$(TARGET).cpp
-FLAGS=-Wall -g -MMD
-INCLUDE=-I. -I../include \
+FLAGS=-Wall -g -MMD -std=c++11
+INCLUDE=-I../include \
-I../../bno055/BNO055_driver -I ../../bno055/bno055/include \
-I../../lsm9ds1/lsm9ds1_STdC/driver -I../../lsm9ds1/lsm9ds1/include \
+ -I../../bno080/sh2 -I../../bno080/bno080/include \
-I/usr/include/eigen3
LIBDIR=-L../lib \
-L../../bno055/BNO055_driver -L../../bno055/bno055/lib \
- -L../../lsm9ds1/lsm9ds1_STdC -L../../lsm9ds1/lsm9ds1/lib
-LIBS=-limufusion -lbno055ifc -lbno055 -llsm9ds1ifc -llsm9ds1
+ -L../../lsm9ds1/lsm9ds1_STdC -L../../lsm9ds1/lsm9ds1/lib \
+ -L../../bno080/sh2 -L../../bno080/bno080/lib
+LIBS= -limufusion \
+ -lbno055ifc -lbno055 \
+ -llsm9ds1ifc -llsm9ds1 \
+ -lbno080ifc -lsh2ifc -lsh2 -lrt -pthread
CXX=$(CROSS_COMPILE)g++
CXXFLAGS=$(FLAGS) $(INCLUDE)
#include <bno055.hpp>
#include <lsm9ds1.hpp>
+#include <bno080.hpp>
#include <ekf.hpp>
enum chip
usageHelp(argv[0]);
}
break;
-
+
case BNO080:
- usageHelp(argv[0]);
+ {
+ const int sensors[]=
+ {
+ SH2_ACCELEROMETER,
+// SH2_MAGNETIC_FIELD_CALIBRATED,
+ SH2_MAGNETIC_FIELD_UNCALIBRATED,
+// SH2_GYROSCOPE_CALIBRATED,
+ SH2_GYROSCOPE_UNCALIBRATED,
+ SH2_ROTATION_VECTOR, // Use for debug, comment-out for production
+ SH2_LINEAR_ACCELERATION, // Use for debug, comment-out for production
+ SH2_GRAVITY // Use for debug, comment-out for production
+ };
+ int nsensors=sizeof sensors/sizeof sensors[0];
+
+ switch(interface)
+ {
+ case 'i':
+ if(imuAddr==0)
+ {
+ std::cerr << "Bad I2C IMU address." << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ imu=new imufusion::Bno080(argv[optind],imuAddr,sensors,nsensors);
+ break;
+ case 's':
+ std::cerr << "SPI not supported yet." << std::endl;
+ exit(EXIT_FAILURE);
+ break;
+ case 't':
+ imu=new imufusion::Bno080(argv[optind],sensors,nsensors);
+ break;
+ default:
+ usageHelp(argv[0]);
+ }
+ break;
+ }
case LSM9DS1:
if(imuAddr==0)
projects / imufusion.git / commitdiff