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/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:
@@ -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();
 

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;
@@ -72,6 +73,8 @@ class ImuInterface {
    */
   virtual std::optional<vec3<double>> getGyro() = 0;
 
+  virtual bool calibrateGyro() = 0;
+
   /**
    * Gets acceleration data vector
    *

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 <string>
+
 #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:
@@ -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_{};

src/imu/adis16448.cpp

@@ -226,6 +226,47 @@ int Adis16448::signedWordToInt(const std::vector<byte> &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);

src/imu_node.cpp

@@ -12,6 +12,7 @@ ImuNode::ImuNode(ImuInterface &imu, int frequency)
   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);
+  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;
+  }