diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index d28b7ff4eb..67262ae526 100644 --- a/src/software/embedded/BUILD +++ b/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", diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index f763e4f8dc..69c2301f99 100644 --- a/src/software/embedded/services/BUILD +++ b/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"], diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp new file mode 100644 index 0000000000..03da1758e0 --- /dev/null +++ b/src/software/embedded/services/imu.cpp @@ -0,0 +1,199 @@ +#include "imu.h" + +#include +#include +#include +#include + +#include // 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(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(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 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(most_significant) << 8) | + static_cast(least_significant); + + return static_cast(combined); +} + +std::optional ImuService::pollHeadingVelocity() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional 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(full_word) / + static_cast(SHRT_MAX) * IMU_FULL_SCALE_DPS; + return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_); +} + + + +std::optional ImuService::pollLinearAcceleration() +{ + if (!initialized_) + { + return std::nullopt; + } + + std::optional opt_x = + readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG); + std::optional 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(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + double a_y = (static_cast(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G * + ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED; + + return Eigen::Vector2d(a_x, a_y); +} diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h new file mode 100644 index 0000000000..c1373d8053 --- /dev/null +++ b/src/software/embedded/services/imu.h @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include +#include + +#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 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 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 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 +}; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index b8a440a683..fafaf5432f 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/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(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(); + 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 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; diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index cf70a12b68..e553a78a54 100644 --- a/src/software/embedded/thunderloop.h +++ b/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 motor_service_; std::unique_ptr network_service_; std::unique_ptr power_service_; + std::unique_ptr imu_service_; // TOML config client std::unique_ptr 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. */