#include <mpu9250.hpp>

Collaboration diagram for Mpu9250:

[legend]

Public Member Functions
	Mpu9250 (SpiDevice spi_device, Gpio gpio_cs, Gpio *gpio_int)

void	CalibrateThreadTask ()

void	RegisterDataCallback (const std::function< void(const Type::Vector3 &, const Type::Vector3 &)> &callback)
	Registers a callback function to be called when new data is available.

void	Initialize ()

void	ReadData ()

void	DisplayData ()

void	WriteMagRegister (uint8_t reg, uint8_t value)

void	SaveCalibrationData ()

void	LoadCalibrationData ()

Data Fields
SpiDevice *	spi_device_

Gpio *	gpio_cs_

Gpio *	gpio_int_

Type::Vector3	accel_

Type::Vector3	gyro_

Type::Vector3	mag_

Type::Vector3	gyro_delta_

Type::Vector3	gyro_bias_ = {0, 0, 0}

float	temperature_ = 0

std::function< void(const Type::Vector3 &accel, const Type::Vector3 &gyro)>	data_callback_

std::thread	calibrate_thread_

Static Public Attributes
static constexpr uint8_t	WHO_AM_I = 0x75

static constexpr uint8_t	PWR_MGMT_1 = 0x6B

static constexpr uint8_t	PWR_MGMT_2 = 0x6C

static constexpr uint8_t	CONFIG = 0x1A

static constexpr uint8_t	SMPLRT_DIV = 0x19

static constexpr uint8_t	GYRO_CONFIG = 0x1B

static constexpr uint8_t	ACCEL_CONFIG = 0x1C

static constexpr uint8_t	ACCEL_CONFIG_2 = 0x1D

static constexpr uint8_t	ACCEL_XOUT_H = 0x3B

static constexpr uint8_t	GYRO_XOUT_H = 0x43

static constexpr uint8_t	USER_CTRL = 0x6A

static constexpr uint8_t	INT_PIN_CFG = 0x37

static constexpr uint8_t	INT_ENABLE = 0x38

static constexpr uint8_t	I2C_MST_CTRL = 0x24

static constexpr uint8_t	I2C_MST_DELAY_CTRL = 0x67

static constexpr uint8_t	I2C_SLV0_CTRL = 0x27

static constexpr uint8_t	AK8963_CNTL1_REG = 0x0A

static constexpr uint8_t	AK8963_CNTL2_REG = 0x0B

static constexpr uint8_t	AK8963_CNTL2_SRST = 0x01

Detailed Description

MPU9250 class

Provides initialization and data retrieval for MPU9250 via SPI.

Definition at line 25 of file mpu9250.hpp.

Constructor & Destructor Documentation

◆ Mpu9250()

Mpu9250::Mpu9250	(	SpiDevice *	spi_device,
		Gpio *	gpio_cs,
		Gpio *	gpio_int
	)

inline

Constructor

Parameters

spi_device	Pointer to an SPI device object
gpio_cs	Pointer to a GPIO object (chip select)

Definition at line 33 of file mpu9250.hpp.

      : spi_device_(spi_device), gpio_cs_(gpio_cs), gpio_int_(gpio_int) {
    assert(spi_device_ && gpio_cs_ && gpio_int_);
 
    Initialize();
    LoadCalibrationData();
 
    /* Register interrupt callback */
    gpio_int_->EnableInterruptRisingEdgeWithCallback([this]() { ReadData(); });
 
    calibrate_thread_ = std::thread(&Mpu9250::CalibrateThreadTask, this);
  }

◆ ~Mpu9250()

Mpu9250::~Mpu9250 ( )

inline

Definition at line 46 of file mpu9250.hpp.

             {
    if (calibrate_thread_.joinable()) {
      calibrate_thread_.join();
    }
  }

Member Function Documentation

◆ CalibrateThreadTask()

void Mpu9250::CalibrateThreadTask ( )

inline

Definition at line 52 of file mpu9250.hpp.

                             {
    while (true) {
      auto start_time = std::chrono::steady_clock::now();
      std::chrono::microseconds period_us(1000);
      std::chrono::steady_clock::time_point next_time =
          std::chrono::steady_clock::now();
      uint32_t counter = 0;
      double gyro_offset_x = 0, gyro_offset_y = 0, gyro_offset_z = 0;
      static bool cali_done = false;
 
      if (cali_done) {
        std::this_thread::sleep_for(std::chrono::seconds(UINT32_MAX));
        continue;
      }
 
      while (true) {
        if (std::fabsf(gyro_delta_.x) > 0.005 ||
            std::fabsf(gyro_delta_.y) > 0.005 ||
            std::fabsf(gyro_delta_.z) > 0.01) {
          next_time += period_us;
          std::this_thread::sleep_until(next_time);
          break;
        }
 
        auto now = std::chrono::steady_clock::now();
        if (now - start_time > std::chrono::seconds(5) &&
            now - start_time < std::chrono::seconds(30)) {
          counter++;
          gyro_offset_x += gyro_.x;
          gyro_offset_y += gyro_.y;
          gyro_offset_z += gyro_.z;
        }
 
        if (now - start_time > std::chrono::seconds(35) && cali_done == false) {
          float bias_x =
              static_cast<float>(gyro_offset_x / static_cast<double>(counter));
          float bias_y =
              static_cast<float>(gyro_offset_y / static_cast<double>(counter));
          float bias_z =
              static_cast<float>(gyro_offset_z / static_cast<double>(counter));
 
          if (std::fabsf(bias_x) > 0.005 || std::fabsf(bias_y) > 0.005 ||
              std::fabsf(bias_z) > 0.005) {
            gyro_bias_.x += bias_x;
            gyro_bias_.y += bias_y;
            gyro_bias_.z += bias_z;
            SaveCalibrationData();
            std::cout << "Calibration data saved\n";
          } else {
            std::cout << "No need to calibrate\n";
          }
          cali_done = true;
          std::cout << "Calibration completed\n";
        }
 
        next_time += period_us;
        std::this_thread::sleep_until(next_time);
      }
    }
  }

◆ DisplayData()

void Mpu9250::DisplayData ( )

inline

Displays the most recently read sensor data.

Definition at line 253 of file mpu9250.hpp.

                     {
    float accel_intensity = std::sqrt(
        accel_.x * accel_.x + accel_.y * accel_.y + accel_.z * accel_.z);
    std::cout << std::format(
        "Acceleration: [X={:+.4f}, Y={:+.4f}, Z={:+.4f} | Intensity={:+.4f}] | "
        "Gyroscope: [X={:+.4f}, Y={:+.4f}, Z={:+.4f}] | Temperature: {:+.4f} "
        "\u00b0C\n",
        accel_.x, accel_.y, accel_.z, accel_intensity, gyro_.x, gyro_.y,
        gyro_.z, temperature_);
  }

◆ Initialize()

void Mpu9250::Initialize ( )

inline

Initializes MPU9250 settings.

Configures power management, digital low-pass filter, sensor ranges, I2C Master mode for the magnetometer, and performs a WHO_AM_I check.

Definition at line 131 of file mpu9250.hpp.

                    {
    /* Reset and wake up the MPU9250 */
    spi_device_->WriteRegister(gpio_cs_, PWR_MGMT_1, 0x80);
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
    /* Read and verify WHO_AM_I register */
    uint8_t who_am_i = spi_device_->ReadRegister(gpio_cs_, WHO_AM_I);
    std::cout << std::format("MPU9250 initialized. WHO_AM_I: 0x{:02X}\n",
                             who_am_i);
 
    if (who_am_i != 0x71 && who_am_i != 0x68 && who_am_i != 0x70) {
      std::perror(
          std::format("Error: MPU9250 connection failed (WHO_AM_I: 0x{:02X})",
                      who_am_i)
              .c_str());
    }
 
    /* Gyroscope clock source configuration */
    spi_device_->WriteRegister(gpio_cs_, PWR_MGMT_1, 0x03);
 
    /* Enable Accelerometer and Gyroscope */
    spi_device_->WriteRegister(gpio_cs_, PWR_MGMT_2, 0x00);
 
    /* INT high level, push-pull */
    spi_device_->WriteRegister(gpio_cs_, INT_PIN_CFG, 0x10);
    /* enable data ready interrupt */
    spi_device_->WriteRegister(gpio_cs_, INT_ENABLE, 0x01);
 
    spi_device_->WriteRegister(gpio_cs_, I2C_MST_CTRL, 0x4D);
 
    /* Enable I2C Master mode by setting the I2C_MST_EN bit in USER_CTRL. */
    spi_device_->WriteRegister(gpio_cs_, USER_CTRL, 0x20);
 
    /* Enable delay for I2C Slave 0 transfers. */
    spi_device_->WriteRegister(gpio_cs_, I2C_MST_DELAY_CTRL, 0x01);
 
    /* Enable I2C communication for external sensors. */
    spi_device_->WriteRegister(gpio_cs_, I2C_SLV0_CTRL, 0x81);
 
    /* Configure Digital Low-Pass Filter (DLPF). */
    spi_device_->WriteRegister(gpio_cs_, CONFIG, 3);
 
    /* Set the gyroscope sampling rate. */
    spi_device_->WriteRegister(gpio_cs_, SMPLRT_DIV, 0x00);
 
    /* Configure gyroscope full-scale range to ±2000°/s. */
    spi_device_->WriteRegister(gpio_cs_, GYRO_CONFIG, 0x18);
 
    /* Configure accelerometer full-scale range to ±16g. */
    spi_device_->WriteRegister(gpio_cs_, ACCEL_CONFIG, 0x18);
 
    /* Configure accelerometer low-pass filter. */
    spi_device_->WriteRegister(gpio_cs_, ACCEL_CONFIG_2, 0x00);
 
    /* Reset the AK8963 magnetometer. */
    WriteMagRegister(AK8963_CNTL2_REG, AK8963_CNTL2_SRST);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
    /* Configure AK8963 magnetometer for continuous measurement mode 1. */
    WriteMagRegister(AK8963_CNTL1_REG, 0x12);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }

◆ LoadCalibrationData()

void Mpu9250::LoadCalibrationData ( )

inline

Load the calibration data from a file.

Definition at line 291 of file mpu9250.hpp.

                             {
    std::ifstream file("cali_data.bin", std::ios::in | std::ios::binary);
    if (!file) {
      std::cerr
          << "MPU9250 calibration file not found. Using default values.\n";
      return;
    }
 
    /* Check file size */
    file.seekg(0, std::ios::end);
    std::streamsize file_size = file.tellg();
    file.seekg(0, std::ios::beg);
 
    if (file_size != sizeof(gyro_bias_)) {
      std::cerr
          << "Error: MPU9250 calibration file size mismatch. Using default "
             "values.\n";
      file.close();
      gyro_bias_ = {0, 0, 0};
      return;
    }
 
    /* Read data */
    file.read(reinterpret_cast<char*>(&gyro_bias_), sizeof(gyro_bias_));
    file.close();
 
    /* Validate data: Ensure gyro_bias values are within a reasonable range */
    if (std::abs(gyro_bias_.x) > 1.0f || std::abs(gyro_bias_.y) > 1.0f ||
        std::abs(gyro_bias_.z) > 1.0f || std::isnan(gyro_bias_.x) ||
        std::isnan(gyro_bias_.y) || std::isnan(gyro_bias_.z) ||
        std::isinf(gyro_bias_.x) || std::isinf(gyro_bias_.y) ||
        std::isinf(gyro_bias_.z)) {
      std::cerr
          << "Error: MPU9250 invalid calibration data detected. Resetting to "
             "default values.\n";
      gyro_bias_ = {0, 0, 0};
    } else {
      std::cout << "MPU9250 calibration data loaded successfully: "
                << "X=" << gyro_bias_.x << ", "
                << "Y=" << gyro_bias_.y << ", "
                << "Z=" << gyro_bias_.z << "\n";
    }
 
    std::cout << '\n';
  }

◆ ReadData()

void Mpu9250::ReadData ( )

inline

Reads sensor data for acceleration and gyroscope.

Definition at line 197 of file mpu9250.hpp.

                  {
    std::array<uint8_t, 14> data{};
 
    spi_device_->ReadRegisters(gpio_cs_, ACCEL_XOUT_H, &data[0], 14);
 
    constexpr float ACCEL_SCALE = 16.0f / 32768.0f * 9.80665f;
    constexpr float GYRO_SCALE = 2000.0f / 32768.0f * M_PI / 180.0f;
 
    uint8_t* accel_data = &data[0];
    uint8_t* temperature_data = &data[6];
    uint8_t* gyro_data = &data[8];
 
    accel_.z = static_cast<float>(
                   static_cast<int16_t>((accel_data[0] << 8) | accel_data[1])) *
               ACCEL_SCALE;
    accel_.y = -static_cast<float>(
                   static_cast<int16_t>((accel_data[2] << 8) | accel_data[3])) *
               ACCEL_SCALE;
    accel_.x = static_cast<float>(
                   static_cast<int16_t>((accel_data[4] << 8) | accel_data[5])) *
               ACCEL_SCALE;
 
    temperature_ = static_cast<float>(static_cast<int16_t>(
                       (temperature_data[0] << 8) | temperature_data[1])) /
                       333.87f +
                   21.0f;
 
    float gyro_z = static_cast<float>(static_cast<int16_t>((gyro_data[0] << 8) |
                                                           gyro_data[1])) *
                       GYRO_SCALE -
                   gyro_bias_.z;
    float gyro_y = -static_cast<float>(static_cast<int16_t>(
                       (gyro_data[2] << 8) | gyro_data[3])) *
                       GYRO_SCALE -
                   gyro_bias_.y;
    float gyro_x = static_cast<float>(static_cast<int16_t>((gyro_data[4] << 8) |
                                                           gyro_data[5])) *
                       GYRO_SCALE -
                   gyro_bias_.x;
 
    gyro_delta_.x = gyro_x - gyro_.x;
    gyro_delta_.y = gyro_y - gyro_.y;
    gyro_delta_.z = gyro_z - gyro_.z;
 
    gyro_.x = gyro_x;
    gyro_.y = gyro_y;
    gyro_.z = gyro_z;
 
    if (data_callback_) {
      data_callback_(accel_, gyro_);
    }
  }

◆ RegisterDataCallback()

void Mpu9250::RegisterDataCallback ( const std::function< void(const Type::Vector3 &, const Type::Vector3 &)> & callback )

inline

Registers a callback function to be called when new data is available.

Parameters

callback The callback function

Definition at line 119 of file mpu9250.hpp.

                    {
    data_callback_ = callback;
  }

◆ SaveCalibrationData()

void Mpu9250::SaveCalibrationData ( )

inline

Save the calibration data to a file.

Definition at line 277 of file mpu9250.hpp.

                             {
    std::ofstream file("cali_data.bin", std::ios::binary);
    if (!file) {
      std::cerr << "Error: Unable to open cali_data.bin for writing.\n";
      return;
    }
    file.write(reinterpret_cast<const char*>(&gyro_bias_), sizeof(gyro_bias_));
    file.close();
    std::cout << "Calibration data saved successfully.\n";
  }

◆ WriteMagRegister()

void Mpu9250::WriteMagRegister	(	uint8_t	reg,
		uint8_t	value
	)

inline

Writes a value to the AK8963 magnetometer via I2C.

Parameters

reg	Magnetometer register address
value	Value to write

Definition at line 270 of file mpu9250.hpp.

                                                    {
    spi_device_->WriteRegister(gpio_cs_, reg, value);
  }

Field Documentation

◆ accel_

Type::Vector3 Mpu9250::accel_

Definition at line 364 of file mpu9250.hpp.

◆ ACCEL_CONFIG

constexpr uint8_t Mpu9250::ACCEL_CONFIG = 0x1C

staticconstexpr

Definition at line 344 of file mpu9250.hpp.

◆ ACCEL_CONFIG_2

constexpr uint8_t Mpu9250::ACCEL_CONFIG_2 = 0x1D

staticconstexpr

Definition at line 345 of file mpu9250.hpp.

◆ ACCEL_XOUT_H

constexpr uint8_t Mpu9250::ACCEL_XOUT_H = 0x3B

staticconstexpr

Definition at line 346 of file mpu9250.hpp.

◆ AK8963_CNTL1_REG

constexpr uint8_t Mpu9250::AK8963_CNTL1_REG = 0x0A

staticconstexpr

AK8963 Magnetometer registers

Definition at line 356 of file mpu9250.hpp.

◆ AK8963_CNTL2_REG

constexpr uint8_t Mpu9250::AK8963_CNTL2_REG = 0x0B

staticconstexpr

Definition at line 357 of file mpu9250.hpp.

◆ AK8963_CNTL2_SRST

constexpr uint8_t Mpu9250::AK8963_CNTL2_SRST = 0x01

staticconstexpr

Definition at line 358 of file mpu9250.hpp.

◆ calibrate_thread_

std::thread Mpu9250::calibrate_thread_

Definition at line 376 of file mpu9250.hpp.

◆ CONFIG

constexpr uint8_t Mpu9250::CONFIG = 0x1A

staticconstexpr

Definition at line 341 of file mpu9250.hpp.

◆ data_callback_

std::function<void(const Type::Vector3& accel, const Type::Vector3& gyro)> Mpu9250::data_callback_

Definition at line 374 of file mpu9250.hpp.

◆ gpio_cs_

Gpio* Mpu9250::gpio_cs_

Definition at line 361 of file mpu9250.hpp.

◆ gpio_int_

Gpio* Mpu9250::gpio_int_

Definition at line 362 of file mpu9250.hpp.

◆ gyro_

Type::Vector3 Mpu9250::gyro_

Definition at line 365 of file mpu9250.hpp.

◆ gyro_bias_

Type::Vector3 Mpu9250::gyro_bias_ = {0, 0, 0}

Definition at line 369 of file mpu9250.hpp.

369{0, 0, 0}; /* Gyroscope calibration data */

◆ GYRO_CONFIG

constexpr uint8_t Mpu9250::GYRO_CONFIG = 0x1B

staticconstexpr

Definition at line 343 of file mpu9250.hpp.

◆ gyro_delta_

Type::Vector3 Mpu9250::gyro_delta_

Definition at line 367 of file mpu9250.hpp.

◆ GYRO_XOUT_H

constexpr uint8_t Mpu9250::GYRO_XOUT_H = 0x43

staticconstexpr

Definition at line 347 of file mpu9250.hpp.

◆ I2C_MST_CTRL

constexpr uint8_t Mpu9250::I2C_MST_CTRL = 0x24

staticconstexpr

Definition at line 351 of file mpu9250.hpp.

◆ I2C_MST_DELAY_CTRL

constexpr uint8_t Mpu9250::I2C_MST_DELAY_CTRL = 0x67

staticconstexpr

Definition at line 352 of file mpu9250.hpp.

◆ I2C_SLV0_CTRL

constexpr uint8_t Mpu9250::I2C_SLV0_CTRL = 0x27

staticconstexpr

Definition at line 353 of file mpu9250.hpp.

◆ INT_ENABLE

constexpr uint8_t Mpu9250::INT_ENABLE = 0x38

staticconstexpr

Definition at line 350 of file mpu9250.hpp.

◆ INT_PIN_CFG

constexpr uint8_t Mpu9250::INT_PIN_CFG = 0x37

staticconstexpr

Definition at line 349 of file mpu9250.hpp.

◆ mag_

Type::Vector3 Mpu9250::mag_

Definition at line 366 of file mpu9250.hpp.

◆ PWR_MGMT_1

constexpr uint8_t Mpu9250::PWR_MGMT_1 = 0x6B

staticconstexpr

Definition at line 339 of file mpu9250.hpp.

◆ PWR_MGMT_2

constexpr uint8_t Mpu9250::PWR_MGMT_2 = 0x6C

staticconstexpr

Definition at line 340 of file mpu9250.hpp.

◆ SMPLRT_DIV

constexpr uint8_t Mpu9250::SMPLRT_DIV = 0x19

staticconstexpr

Definition at line 342 of file mpu9250.hpp.

◆ spi_device_

SpiDevice* Mpu9250::spi_device_

Definition at line 360 of file mpu9250.hpp.

◆ temperature_

float Mpu9250::temperature_ = 0

Definition at line 371 of file mpu9250.hpp.

◆ USER_CTRL

constexpr uint8_t Mpu9250::USER_CTRL = 0x6A

staticconstexpr

Definition at line 348 of file mpu9250.hpp.

◆ WHO_AM_I

constexpr uint8_t Mpu9250::WHO_AM_I = 0x75

staticconstexpr

MPU9250 register addresses

Definition at line 338 of file mpu9250.hpp.

The documentation for this class was generated from the following file:

src/device/mpu9250.hpp

Public Member Functions

Data Fields

Static Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Mpu9250()

◆ ~Mpu9250()

Member Function Documentation

◆ CalibrateThreadTask()

◆ DisplayData()

◆ Initialize()

◆ LoadCalibrationData()

◆ ReadData()

◆ RegisterDataCallback()

◆ SaveCalibrationData()

◆ WriteMagRegister()

Field Documentation

◆ accel_

◆ ACCEL_CONFIG

◆ ACCEL_CONFIG_2

◆ ACCEL_XOUT_H

◆ AK8963_CNTL1_REG

◆ AK8963_CNTL2_REG

◆ AK8963_CNTL2_SRST

◆ calibrate_thread_

◆ CONFIG

◆ data_callback_

◆ gpio_cs_

◆ gpio_int_

◆ gyro_

◆ gyro_bias_

◆ GYRO_CONFIG

◆ gyro_delta_

◆ GYRO_XOUT_H

◆ I2C_MST_CTRL

◆ I2C_MST_DELAY_CTRL

◆ I2C_SLV0_CTRL

◆ INT_ENABLE

◆ INT_PIN_CFG

◆ mag_

◆ PWR_MGMT_1

◆ PWR_MGMT_2

◆ SMPLRT_DIV

◆ spi_device_

◆ temperature_

◆ USER_CTRL

◆ WHO_AM_I