diff --git a/.clang-format b/.clang-format index d677056..93444cf 100644 --- a/.clang-format +++ b/.clang-format @@ -63,3 +63,18 @@ SpacesInParentheses: false SpacesInSquareBrackets: false TabWidth: 4 UseTab: Never +IncludeBlocks: Regroup +IncludeCategories: + # The main header for a source file is automatically assigned priority 0 + # C system headers + - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' + Priority: 1 + # C++ system headers + - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' + Priority: 2 + # Other libraries' .h files + - Regex: '^<' + Priority: 3 + # Your project's .h files + - Regex: '^"' + Priority: 4 diff --git a/include/imu/ImuFactory.h b/include/imu/ImuFactory.h index 534205f..8203529 100644 --- a/include/imu/ImuFactory.h +++ b/include/imu/ImuFactory.h @@ -5,9 +5,10 @@ #ifndef MAV_IMU_SRC_IMU_IMUFACTORY_H_ #define MAV_IMU_SRC_IMU_IMUFACTORY_H_ -#include "imu_interface.h" #include +#include "imu_interface.h" + typedef std::shared_ptr ImuInterfacePtr; class ImuFactory { diff --git a/include/imu/adis16448.h b/include/imu/adis16448.h index cb5753d..6e1a5ce 100644 --- a/include/imu/adis16448.h +++ b/include/imu/adis16448.h @@ -4,9 +4,10 @@ #ifndef MAV_IMU_SRC_IMU_ADIS16448_H_ #define MAV_IMU_SRC_IMU_ADIS16448_H_ +#include + #include "imu_interface.h" #include "spi_driver.h" -#include class Adis16448 : public ImuInterface { public: @@ -48,6 +49,8 @@ class Adis16448 : public ImuInterface { */ ImuBurstResult burst() override; + vec3 calibrateGyro() override; + /*! * @brief Reads accelerometer and gyroscope config from registers and prints them out. @@ -82,7 +85,7 @@ class Adis16448 : public ImuInterface { void writeReg(uint8_t addr, const std::vector &data, const std::string &name) const; /** - * Run a test read sequence for SPI communcation. + * Run a test read sequence for SPI communication. */ bool testSPI(); diff --git a/include/imu/bmi088.h b/include/imu/bmi088.h index 74f6589..83e31a4 100644 --- a/include/imu/bmi088.h +++ b/include/imu/bmi088.h @@ -34,6 +34,8 @@ class Bmi088 : public ImuInterface { int getRaw(std::vector cmd) override; + vec3 calibrateGyro() override; + /*! * @brief Reads accelerometer and gyroscope config from registers and prints them out. diff --git a/include/imu/imu_interface.h b/include/imu/imu_interface.h index 67a5723..debea25 100644 --- a/include/imu/imu_interface.h +++ b/include/imu/imu_interface.h @@ -5,10 +5,11 @@ #ifndef MAV_IMU_INCLUDE_IMU_INTERFACE_H_ #define MAV_IMU_INCLUDE_IMU_INTERFACE_H_ -#include "spi_driver.h" #include #include +#include "spi_driver.h" + template struct vec3 { T x; @@ -71,6 +72,8 @@ class ImuInterface { */ virtual std::optional> getGyro() = 0; + virtual vec3 calibrateGyro() = 0; + /** * Gets acceleration data vector * diff --git a/include/imu_node.h b/include/imu_node.h index 44aa5d4..0541985 100644 --- a/include/imu_node.h +++ b/include/imu_node.h @@ -5,19 +5,24 @@ #ifndef MAV_IMU__IMU_TEST_H_ #define MAV_IMU__IMU_TEST_H_ -#include "imu/adis16448.h" -#include "spi_driver.h" +#include + +#include #include #include #include #include #include -#include +#include + +#include "imu/adis16448.h" +#include "spi_driver.h" class ImuNode { public: ImuNode(ImuInterface &imu_interface, int frequency); int run(); + void calibrateGyro(); inline static bool run_node = true; @@ -27,10 +32,15 @@ class ImuNode { void processTemperature(); void processFluidpressure(); + bool calibrateGyroServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + ros::Publisher imu_data_raw_pub_{}; ros::Publisher imu_mag_pub_{}; ros::Publisher imu_temp_pub_{}; - ros::Publisher imu_baro_pub_; + ros::Publisher imu_baro_pub_{}; + ros::Publisher imu_gyro_offset_pub_{}; + + ros::ServiceServer gyro_calib_srv_{}; ImuInterface &imu_interface_; int frequency_{}; diff --git a/launch/imu.launch b/launch/imu.launch index bff470c..2c356d6 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -1,14 +1,18 @@ - - - - - + + + + + + - - - - - + + + + + + diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index 3e206ed..6b7c8d7 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -3,13 +3,17 @@ // #include "imu/adis16448.h" -#include "imu/adis16448_cmds.h" + +#include + #include #include + #include #include #include -#include + +#include "imu/adis16448_cmds.h" Adis16448::Adis16448(const std::string &path) : spi_driver_(path) {} @@ -230,6 +234,51 @@ int Adis16448::signedWordToInt(const std::vector &word) { return (((int) *(signed char *) (word.data())) * 1 << CHAR_BIT) | word[1]; } +vec3 Adis16448::calibrateGyro() { + // set offsets to zero + writeReg(XGYRO_OFF, {0x00, 0x00}, "XGYRO_OFF"); + writeReg(YGYRO_OFF, {0x00, 0x00}, "YGYRO_OFF"); + writeReg(ZGYRO_OFF, {0x00, 0x00}, "ZGYRO_OFF"); + + // read offsets + vec3 gyro_off_raw{}; + gyro_off_raw.x = (double) signedWordToInt(readReg(XGYRO_OFF)); + gyro_off_raw.y = (double) signedWordToInt(readReg(YGYRO_OFF)); + gyro_off_raw.z = (double) signedWordToInt(readReg(ZGYRO_OFF)); + vec3 gyro_off = convertGyro(gyro_off_raw); + LOG(I, "Adis16448 Starting Gyro Calibration. Keep IMU steady for 5s."); + LOG(D, "Gyro offset reset to:\t" << gyro_off.x << " , " << gyro_off.y << " , " << gyro_off.z); + + // Store sensitivity and sample period + auto sens_avg = readReg(SENS_AVG); + auto smpl_prd = readReg(SMPL_PRD); + + // Calibrate + auto calib_sens_avg = sens_avg; + auto calib_smpl_prd = smpl_prd; + calib_sens_avg[0] = 0x01; // Set high sensitivity + calib_smpl_prd[0] = 0x0C; // Sample for (2^12 / 819.2) = 5s + writeReg(SENS_AVG, calib_sens_avg, "CALIB_SENS_AVG"); + writeReg(SMPL_PRD, calib_smpl_prd, "CALIB_SMPL_PRD"); + usleep(5 * 1e6); // wait for next measurement + writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets + usleep(ms_); + + gyro_off_raw.x = (double) signedWordToInt(readReg(XGYRO_OFF)); + gyro_off_raw.y = (double) signedWordToInt(readReg(YGYRO_OFF)); + gyro_off_raw.z = (double) signedWordToInt(readReg(ZGYRO_OFF)); + gyro_off = convertGyro(gyro_off_raw); + LOG(I, + "Adis16448 Setting Gyro offsets to:\t" << gyro_off.x << " , " << gyro_off.y << " , " + << gyro_off.z); + + // Reset sensitivity and sample period + writeReg(SENS_AVG, sens_avg, "SENS_AVG"); + writeReg(SMPL_PRD, smpl_prd, "SMPL_PRD"); + + return gyro_off; +} + bool Adis16448::setBurstCRCEnabled(bool b) { if (b) { auto msc_ctrl = readReg(MSC_CTRL); diff --git a/src/imu/bmi088.cpp b/src/imu/bmi088.cpp index a72a1d5..d74c1a6 100644 --- a/src/imu/bmi088.cpp +++ b/src/imu/bmi088.cpp @@ -7,9 +7,10 @@ #include #include #include +#include + #include #include -#include Bmi088::Bmi088(std::string acc_path, std::string gyro_path) : acc_spi_driver_(std::move(acc_path)), gyro_spi_driver_(std::move(gyro_path)) {} @@ -50,8 +51,8 @@ bool Bmi088::setupBmiSpi() { LOG(I, rslt == BMI08_OK, "Accelerometer soft reset."); if (rslt != BMI08_OK) { return false; } - // Issue #19 (YR0th - Yann Roth) This block needs to be here for config to load correctly - // https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BMI088-accelerometer-noise-too-high/td-p/57674 + // Issue #19 (YR0th - Yann Roth) This block needs to be here for config to load correctly + // https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BMI088-accelerometer-noise-too-high/td-p/57674 rslt = bmi08a_load_config_file(&dev_); printErrorCodeResults("bmi08a_load_config_file", rslt); if (rslt != BMI08_OK) { return false; } @@ -69,8 +70,6 @@ bool Bmi088::setupBmiSpi() { printErrorCodeResults("bmi08g_set_power_mode", rslt); if (rslt != BMI08_OK) { return false; } - - rslt = bmi08xa_set_meas_conf(&dev_); printErrorCodeResults("bmi08xa_set_meas_conf", rslt); if (rslt != BMI08_OK) { return false; } @@ -313,6 +312,11 @@ double Bmi088::computeAccOdr(uint16_t accel_cfg_odr) { return acc_odr_min_ * (1 << (accel_cfg_odr - BMI08_ACCEL_ODR_12_5_HZ)); } +vec3 Bmi088::calibrateGyro() { + LOG(E, "Bmi088 calibrateGyro not implemented."); + return {0., 0., 0.}; +} + void Bmi088::printImuConfig() { int8_t rslt = bmi08g_get_meas_conf(&dev_); printErrorCodeResults("bmi08g_get_meas_conf", rslt); @@ -347,4 +351,4 @@ ImuBurstResult Bmi088::burst() { } return ret; -} \ No newline at end of file +} diff --git a/src/imu_node.cpp b/src/imu_node.cpp index 91a87c6..0c4b95a 100644 --- a/src/imu_node.cpp +++ b/src/imu_node.cpp @@ -7,10 +7,13 @@ ImuNode::ImuNode(ImuInterface &imu, int frequency) : imu_interface_(imu), frequency_(frequency) { ros::NodeHandle nh; - imu_data_raw_pub_ = nh.advertise("imu/data_raw", 1); - imu_mag_pub_ = nh.advertise("imu/mag", 1); - imu_temp_pub_ = nh.advertise("imu/temp", 1); - imu_baro_pub_ = nh.advertise("imu/pressure", 1); + imu_data_raw_pub_ = nh.advertise("imu/data_raw", 1); + imu_mag_pub_ = nh.advertise("imu/mag", 1); + imu_temp_pub_ = nh.advertise("imu/temp", 1); + imu_baro_pub_ = nh.advertise("imu/pressure", 1); + imu_gyro_offset_pub_ = nh.advertise("imu/gyro_bias", 1, true); + gyro_calib_srv_ = + nh.advertiseService("imu/run_gyro_calib", &ImuNode::calibrateGyroServiceCallback, this); } int ImuNode::run() { @@ -90,3 +93,19 @@ void ImuNode::processFluidpressure() { imu_baro_pub_.publish(pressure_msg); } } + +void ImuNode::calibrateGyro() { + auto gyro_offset = imu_interface_.calibrateGyro(); + geometry_msgs::Vector3Stamped goff_msg{}; + goff_msg.header.stamp = time_now_; + goff_msg.vector.x = gyro_offset.x; + goff_msg.vector.y = gyro_offset.y; + goff_msg.vector.z = gyro_offset.z; + imu_gyro_offset_pub_.publish(goff_msg); +} + +bool ImuNode::calibrateGyroServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) { + calibrateGyro(); + return true; +} diff --git a/src/main.cpp b/src/main.cpp index 84ebe29..f00a62a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,10 +1,12 @@ -#include "imu/ImuFactory.h" -#include "imu/adis16448.h" -#include "imu_node.h" #include + #include #include +#include "imu/ImuFactory.h" +#include "imu/adis16448.h" +#include "imu_node.h" + void SignalHandler(int signum) { if (signum == SIGINT) { LOG(I, "Received sigint. Shutting down."); @@ -17,10 +19,10 @@ int main(int argc, char **argv) { ros::init(argc, argv, "mav_imu_node"); ros::NodeHandle nh_private("~"); - std::string spi_path = - nh_private.param("spi_path", std::string("/dev/spidev0.1")); + std::string spi_path = nh_private.param("spi_path", std::string("/dev/spidev0.1")); int frequency = nh_private.param("frequency", 200); std::string imu_name = nh_private.param("imu", std::string("adis16448")); + bool calibrate_gyro = nh_private.param("calibrate_gyro", false); LOG(I, "Spi path: " << spi_path); LOG(I, "Loop frequency " << frequency); @@ -32,6 +34,7 @@ int main(int argc, char **argv) { } ImuNode node{*imu_interface, frequency}; + if (calibrate_gyro) { node.calibrateGyro(); } signal(SIGINT, SignalHandler); node.run();