From: Walter Fetter Lages Date: Mon, 19 Aug 2019 01:26:01 +0000 (-0300) Subject: Add abstraction of BNO080. X-Git-Tag: f1~3 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e532018c6e2e12b3ac3aaa0bffd0564047380564;p=imufusion.git Add abstraction of BNO080. --- diff --git a/include/bno080.hpp b/include/bno080.hpp new file mode 100644 index 0000000..0aa9b8d --- /dev/null +++ b/include/bno080.hpp @@ -0,0 +1,142 @@ +/****************************************************************************** + IMU Fusion Library + BNO080 Abstraction + Copyright (C) 2019 Walter Fetter Lages + + 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 + . + +*******************************************************************************/ + +/** @file bno080.hpp + @brief BNO080 abstraction. + @author Walter Fetter Lages +*/ + +#ifndef __IMUFUSION_BNO080_H__ +#define __IMUFUSION_BNO080_H__ + +#include + +#include + +extern "C" +{ +#include +} + +#include + +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 diff --git a/include/ekf.hpp b/include/ekf.hpp index 88bfd1e..f42b9ce 100644 --- a/include/ekf.hpp +++ b/include/ekf.hpp @@ -99,6 +99,9 @@ class Ekf @return Euler angles */ Eigen::EulerAnglesZYXd euler(void); + + // Force 16-byte aligment of fixed-size vectorizable Eigen types, + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } diff --git a/lib/Makefile b/lib/Makefile index d19ea5c..3c298f6 100644 --- a/lib/Makefile +++ b/lib/Makefile @@ -1,10 +1,11 @@ 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= diff --git a/lib/bno080.cpp b/lib/bno080.cpp new file mode 100644 index 0000000..c8add04 --- /dev/null +++ b/lib/bno080.cpp @@ -0,0 +1,235 @@ +/****************************************************************************** + IMU Fusion Library + BNO080 Abstraction + Copyright (C) 2019 Walter Fetter Lages + + 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 + . + +*******************************************************************************/ + +extern "C" +{ +#include +#include +#include +} + +#include +//#include +#include + +#include + +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 diff --git a/test/Makefile b/test/Makefile index 647bd1d..bf6b844 100644 --- a/test/Makefile +++ b/test/Makefile @@ -1,15 +1,20 @@ 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) diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp index 33e210e..85381ef 100644 --- a/test/ekf_test.cpp +++ b/test/ekf_test.cpp @@ -27,6 +27,7 @@ #include #include +#include #include enum chip @@ -110,9 +111,44 @@ int main(int argc,char *argv[]) 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)