WIP: Gyro calibration

.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

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 <memory>
 
+#include "imu_interface.h"
+
 typedef std::shared_ptr<ImuInterface> ImuInterfacePtr;
 
 class ImuFactory {

include/imu/adis16448.h

@@ -4,9 +4,10 @@
 
 #ifndef MAV_IMU_SRC_IMU_ADIS16448_H_
 #define MAV_IMU_SRC_IMU_ADIS16448_H_
+#include <string>
+
 #include "imu_interface.h"
 #include "spi_driver.h"
-#include <string>
 
 class Adis16448 : public ImuInterface {
  public:
@@ -48,6 +49,8 @@ class Adis16448 : public ImuInterface {
    */
   ImuBurstResult burst() override;
 
+  vec3<double> 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<byte> &data, const std::string &name) const;
 
   /**
-   * Run a test read sequence for SPI communcation.
+   * Run a test read sequence for SPI communication.
    */
   bool testSPI();
 

include/imu/bmi088.h

@@ -34,6 +34,8 @@ class Bmi088 : public ImuInterface {
 
   int getRaw(std::vector<byte> cmd) override;
 
+  vec3<double> calibrateGyro() override;
+
   /*!
   *  @brief Reads accelerometer and gyroscope config from registers and prints them out.
 

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 <optional>
 #include <sstream>
 
+#include "spi_driver.h"
+
 template<typename T>
 struct vec3 {
   T x;
@@ -71,6 +72,8 @@ class ImuInterface {
    */
   virtual std::optional<vec3<double>> getGyro() = 0;
 
+  virtual vec3<double> calibrateGyro() = 0;
+
   /**
    * Gets acceleration data vector
    *

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 <string>
+
+#include <geometry_msgs/Vector3Stamped.h>
 #include <ros/ros.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/Temperature.h>
-#include <string>
+#include <std_srvs/Empty.h>
+
+#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_{};

launch/imu.launch

@@ -1,14 +1,18 @@
 <launch>
-<arg name="imu" default="adis16448" doc="The IMU model (adis16448 or bmi088)."/>
-<arg name="spi_path" default="/dev/spidev0.1" doc="The SPI directory, where the first digit is the SPI port and the second digit is the chip select number."/>
-<arg name="frequency" default="200" doc="The IMU sample frequency."/> 
-<arg name="launch-prefix" default="" doc="Start nodes with launch-prefix."/>
-<arg name="output" default="screen" doc="Logging directory."/>
+    <arg name="imu" default="adis16448" doc="The IMU model (adis16448 or bmi088)."/>
+    <arg name="spi_path" default="/dev/spidev0.1"
+         doc="The SPI directory, where the first digit is the SPI port and the second digit is the chip select number."/>
+    <arg name="frequency" default="200" doc="The IMU sample frequency."/>
+    <arg name="launch-prefix" default="" doc="Start nodes with launch-prefix."/>
+    <arg name="output" default="screen" doc="Logging directory."/>
+    <arg name="calibrate_gyro" default="false" />
 
-<node name="mav_imu_node" pkg="mav_imu" type="mav_imu_node" launch-prefix="$(arg launch-prefix)" output="$(arg output)">
-    <param name="imu" value="$(arg imu)" type="string"/>
-    <param name="spi_path" value="$(arg spi_path)" type="string"/>
-    <param name="frequency" value="$(arg frequency)" type="int"/>
-</node>
+    <node name="mav_imu_node" pkg="mav_imu" type="mav_imu_node" launch-prefix="$(arg launch-prefix)"
+          output="$(arg output)">
+        <param name="imu" value="$(arg imu)" type="string"/>
+        <param name="spi_path" value="$(arg spi_path)" type="string"/>
+        <param name="frequency" value="$(arg frequency)" type="int"/>
+        <param name="calibrate_gyro" value="$(arg calibrate_gyro)"/>
+    </node>
 
 </launch>

src/imu/adis16448.cpp

@@ -3,13 +3,17 @@
 //
 
 #include "imu/adis16448.h"
-#include "imu/adis16448_cmds.h"
+
+#include <unistd.h>
+
 #include <cstring>
 #include <iostream>
+
 #include <linux/spi/spidev.h>
 #include <log++.h>
 #include <sys/ioctl.h>
-#include <unistd.h>
+
+#include "imu/adis16448_cmds.h"
 
 Adis16448::Adis16448(const std::string &path) : spi_driver_(path) {}
 
@@ -230,6 +234,51 @@ int Adis16448::signedWordToInt(const std::vector<byte> &word) {
   return (((int) *(signed char *) (word.data())) * 1 << CHAR_BIT) | word[1];
 }
 
+vec3<double> 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<double> 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<double> 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);

src/imu/bmi088.cpp

@@ -7,9 +7,10 @@
 #include <algorithm>
 #include <cmath>
 #include <iostream>
+#include <string>
+
 #include <linux/spi/spidev.h>
 #include <log++.h>
-#include <string>
 
 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<double> 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;
-}
+}

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<sensor_msgs::Imu>("imu/data_raw", 1);
-  imu_mag_pub_      = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 1);
-  imu_temp_pub_     = nh.advertise<sensor_msgs::Temperature>("imu/temp", 1);
-  imu_baro_pub_     = nh.advertise<sensor_msgs::FluidPressure>("imu/pressure", 1);
+  imu_data_raw_pub_    = nh.advertise<sensor_msgs::Imu>("imu/data_raw", 1);
+  imu_mag_pub_         = nh.advertise<sensor_msgs::MagneticField>("imu/mag", 1);
+  imu_temp_pub_        = nh.advertise<sensor_msgs::Temperature>("imu/temp", 1);
+  imu_baro_pub_        = nh.advertise<sensor_msgs::FluidPressure>("imu/pressure", 1);
+  imu_gyro_offset_pub_ = nh.advertise<geometry_msgs::Vector3Stamped>("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;
+}

src/main.cpp

@@ -1,10 +1,12 @@
-#include "imu/ImuFactory.h"
-#include "imu/adis16448.h"
-#include "imu_node.h"
 #include <csignal>
+
 #include <log++.h>
 #include <ros/ros.h>
 
+#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();