From: Walter Fetter Lages Date: Sun, 18 Aug 2019 07:31:44 +0000 (-0300) Subject: Add abstraction of LSM9DS1. X-Git-Tag: f1~5 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=61313997d8a29de5e2e275e2dd88f31cae505709;p=imufusion.git Add abstraction of LSM9DS1. --- diff --git a/include/bno055.hpp b/include/bno055.hpp index 305117d..2ec8758 100644 --- a/include/bno055.hpp +++ b/include/bno055.hpp @@ -42,7 +42,7 @@ class Bno055: public Imu { private: struct bno055_t bno055_; ///< BNO055_driver handle. - char interface_; ///< Interface type: 'i' for I2C, 't" for UART. + char interface_; ///< Interface type: 'i' for I2C, 't' for UART. /** @brief Initialize Bno055. @param mode Operation mode. diff --git a/include/imu.hpp b/include/imu.hpp index 97f1586..a72db0e 100644 --- a/include/imu.hpp +++ b/include/imu.hpp @@ -58,22 +58,22 @@ class Imu /** @brief Euler angles obtained through internal fusion. @return Euler angles. */ - virtual Eigen::EulerAnglesZYXd euler(void)=0; + virtual Eigen::EulerAnglesZYXd euler(void); /** @brief Unitary quaternion describing orientation obtained through internal fusion. @return unitary quaternion. */ - virtual Eigen::Quaterniond quaternion(void)=0; + virtual Eigen::Quaterniond quaternion(void); /** @brief Linear acceleration obtained through internal fusion. @return linear acceleration. */ - virtual Eigen::Vector3d linearAccel(void)=0; + virtual Eigen::Vector3d linearAccel(void); /** @brief Gravity vector obtained through internal fusion. @return gravity vector. */ - virtual Eigen::Vector3d gravity(void)=0; + virtual Eigen::Vector3d gravity(void); /** @brief Handle exceptions. */ diff --git a/include/lsm9ds1.hpp b/include/lsm9ds1.hpp new file mode 100644 index 0000000..88c88f8 --- /dev/null +++ b/include/lsm9ds1.hpp @@ -0,0 +1,83 @@ +/****************************************************************************** + IMU Fusion Library + LSM9DS1 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 lsm9ds1.hpp + @brief LSM9DS1 abstraction. + @author Walter Fetter Lages +*/ + +#ifndef __IMUFUSION_LSM9DS1_H__ +#define __IMUFUSION_LSM9DS1_H__ + +#include + +#include + +#include + +namespace imufusion +{ + +/** @brief LSM9DS1 Abstraction. +*/ +class Lsm9ds1: public Imu +{ + private: + lsm9ds1_ctx_t imuCtx_; ///< LSM9DS1 driver handle for accelerometer and gyroscope. + lsm9ds1_ctx_t magCtx_; ///< LSM9DS1 driver handle gor magnetometer. + char interface_; ///< Interface type: 'i' for I2C, 's' for SPI. + + /** @brief Initialize Lsm9ds1. + @param mode Operation mode. + */ + void init(void); + + public: + /** @brief Constructor of Lsm9ds1 for I2C interface. + @param device I2C device name. Something like /dev/i2c-0. + @param magAddr Address of LSM9DS1 magnetometer in I2C bus. + @param imuAddr Address of LSM9DS1 accelerometer and gyroscope in I2C bus. + @param interface Interface 'i' for I2C, 's' for SPI. + */ + Lsm9ds1(const char *device,uint8_t magAddr,uint8_t imuAddr,char interface); + + /** @brief Destructor of Lsm9ds1. + */ + ~Lsm9ds1(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); +}; + +} +#endif diff --git a/lib/Makefile b/lib/Makefile index 5174035..d19ea5c 100644 --- a/lib/Makefile +++ b/lib/Makefile @@ -1,9 +1,10 @@ TARGET=libimufusion.a -SRCS=bno055.cpp ekf.cpp +SRCS=imu.cpp bno055.cpp lsm9ds1.cpp ekf.cpp FLAGS=-O2 -Wall -MMD INCLUDE=-I. -I../include \ -I../../bno055/BNO055_driver -I../../bno055/bno055/include \ + -I../../lsm9ds1/lsm9ds1_STdC/driver -I../../lsm9ds1/lsm9ds1/include \ -I /usr/include/eigen3 LIBDIR= LIBS= diff --git a/lib/imu.cpp b/lib/imu.cpp new file mode 100644 index 0000000..f2be207 --- /dev/null +++ b/lib/imu.cpp @@ -0,0 +1,62 @@ +/****************************************************************************** + IMU Fusion Library + IMU 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 + . + +*******************************************************************************/ + + +#include + +namespace imufusion +{ + +Eigen::EulerAnglesZYXd Imu::euler(void) +{ + throw Exception("Error reading Euler angles."); + Eigen::EulerAnglesZYXd eulerAngles(0.0,0.0,0.0); + return eulerAngles; +} + +Eigen::Quaterniond Imu::quaternion(void) +{ + throw Exception("Error reading quaternion."); + Eigen::Quaterniond q; + q.w()=1.0; + q.x()=0.0; + q.y()=0.0; + q.z()=0.0; + return q; +} + +Eigen::Vector3d Imu::linearAccel(void) +{ + throw Exception("Error reading linear acceleration."); + Eigen::Vector3d accel; + accel.setZero(); + return accel; +} + +Eigen::Vector3d Imu::gravity(void) +{ + throw Exception("Error reading gravity."); + Eigen::Vector3d g; + g.setZero(); + return g; +} + +} // namespace diff --git a/lib/lsm9ds1.cpp b/lib/lsm9ds1.cpp new file mode 100644 index 0000000..06515ca --- /dev/null +++ b/lib/lsm9ds1.cpp @@ -0,0 +1,149 @@ +/****************************************************************************** + IMU Fusion Library + LSM9DS1 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 + . + +*******************************************************************************/ + +#include +#include + +namespace imufusion +{ + +Lsm9ds1::Lsm9ds1(const char *device,uint8_t magAddr,uint8_t imuAddr,char interface) +{ + interface_=interface; + switch(interface) + { + case 'i': + if(lsm9ds1_i2c_init(device,magAddr,imuAddr,&magCtx_,&imuCtx_)) + throw Exception("Error initializing I2C interface."); + break; + case 's': + // if(lsm9ds1_spi_init(device,magAddr,imuAddr,&magCtx_,&imuCtx_)) + throw Exception("Error initializing SPI interface."); + break; + default: + throw Exception("Unknown interface."); + } + init(); +} + +void Lsm9ds1::init(void) +{ + if(lsm9ds1_dev_reset_set(&magCtx_,&imuCtx_,PROPERTY_ENABLE)!=0) + throw Exception("Error resetting."); + + uint8_t in_reset; + do if(lsm9ds1_dev_reset_get(&magCtx_,&imuCtx_,&in_reset)!=0) + throw Exception("Error waiting for reset."); + while(in_reset); + + if(lsm9ds1_block_data_update_set(&magCtx_,&imuCtx_,PROPERTY_ENABLE)!=0) + throw Exception("Error setting block data update."); + + if(lsm9ds1_xl_full_scale_set(&imuCtx_,LSM9DS1_4g)!=0) + throw Exception("Error setting accelerometer scale."); + + if(lsm9ds1_gy_full_scale_set(&imuCtx_,LSM9DS1_2000dps)!=0) + throw Exception("Error setting gyroscope scale."); + + if(lsm9ds1_mag_full_scale_set(&magCtx_,LSM9DS1_16Ga)!=0) + throw Exception("Error setting magnetometer scale."); + + if(lsm9ds1_xl_filter_aalias_bandwidth_set(&imuCtx_,LSM9DS1_AUTO)!=0) + throw Exception("Error setting accelerometer anti-aliasing filter."); + + if(lsm9ds1_xl_filter_lp_bandwidth_set(&imuCtx_,LSM9DS1_LP_ODR_DIV_50)!=0) + throw Exception("Error setting accelerometer low pass filter cutoff frequency."); + + if(lsm9ds1_xl_filter_out_path_set(&imuCtx_,LSM9DS1_LP_OUT)!=0) + throw Exception("Error setting accelerometer output filter path."); + + if(lsm9ds1_gy_filter_lp_bandwidth_set(&imuCtx_,LSM9DS1_LP_ULTRA_LIGHT)!=0) + throw Exception("Error setting gyroscope low pass filter bandwidth."); + if(lsm9ds1_gy_filter_hp_bandwidth_set(&imuCtx_,LSM9DS1_HP_MEDIUM)!=0) + throw Exception("Error setting gyroscope high pass filter bandwidth."); + if(lsm9ds1_gy_filter_out_path_set(&imuCtx_,LSM9DS1_LPF1_HPF_LPF2_OUT)!=0) + throw Exception("Error setting gyroscope output filter path."); + + if(lsm9ds1_imu_data_rate_set(&imuCtx_,LSM9DS1_IMU_59Hz5)!=0) + throw Exception("Error setting accelerometer and gyroscope data rate."); + if(lsm9ds1_mag_data_rate_set(&magCtx_,LSM9DS1_MAG_UHP_10Hz)!=0) + throw Exception("Error setting magnetometer data rate."); +} + +Lsm9ds1::~Lsm9ds1(void) +{ + switch(interface_) + { + case 's': +// lsm9ds1_spi_close(&magCtx_,&imuCtx_); + break; + case 'i': + lsm9ds1_i2c_close(&magCtx_,&imuCtx_); + break; + } +} + +Eigen::Vector3d Lsm9ds1::accel(void) +{ + lsm9ds1_status_t status; + if(lsm9ds1_dev_status_get(&magCtx_,&imuCtx_,&status)!=0) + throw Exception("Error getting device status."); + + axis3bit16_t accel; + if(status.status_imu.xlda && lsm9ds1_acceleration_raw_get(&imuCtx_,accel.u8bit)!=0) + throw Exception("Error getting acceleration."); + Eigen::Vector3d accelVector; + for(int i=0;i < 3;i++) accelVector[i]=lsm9ds1_from_fs4g_to_mpssq(accel.i16bit[i]); + + return accelVector; +} + +Eigen::Vector3d Lsm9ds1::mag(void) +{ + lsm9ds1_status_t status; + if(lsm9ds1_dev_status_get(&magCtx_,&imuCtx_,&status)!=0) + throw Exception("Error getting device status."); + + axis3bit16_t mag; + if(status.status_mag.zyxda && lsm9ds1_magnetic_raw_get(&magCtx_,mag.u8bit)!=0) + throw Exception("Error getting magnetometer."); + Eigen::Vector3d magVector; + for(int i=0;i < 3;i++) magVector[i]=lsm9ds1_from_fs16gauss_to_uT(mag.i16bit[i]); + + return magVector; +} + +Eigen::Vector3d Lsm9ds1::gyro(void) +{ + lsm9ds1_status_t status; + if(lsm9ds1_dev_status_get(&magCtx_,&imuCtx_,&status)!=0) + throw Exception("Error getting device status."); + + axis3bit16_t gyro; + if(status.status_imu.gda && lsm9ds1_angular_rate_raw_get(&imuCtx_,gyro.u8bit)!=0) + throw Exception("Error getting gyroscope."); + Eigen::Vector3d gyroVector; + for(int i=0;i < 3;i++) gyroVector[i]=lsm9ds1_from_fs2000dps_to_rps(gyro.i16bit[i]); + + return gyroVector; +} + +} // namespace diff --git a/test/Makefile b/test/Makefile index 45190cd..647bd1d 100644 --- a/test/Makefile +++ b/test/Makefile @@ -4,9 +4,12 @@ SRCS=$(TARGET).cpp FLAGS=-Wall -g -MMD INCLUDE=-I. -I../include \ -I../../bno055/BNO055_driver -I ../../bno055/bno055/include \ + -I../../lsm9ds1/lsm9ds1_STdC/driver -I../../lsm9ds1/lsm9ds1/include \ -I/usr/include/eigen3 -LIBDIR=-L../lib -L../../bno055/BNO055_driver -L../../bno055/bno055/lib -LIBS=-limufusion -lbno055ifc -lbno055 +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 CXX=$(CROSS_COMPILE)g++ CXXFLAGS=$(FLAGS) $(INCLUDE) diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp index d7161a9..33e210e 100644 --- a/test/ekf_test.cpp +++ b/test/ekf_test.cpp @@ -19,51 +19,119 @@ *******************************************************************************/ +#include #include #include #include #include +#include #include -void usage_help(const char *name) +enum chip { - std::cerr << "Usage: " << name << " <-i i2c_address | -t> device" << std::endl - << "For I2C, device is something like /dev/i2c-0." << std::endl - << "For TTY, device is something like /dev/ttyUSB0." << std::endl; + BNO055, + BNO080, + LSM9DS1, + UNKNOWN +}; + +void usageHelp(const char *name) +{ + std::cerr << "Usage: " << name << " <-c chip> <-i imu_address | -s imu_address | -t > [-m mag_address] device" << std::endl + << std::endl + << "\t-i for I2C," << std::endl + << "\t-s for SPI," << std::endl + << "\t-t for UART (tty)," << std::endl + << "\tchip is BNO055, BNO080 or LSM9DS1" << std::endl + << "\tdevice is:\t/dev/i2c-? for I2C,\n\t\t\t/dev/spidev?.? for SPI and\n\t\t\t/dev/ttyS?? or /dev/ttyUSB? for UART." << std::endl; exit(EXIT_FAILURE); } int main(int argc,char *argv[]) { - if(argc < 2) usage_help(argv[0]); + if(argc < 2) usageHelp(argv[0]); - imufusion::Imu *imu; + char interface='u'; + uint8_t imuAddr=0; + uint8_t magAddr=0; + int chip; int opt; + while((opt=getopt(argc,argv,"i:tc:")) != -1) + { + switch(opt) + { + case 'c': + if(strcasecmp(optarg,"BNO055")==0) chip=BNO055; + else if(strcasecmp(optarg,"BNO080")==0) chip=BNO080; + else if(strcasecmp(optarg,"LSM9DS1")==0) chip=BNO080; + else chip=UNKNOWN; + break; + case 'i': + interface='i'; + imuAddr=strtol(optarg,NULL,0); + break; + case 'm': + magAddr=strtol(optarg,NULL,0); + break; + case 's': + interface='s'; + imuAddr=strtol(optarg,NULL,0); + break; + case 't': + interface='t'; + break; + default: + usageHelp(argv[0]); + } + } + + imufusion::Imu *imu; try { - while((opt=getopt(argc,argv,"i:t")) != -1) + switch(chip) { - switch(opt) - { - case 'i': + case BNO055: + switch(interface) { - /* I2C interface */ - uint8_t devAddr=strtol(optarg,NULL,0); - imu=new imufusion::Bno055(argv[optind],devAddr,BNO055_OPERATION_MODE_NDOF); // Use this mode for debug, use AMG for production - break; - } - case 't': + case 'i': + if(imuAddr==0) + { + std::cerr << "Bad I2C IMU address." << std::endl; + exit(EXIT_FAILURE); + } + imu=new imufusion::Bno055(argv[optind],imuAddr,BNO055_OPERATION_MODE_AMG); // Use NDOF for debug, use AMG for production + break; + case 't': + imu=new imufusion::Bno055(argv[optind],BNO055_OPERATION_MODE_NDOF); // Use NDOF for debug, use AMG for production + break; + default: + usageHelp(argv[0]); + } + break; + + case BNO080: + usageHelp(argv[0]); + + case LSM9DS1: + if(imuAddr==0) { - /* TTY interface */ - imu=new imufusion::Bno055(argv[optind],BNO055_OPERATION_MODE_NDOF); - break; + std::cerr << "Bad IMU address." << std::endl; + exit(EXIT_FAILURE); } - default: - usage_help(argv[0]); - } + if(magAddr==0) + { + std::cerr << "Bad Magnetometer address." << std::endl; + exit(EXIT_FAILURE); + } + imu=new imufusion::Lsm9ds1(argv[optind],magAddr,imuAddr,interface); + break; + + default: + usageHelp(argv[0]); } + } catch(imufusion::Imu::Exception &e) {