{
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.
/** @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.
*/
--- /dev/null
+/******************************************************************************
+ IMU Fusion Library
+ LSM9DS1 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 lsm9ds1.hpp
+ @brief LSM9DS1 abstraction.
+ @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+
+#ifndef __IMUFUSION_LSM9DS1_H__
+#define __IMUFUSION_LSM9DS1_H__
+
+#include <stdint.h>
+
+#include <lsm9ds1_i2c.h>
+
+#include <imu.hpp>
+
+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
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=
--- /dev/null
+/******************************************************************************
+ IMU Fusion Library
+ IMU 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/>.
+
+*******************************************************************************/
+
+
+#include <imu.hpp>
+
+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
--- /dev/null
+/******************************************************************************
+ IMU Fusion Library
+ LSM9DS1 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/>.
+
+*******************************************************************************/
+
+#include <lsm9ds1_units.h>
+#include <lsm9ds1.hpp>
+
+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
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)
*******************************************************************************/
+#include <strings.h>
#include <unistd.h>
#include <sys/time.h>
#include <iostream>
#include <bno055.hpp>
+#include <lsm9ds1.hpp>
#include <ekf.hpp>
-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)
{
projects / imufusion.git / commitdiff