IMU Integration

src/software/embedded/BUILD

@@ -26,6 +26,7 @@ cc_library(
     deps = [
         ":primitive_executor",
         "//proto:tbots_cc_proto",
+        "//software/embedded/services:imu",
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",

src/software/embedded/services/BUILD

@@ -31,6 +31,19 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "imu",
+    srcs = ["imu.cpp"],
+    hdrs = ["imu.h"],
+    deps = [
+        "//proto:tbots_cc_proto",
+        "//shared:constants",
+        "//software/geom:angular_velocity",
+        "//software/logger",
+        "@eigen",
+    ],
+)
+
 cc_binary(
     name = "robot_auto_test",
     srcs = ["robot_auto_test.cpp"],

src/software/embedded/services/imu.cpp

@@ -0,0 +1,199 @@
+#include "imu.h"
+
+#include <fcntl.h>
+#include <linux/i2c-dev.h>
+#include <linux/i2c.h>
+#include <sys/ioctl.h>
+
+#include <climits>  // For SHRT_MAX
+
+#include "shared/constants.h"
+#include "software/logger/logger.h"
+
+// these functions taken from
+// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c
+__s32 i2c_smbus_access(int file, char read_write, __u8 command, int size,
+                       union i2c_smbus_data* data)
+{
+    struct i2c_smbus_ioctl_data args;
+    __s32 err;
+
+    args.read_write = read_write;
+    args.command    = command;
+    args.size       = size;
+    args.data       = data;
+
+    err = ioctl(file, I2C_SMBUS, &args);
+    if (err == -1)
+        err = -errno;
+    return err;
+}
+
+__s32 i2c_smbus_read_byte_data(int file, __u8 command)
+{
+    union i2c_smbus_data data;
+    int err;
+
+    err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data);
+    if (err < 0)
+        return err;
+
+    return 0x0FF & data.byte;
+}
+
+__s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value)
+{
+    union i2c_smbus_data data;
+    data.byte = value;
+    return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data);
+}
+
+
+ImuService::ImuService() : initialized_(false)
+{
+    // Establish connection to the IMU and verify that the who am I pin is correct.
+    file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR);
+    int ret          = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b);
+    if (ret < 0)
+    {
+        LOG(WARNING)
+            << "Failed to initialize the IMU: failed to establish i2c connection.";
+        return;
+    }
+    if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106)
+    {
+        LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register "
+                     << static_cast<int>(WHOAMI_REG) << " read incorrectly.";
+        return;
+    }
+    // Attempt to enable gyro and accelerometer, checking that writes are successful
+    // See lsm6dsl datasheet for how to set these registers.
+    i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000);
+    if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000)
+    {  // write unsuccessful
+        LOG(WARNING)
+            << "Failed to initialize the IMU: writing to accelerometer config register "
+            << static_cast<int>(ACCEL_CONTROL_REG) << " unsuccessful";
+        return;
+    }
+    // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of
+    // datasheet for lsm6dsl, pg 21)
+    i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000);
+    if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000)
+    {  // write unsuccessful
+        LOG(WARNING)
+            << "Failed to initialize the IMU: writing to gyroscope config register "
+            << "unsuccessful";
+        return;
+    }
+
+    // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000)
+    // CTRL4_C: bit 1 (LPF1_SEL_G) = 1
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010);
+    // CTRL6_C: bits 2:0 (FTYPE) = 000
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000);
+
+    // Enable Accelerometer digital LPF2
+    // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000);
+
+    initialized_ = true;
+    LOG(INFO) << "Initialized IMU! Calibrating...";
+    degrees_error_ = 0;
+    // get 100 sample average of stationary reading, so all future readings can be
+    // corrected
+    double sum        = 0;
+    int valid_samples = 0;
+    for (int i = 0; i < 100; i++)
+    {
+        // Fixed: Call the actual implemented function name
+        auto poll = pollHeadingVelocity();
+        if (poll.has_value())
+        {
+            sum += poll->toDegrees();
+            valid_samples++;
+        }
+        usleep(50000);
+    }
+
+    if (valid_samples > 0)
+    {
+        degrees_error_ = sum / valid_samples;
+        LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps";
+    }
+    else
+    {
+        LOG(WARNING) << "IMU Calibration failed: no valid samples received. Heading "
+                        "stability will be poor.";
+        initialized_ = false;
+    }
+}
+
+std::optional<int16_t> ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg)
+{
+    int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg);
+    int most_significant  = i2c_smbus_read_byte_data(file_descriptor_, ms_reg);
+
+    if (least_significant < 0 || most_significant < 0)
+    {
+        return std::nullopt;
+    }
+
+    uint16_t combined = (static_cast<uint8_t>(most_significant) << 8) |
+                        static_cast<uint8_t>(least_significant);
+
+    return static_cast<int16_t>(combined);
+}
+
+std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
+{
+    if (!initialized_)
+    {
+        return std::nullopt;
+    }
+
+    std::optional<int16_t> opt_full_word =
+        readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
+
+    if (!opt_full_word.has_value())
+    {
+        return std::nullopt;
+    }
+
+    int16_t full_word = opt_full_word.value();
+
+    double degrees_per_sec = static_cast<double>(full_word) /
+                             static_cast<double>(SHRT_MAX) * IMU_FULL_SCALE_DPS;
+    return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
+}
+
+
+
+std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
+{
+    if (!initialized_)
+    {
+        return std::nullopt;
+    }
+
+    std::optional<int16_t> opt_x =
+        readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
+    std::optional<int16_t> opt_y =
+        readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
+
+    if (!opt_x.has_value() || !opt_y.has_value())
+    {
+        return std::nullopt;
+    }
+
+    int16_t raw_x = opt_x.value();
+    int16_t raw_y = opt_y.value();
+
+    double a_x = (static_cast<double>(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
+                 ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+
+    double a_y = (static_cast<double>(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
+                 ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+
+    return Eigen::Vector2d(a_x, a_y);
+}

src/software/embedded/services/imu.h

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <climits>
+#include <numbers>
+#include <utility>
+
+#include "proto/tbots_software_msgs.pb.h"
+#include "software/geom/angular_velocity.h"
+
+/**
+ * Handles low level IMU I2C communication, and some minor offset filtering.
+ */
+class ImuService
+{
+   public:
+    /**
+     * Constructs and initializes a new IMU service object.
+     *
+     * If successfully initialized, will try to do a simple calibration of the IMU.
+     */
+    ImuService();
+    /*
+     * Polls the latest IMU reading of the angular velocity of the robot on the z axis
+     * @return the current angular velocty of the robot on the z axis
+     */
+    std::optional<AngularVelocity> pollHeadingVelocity();
+    /*
+     * Polls the latest IMU reading of the linear acceleration of the robot on the z plane
+     * @return the current linear acceleration of the robot on the z plane
+     */
+    std::optional<Eigen::Vector2d> pollLinearAcceleration();
+
+
+    // Variance from datasheet (in rad^2/s^2)
+    static constexpr double IMU_VARIANCE =
+        (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
+        (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
+
+   private:
+    /*
+     * Reads byte data from two registers, and combine them into a single value
+     * @parama ls_reg register of the least significant register
+     * @parama ms_reg register of the most significant register
+     * @return the combined integer value of the two registers
+     */
+
+    std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
+
+    bool initialized_ = false;
+
+    int file_descriptor_ = 0;
+
+    double degrees_error_;
+
+    // Maps the maximum raw reading from 16-bit integer to be 2 times gravity
+    static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
+    // Same for gyroscope, 1000 degrees per second
+    static constexpr double IMU_FULL_SCALE_DPS = 1000.0;
+
+    /// Various i2c registers.
+    static const uint8_t WHOAMI_REG        = 0xf;
+    static const uint8_t ACCEL_CONTROL_REG = 0x10;
+    static const uint8_t GYRO_CONTROL_REG  = 0x11;
+    static const uint8_t CTRL4_C           = 0x13;
+    static const uint8_t CTRL6_C           = 0x15;
+    static const uint8_t CTRL8_XL          = 0x17;
+
+    // Device path for the IMU
+    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
+
+    // Gyroscope Z-axis (Yaw) Output Data Registers
+    static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26;  // OUTZ_L_G
+    static constexpr uint8_t HEADING_MOST_SIG_REG  = 0x27;  // OUTZ_H_G
+
+    // Accelerometer X-axis Output Data Registers
+    static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28;  // OUTX_L_XL
+    static constexpr uint8_t ACCEL_X_MOST_SIG_REG  = 0x29;  // OUTX_H_XL
+
+    // Accelerometer Y-axis Output Data Registers
+    static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A;  // OUTY_L_XL
+    static constexpr uint8_t ACCEL_Y_MOST_SIG_REG  = 0x2B;  // OUTY_H_XL
+};

src/software/embedded/thunderloop.cpp

@@ -9,6 +9,7 @@
 #include "proto/tbots_software_msgs.pb.h"
 #include "shared/constants.h"
 #include "software/embedded/primitive_executor.h"
+#include "software/embedded/services/imu.h"
 #include "software/embedded/services/motor.h"
 #include "software/logger/logger.h"
 #include "software/logger/network_logger.h"
@@ -121,7 +122,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
     motor_service_  = std::make_unique<MotorService>(robot_constants, loop_hz);
     g_motor_service = motor_service_.get();
     motor_service_->setup();
-    LOG(INFO) << "THUNDERLOOP: Motor Service initialized!";
+
+    LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
+
+    imu_service_ = std::make_unique<ImuService>();
+    LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";
 
     LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_
               << ", CHANNEL ID: " << channel_id_
@@ -177,6 +182,15 @@ void Thunderloop::runLoop()
     robot_status_.set_thunderloop_version(thunderloop_hash);
     robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed);
 
+
+    std::optional<AngularVelocity> imu_poll = imu_service_->pollHeadingVelocity();
+
+    // TODO: Replace with actual IMU data usage
+    if (imu_poll.has_value())
+    {
+        LOG(INFO) << "IMU Heading Velocity" << imu_poll.value();
+    }
+
     for (;;)
     {
         struct timespec time_since_prev_iter;

src/software/embedded/thunderloop.h

@@ -10,6 +10,7 @@
 #include "shared/constants.h"
 #include "shared/robot_constants.h"
 #include "software/embedded/primitive_executor.h"
+#include "software/embedded/services/imu.h"
 #include "software/embedded/services/motor.h"
 #include "software/embedded/services/network/network.h"
 #include "software/embedded/services/power.h"
@@ -60,6 +61,7 @@ class Thunderloop
     std::unique_ptr<MotorService> motor_service_;
     std::unique_ptr<NetworkService> network_service_;
     std::unique_ptr<PowerService> power_service_;
+    std::unique_ptr<ImuService> imu_service_;
 
     // TOML config client
     std::unique_ptr<TomlConfigClient> toml_config_client_;
@@ -118,6 +120,7 @@ class Thunderloop
      */
     TbotsProto::PowerStatus pollPowerService(struct timespec& poll_time);
 
+
     /**
      * Wait for networking communication to be established. This function is blocking.
      */