From 4fc7d64ce5a4af09339e56d67217200e4211e74b Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Sat, 29 Oct 2022 11:53:01 +0200 Subject: [PATCH 1/7] Added method for gyro calibration for adis16448 (not tested yet) --- .clang-format | 15 ++++++++++++++ include/imu/adis16448.h | 6 ++++-- include/imu/imu_interface.h | 5 ++++- include/imu_node.h | 15 ++++++++++---- src/imu/adis16448.cpp | 41 +++++++++++++++++++++++++++++++++++++ src/imu_node.cpp | 7 +++++++ 6 files changed, 82 insertions(+), 7 deletions(-) diff --git a/.clang-format b/.clang-format index 1e64b8a..4b2adc0 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/adis16448.h b/include/imu/adis16448.h index 1f1fd54..0c17b0d 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: @@ -50,6 +51,7 @@ class Adis16448 : public ImuInterface { */ ImuBurstResult burst() override; + bool calibrateGyro() override; /** * Free file descriptor * @return true if successful, otherwise false and errno is set. @@ -76,7 +78,7 @@ class Adis16448 : public ImuInterface { const std::string &name); /** - * Run a test read sequence for SPI communcation. + * Run a test read sequence for SPI communication. */ bool testSPI(); diff --git a/include/imu/imu_interface.h b/include/imu/imu_interface.h index 7ef4ef9..aa2267d 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; @@ -72,6 +73,8 @@ class ImuInterface { */ virtual std::optional> getGyro() = 0; + virtual bool calibrateGyro() = 0; + /** * Gets acceleration data vector * diff --git a/include/imu_node.h b/include/imu_node.h index a6c9c53..ea9531e 100644 --- a/include/imu_node.h +++ b/include/imu_node.h @@ -5,14 +5,17 @@ #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 "imu/adis16448.h" +#include "spi_driver.h" class ImuNode { public: @@ -28,10 +31,14 @@ class ImuNode { void processTemperature(); void processFluidpressure(); + bool runGyroCalibration(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::ServiceServer gyro_calib_srv_{}; ImuInterface &imu_interface_; int frequency_{}; diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index 010dae6..df9f5cd 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -226,6 +226,47 @@ int Adis16448::signedWordToInt(const std::vector &word) { return (((int) *(signed char *) (word.data())) * 1 << CHAR_BIT) | word[1]; } +bool Adis16448::calibrateGyro(){ + // set offsets to zero + writeReg(0x1A, {0x00, 0x00}, "XGYRO_OFF"); + writeReg(0x1C, {0x00, 0x00}, "YGYRO_OFF"); + writeReg(0x1E, {0x00, 0x00}, "ZGYRO_OFF"); + + // read offsets + auto xgyro_off = readReg(0x1A); + auto ygyro_off = readReg(0x1C); + auto zgyro_off = readReg(0x1E); + + std::cout << "Gyro offers after reset:\n" << xgyro_off[0] << " , " << xgyro_off[1] << " , " << ygyro_off[1] << " , " << zgyro_off[1] << std::endl; + + // 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] = 0x0F; // Sample for (2^15 / 819.2) = 40s + std::cout << "Starting Gyro Calibration. Keep IMU steady and wait for 40s.." << std::endl; + writeReg(SENS_AVG, calib_sens_avg, "CALIB_SENS_AVG"); + writeReg(SMPL_PRD, calib_smpl_prd, "CALIB_SMPL_PRD"); + usleep(40*1e6); // wait for next measurement + writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets + usleep(ms_); + + ygyro_off = readReg(0x1C); + xgyro_off = readReg(0x1A); + zgyro_off = readReg(0x1E); + std::cout << "Gyro offsets after calibration\n" << "[" << xgyro_off[0] << " , " << xgyro_off[1] << "]" << " , " << ygyro_off[1] << " , " << zgyro_off[1] << std::endl; + + // Reset sensitivity and sample period + writeReg(SENS_AVG, sens_avg, "SENS_AVG"); + writeReg(SMPL_PRD, smpl_prd, "SMPL_PRD"); + + return true; +} + bool Adis16448::setBurstCRCEnabled(bool b) { if (b) { auto msc_ctrl = readReg(MSC_CTRL); diff --git a/src/imu_node.cpp b/src/imu_node.cpp index f6c1ab6..76f4970 100644 --- a/src/imu_node.cpp +++ b/src/imu_node.cpp @@ -12,6 +12,7 @@ ImuNode::ImuNode(ImuInterface &imu, int frequency) imu_mag_pub_ = nh.advertise("imu/mag", 1); imu_temp_pub_ = nh.advertise("imu/temp", 1); imu_baro_pub_ = nh.advertise("imu/pressure", 1); + gyro_calib_srv_ = nh.advertiseService("imu/run_gyro_calib", &ImuNode::runGyroCalibration, this); } bool ImuNode::init() { @@ -96,4 +97,10 @@ void ImuNode::processFluidpressure() { imu_baro_pub_.publish(pressure_msg); } + } + + bool ImuNode::runGyroCalibration(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res ) { + imu_interface_.calibrateGyro(); + return true; + } From 4d9f0689875b42c820a733e49e1ef2577c3d54e0 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 2 May 2024 12:36:23 +0200 Subject: [PATCH 2/7] Changed gyro calib to 3s and switched logging to lpp --- include/imu/adis16448.h | 1 + src/imu/adis16448.cpp | 28 +++++++++++++++++++--------- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/include/imu/adis16448.h b/include/imu/adis16448.h index d26189c..b535513 100644 --- a/include/imu/adis16448.h +++ b/include/imu/adis16448.h @@ -50,6 +50,7 @@ class Adis16448 : public ImuInterface { ImuBurstResult burst() override; bool calibrateGyro() override; + /*! * @brief Reads accelerometer and gyroscope config from registers and prints them out. diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index 99fd3b1..6df03c1 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,7 +234,7 @@ int Adis16448::signedWordToInt(const std::vector &word) { return (((int) *(signed char *) (word.data())) * 1 << CHAR_BIT) | word[1]; } -bool Adis16448::calibrateGyro(){ +bool Adis16448::calibrateGyro() { // set offsets to zero writeReg(0x1A, {0x00, 0x00}, "XGYRO_OFF"); writeReg(0x1C, {0x00, 0x00}, "YGYRO_OFF"); @@ -241,7 +245,11 @@ bool Adis16448::calibrateGyro(){ auto ygyro_off = readReg(0x1C); auto zgyro_off = readReg(0x1E); - std::cout << "Gyro offers after reset:\n" << xgyro_off[0] << " , " << xgyro_off[1] << " , " << ygyro_off[1] << " , " << zgyro_off[1] << std::endl; + LOG(I, + "Gyro offsets after reset:\n" + << xgyro_off[0] << " , " << xgyro_off[1] << " , " << ygyro_off[1] << " , " + << zgyro_off[1]); + LOG(I, "Starting Gyro Calibration. Keep IMU steady and wait for 3s."); // Store sensitivity and sample period auto sens_avg = readReg(SENS_AVG); @@ -250,19 +258,21 @@ bool Adis16448::calibrateGyro(){ // 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] = 0x0F; // Sample for (2^15 / 819.2) = 40s - std::cout << "Starting Gyro Calibration. Keep IMU steady and wait for 40s.." << std::endl; + calib_sens_avg[0] = 0x01; // Set high sensitivity + calib_smpl_prd[0] = 0x0B; // Sample for (2^11 / 819.2) = 2.5s writeReg(SENS_AVG, calib_sens_avg, "CALIB_SENS_AVG"); writeReg(SMPL_PRD, calib_smpl_prd, "CALIB_SMPL_PRD"); - usleep(40*1e6); // wait for next measurement + usleep(3 * 1e6); // wait for next measurement writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets usleep(ms_); ygyro_off = readReg(0x1C); xgyro_off = readReg(0x1A); zgyro_off = readReg(0x1E); - std::cout << "Gyro offsets after calibration\n" << "[" << xgyro_off[0] << " , " << xgyro_off[1] << "]" << " , " << ygyro_off[1] << " , " << zgyro_off[1] << std::endl; + LOG(I, + "Gyro offsets after calibration\n" + << "[" << xgyro_off[0] << " , " << xgyro_off[1] << "]" + << " , " << ygyro_off[1] << " , " << zgyro_off[1]); // Reset sensitivity and sample period writeReg(SENS_AVG, sens_avg, "SENS_AVG"); From e4832d4f167541b316f3b952956a30b5d4c2834a Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 2 May 2024 13:14:41 +0200 Subject: [PATCH 3/7] trying to fix gyro output --- src/imu/adis16448.cpp | 27 ++++++++++++--------------- 1 file changed, 12 insertions(+), 15 deletions(-) diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index 6df03c1..aeb64cb 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -241,15 +241,12 @@ bool Adis16448::calibrateGyro() { writeReg(0x1E, {0x00, 0x00}, "ZGYRO_OFF"); // read offsets - auto xgyro_off = readReg(0x1A); - auto ygyro_off = readReg(0x1C); - auto zgyro_off = readReg(0x1E); + auto xgyro_off = (double) signedWordToInt(readReg(0x1A)); + auto ygyro_off = (double) signedWordToInt(readReg(0x1C)); + auto zgyro_off = (double) signedWordToInt(readReg(0x1E)); - LOG(I, - "Gyro offsets after reset:\n" - << xgyro_off[0] << " , " << xgyro_off[1] << " , " << ygyro_off[1] << " , " - << zgyro_off[1]); - LOG(I, "Starting Gyro Calibration. Keep IMU steady and wait for 3s."); + LOG(I, "Gyro offsets after reset:\n" << xgyro_off << " , " << ygyro_off << " , " << zgyro_off); + LOG(I, "Starting Gyro Calibration. Keep IMU steady and wait for 5s."); // Store sensitivity and sample period auto sens_avg = readReg(SENS_AVG); @@ -259,20 +256,20 @@ bool Adis16448::calibrateGyro() { auto calib_sens_avg = sens_avg; auto calib_smpl_prd = smpl_prd; calib_sens_avg[0] = 0x01; // Set high sensitivity - calib_smpl_prd[0] = 0x0B; // Sample for (2^11 / 819.2) = 2.5s + calib_smpl_prd[0] = 0x0C; // Sample for (2^11 / 819.2) = 2.5s writeReg(SENS_AVG, calib_sens_avg, "CALIB_SENS_AVG"); writeReg(SMPL_PRD, calib_smpl_prd, "CALIB_SMPL_PRD"); - usleep(3 * 1e6); // wait for next measurement + usleep(5 * 1e6); // wait for next measurement writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets usleep(ms_); - ygyro_off = readReg(0x1C); - xgyro_off = readReg(0x1A); - zgyro_off = readReg(0x1E); + xgyro_off = (double) signedWordToInt(readReg(0x1A)); + ygyro_off = (double) signedWordToInt(readReg(0x1C)); + zgyro_off = (double) signedWordToInt(readReg(0x1E)); LOG(I, "Gyro offsets after calibration\n" - << "[" << xgyro_off[0] << " , " << xgyro_off[1] << "]" - << " , " << ygyro_off[1] << " , " << zgyro_off[1]); + << "[" << xgyro_off << "]" + << " , " << ygyro_off << " , " << zgyro_off); // Reset sensitivity and sample period writeReg(SENS_AVG, sens_avg, "SENS_AVG"); From b5923dd74e14d83c9bc71dd225f067edb16c3f8a Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 2 May 2024 14:46:47 +0200 Subject: [PATCH 4/7] Gyro calibration can be now started at startup. Also publishes latched topic with the set offset values. --- include/imu/adis16448.h | 2 +- include/imu/bmi088.h | 2 ++ include/imu/imu_interface.h | 2 +- include/imu_node.h | 7 +++++-- launch/imu.launch | 23 +++++++++++++---------- src/imu/adis16448.cpp | 29 +++++++++++++++-------------- src/imu/bmi088.cpp | 16 ++++++++++------ src/imu_node.cpp | 34 ++++++++++++++++++++++++---------- src/main.cpp | 18 ++++++++++-------- 9 files changed, 81 insertions(+), 52 deletions(-) diff --git a/include/imu/adis16448.h b/include/imu/adis16448.h index b535513..6e1a5ce 100644 --- a/include/imu/adis16448.h +++ b/include/imu/adis16448.h @@ -49,7 +49,7 @@ class Adis16448 : public ImuInterface { */ ImuBurstResult burst() override; - bool calibrateGyro() override; + vec3 calibrateGyro() override; /*! * @brief Reads accelerometer and gyroscope config from registers and prints them out. 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 0aaa9df..debea25 100644 --- a/include/imu/imu_interface.h +++ b/include/imu/imu_interface.h @@ -72,7 +72,7 @@ class ImuInterface { */ virtual std::optional> getGyro() = 0; - virtual bool calibrateGyro() = 0; + virtual vec3 calibrateGyro() = 0; /** * Gets acceleration data vector diff --git a/include/imu_node.h b/include/imu_node.h index 4a9e7b6..177d0c6 100644 --- a/include/imu_node.h +++ b/include/imu_node.h @@ -7,6 +7,7 @@ #include +#include #include #include #include @@ -20,7 +21,7 @@ class ImuNode { public: ImuNode(ImuInterface &imu_interface, int frequency); - int run(); + int run(bool calibrate_at_startup = false); inline static bool run_node = true; @@ -30,12 +31,14 @@ class ImuNode { void processTemperature(); void processFluidpressure(); - bool runGyroCalibration(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); + void calibrateGyro(); + 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_gyro_offset_pub_{}; ros::ServiceServer gyro_calib_srv_{}; diff --git a/launch/imu.launch b/launch/imu.launch index bff470c..31e597e 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -1,14 +1,17 @@ - - - - - + + + + + - - - - - + + + + + + diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index aeb64cb..7be721f 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -234,19 +234,20 @@ int Adis16448::signedWordToInt(const std::vector &word) { return (((int) *(signed char *) (word.data())) * 1 << CHAR_BIT) | word[1]; } -bool Adis16448::calibrateGyro() { +vec3 Adis16448::calibrateGyro() { // set offsets to zero writeReg(0x1A, {0x00, 0x00}, "XGYRO_OFF"); writeReg(0x1C, {0x00, 0x00}, "YGYRO_OFF"); writeReg(0x1E, {0x00, 0x00}, "ZGYRO_OFF"); // read offsets - auto xgyro_off = (double) signedWordToInt(readReg(0x1A)); - auto ygyro_off = (double) signedWordToInt(readReg(0x1C)); - auto zgyro_off = (double) signedWordToInt(readReg(0x1E)); - - LOG(I, "Gyro offsets after reset:\n" << xgyro_off << " , " << ygyro_off << " , " << zgyro_off); + vec3 gyro_off_raw{}; + gyro_off_raw.x = signedWordToInt(readReg(0x1A)); + gyro_off_raw.y = signedWordToInt(readReg(0x1C)); + gyro_off_raw.z = signedWordToInt(readReg(0x1E)); + vec3 gyro_off = convertGyro(gyro_off_raw); LOG(I, "Starting Gyro Calibration. Keep IMU steady and wait for 5s."); + LOG(I, "Gyro offset reset to:\n" << gyro_off.x << " , " << gyro_off.y << " , " << gyro_off.z); // Store sensitivity and sample period auto sens_avg = readReg(SENS_AVG); @@ -256,26 +257,26 @@ bool Adis16448::calibrateGyro() { 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^11 / 819.2) = 2.5s + 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_); - xgyro_off = (double) signedWordToInt(readReg(0x1A)); - ygyro_off = (double) signedWordToInt(readReg(0x1C)); - zgyro_off = (double) signedWordToInt(readReg(0x1E)); + gyro_off_raw.x = signedWordToInt(readReg(0x1A)); + gyro_off_raw.y = signedWordToInt(readReg(0x1C)); + gyro_off_raw.z = signedWordToInt(readReg(0x1E)); + gyro_off = convertGyro(gyro_off_raw); LOG(I, - "Gyro offsets after calibration\n" - << "[" << xgyro_off << "]" - << " , " << ygyro_off << " , " << zgyro_off); + "Calibration done. New Gyro offsets:\n" + << 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 true; + return gyro_off; } bool Adis16448::setBurstCRCEnabled(bool b) { 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 c33e86d..2d61290 100644 --- a/src/imu_node.cpp +++ b/src/imu_node.cpp @@ -7,14 +7,18 @@ 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); - gyro_calib_srv_ = nh.advertiseService("imu/run_gyro_calib", &ImuNode::runGyroCalibration, this); + 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() { +int ImuNode::run(bool calibrate_at_startup) { + if (calibrate_at_startup) { imu_interface_.calibrateGyro(); } + LOG(I, "Node started"); imu_interface_.printImuConfig(); while (ros::ok() && run_node) { @@ -90,10 +94,20 @@ 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::runGyroCalibration(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res ) { - imu_interface_.calibrateGyro(); - return true; - } +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..17dd742 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")); - int frequency = nh_private.param("frequency", 200); - std::string imu_name = nh_private.param("imu", std::string("adis16448")); + 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_at_start = nh_private.param("gyro_calibration", false); LOG(I, "Spi path: " << spi_path); LOG(I, "Loop frequency " << frequency); @@ -34,6 +36,6 @@ int main(int argc, char **argv) { ImuNode node{*imu_interface, frequency}; signal(SIGINT, SignalHandler); - node.run(); + node.run(calibrate_gyro_at_start); return 0; } \ No newline at end of file From 18ec3cdb64497f541ddce5cd2fbcd740a2d02957 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 2 May 2024 15:18:38 +0200 Subject: [PATCH 5/7] Moved gyro calibration outside of run into separate function per default calibration is turned off --- include/imu/ImuFactory.h | 3 ++- include/imu_node.h | 4 ++-- launch/imu.launch | 2 +- src/imu/adis16448.cpp | 20 ++++++++++---------- src/imu_node.cpp | 4 +--- src/main.cpp | 11 ++++++----- 6 files changed, 22 insertions(+), 22 deletions(-) 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_node.h b/include/imu_node.h index 177d0c6..0541985 100644 --- a/include/imu_node.h +++ b/include/imu_node.h @@ -21,7 +21,8 @@ class ImuNode { public: ImuNode(ImuInterface &imu_interface, int frequency); - int run(bool calibrate_at_startup = false); + int run(); + void calibrateGyro(); inline static bool run_node = true; @@ -31,7 +32,6 @@ class ImuNode { void processTemperature(); void processFluidpressure(); - void calibrateGyro(); bool calibrateGyroServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); ros::Publisher imu_data_raw_pub_{}; diff --git a/launch/imu.launch b/launch/imu.launch index 31e597e..08bda6b 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -11,7 +11,7 @@ - + diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index 7be721f..a9f2068 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -242,12 +242,12 @@ vec3 Adis16448::calibrateGyro() { // read offsets vec3 gyro_off_raw{}; - gyro_off_raw.x = signedWordToInt(readReg(0x1A)); - gyro_off_raw.y = signedWordToInt(readReg(0x1C)); - gyro_off_raw.z = signedWordToInt(readReg(0x1E)); + gyro_off_raw.x = (double) signedWordToInt(readReg(0x1A)); + gyro_off_raw.y = (double) signedWordToInt(readReg(0x1C)); + gyro_off_raw.z = (double) signedWordToInt(readReg(0x1E)); vec3 gyro_off = convertGyro(gyro_off_raw); - LOG(I, "Starting Gyro Calibration. Keep IMU steady and wait for 5s."); - LOG(I, "Gyro offset reset to:\n" << gyro_off.x << " , " << gyro_off.y << " , " << gyro_off.z); + 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); @@ -264,13 +264,13 @@ vec3 Adis16448::calibrateGyro() { writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets usleep(ms_); - gyro_off_raw.x = signedWordToInt(readReg(0x1A)); - gyro_off_raw.y = signedWordToInt(readReg(0x1C)); - gyro_off_raw.z = signedWordToInt(readReg(0x1E)); + gyro_off_raw.x = (double) signedWordToInt(readReg(0x1A)); + gyro_off_raw.y = (double) signedWordToInt(readReg(0x1C)); + gyro_off_raw.z = (double) signedWordToInt(readReg(0x1E)); gyro_off = convertGyro(gyro_off_raw); LOG(I, - "Calibration done. New Gyro offsets:\n" - << gyro_off.x << " , " << gyro_off.y << " , " << gyro_off.z); + "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"); diff --git a/src/imu_node.cpp b/src/imu_node.cpp index 2d61290..0c4b95a 100644 --- a/src/imu_node.cpp +++ b/src/imu_node.cpp @@ -16,9 +16,7 @@ ImuNode::ImuNode(ImuInterface &imu, int frequency) : imu_interface_(imu), freque nh.advertiseService("imu/run_gyro_calib", &ImuNode::calibrateGyroServiceCallback, this); } -int ImuNode::run(bool calibrate_at_startup) { - if (calibrate_at_startup) { imu_interface_.calibrateGyro(); } - +int ImuNode::run() { LOG(I, "Node started"); imu_interface_.printImuConfig(); while (ros::ok() && run_node) { diff --git a/src/main.cpp b/src/main.cpp index 17dd742..f00a62a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,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")); - int frequency = nh_private.param("frequency", 200); - std::string imu_name = nh_private.param("imu", std::string("adis16448")); - bool calibrate_gyro_at_start = nh_private.param("gyro_calibration", false); + 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); @@ -34,8 +34,9 @@ int main(int argc, char **argv) { } ImuNode node{*imu_interface, frequency}; + if (calibrate_gyro) { node.calibrateGyro(); } signal(SIGINT, SignalHandler); - node.run(calibrate_gyro_at_start); + node.run(); return 0; } \ No newline at end of file From 104b90013861221d32d90d4d021716cc90f3ecc0 Mon Sep 17 00:00:00 2001 From: Christian Lanegger Date: Thu, 2 May 2024 15:28:13 +0200 Subject: [PATCH 6/7] using proper commands for gyro offset --- src/imu/adis16448.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/imu/adis16448.cpp b/src/imu/adis16448.cpp index a9f2068..6b7c8d7 100644 --- a/src/imu/adis16448.cpp +++ b/src/imu/adis16448.cpp @@ -236,15 +236,15 @@ int Adis16448::signedWordToInt(const std::vector &word) { vec3 Adis16448::calibrateGyro() { // set offsets to zero - writeReg(0x1A, {0x00, 0x00}, "XGYRO_OFF"); - writeReg(0x1C, {0x00, 0x00}, "YGYRO_OFF"); - writeReg(0x1E, {0x00, 0x00}, "ZGYRO_OFF"); + 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(0x1A)); - gyro_off_raw.y = (double) signedWordToInt(readReg(0x1C)); - gyro_off_raw.z = (double) signedWordToInt(readReg(0x1E)); + 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); @@ -264,9 +264,9 @@ vec3 Adis16448::calibrateGyro() { writeReg(GLOB_CMD, {0x0, 1 << 0}, "GLOB_CMD"); // Set offsets usleep(ms_); - gyro_off_raw.x = (double) signedWordToInt(readReg(0x1A)); - gyro_off_raw.y = (double) signedWordToInt(readReg(0x1C)); - gyro_off_raw.z = (double) signedWordToInt(readReg(0x1E)); + 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 << " , " From 5ca03849bdc297be617bfcccd9cb6995e1f98bbf Mon Sep 17 00:00:00 2001 From: clanegge <33281019+clanegge@users.noreply.github.com> Date: Thu, 2 May 2024 15:29:57 +0200 Subject: [PATCH 7/7] exposed calibrate gyro param in launch file --- launch/imu.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/imu.launch b/launch/imu.launch index 08bda6b..2c356d6 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -5,13 +5,14 @@ + - +