Code Monkey home page Code Monkey logo

invensense-imu's Introduction

License: MIT

Bolder Flight Systems Logo     Arduino Logo

InvensenseImu

This library communicates with InvensenseMPU-6500 and InvenSense MPU-9250 and MPU-9255 Inertial Measurement Units (IMUs). This library is compatible with Arduino and CMake build systems.

Description

The Invense MPU-6500 is a three-axis gyroscope and three-axis accelerometer. The InvenSense MPU-9250 is a System in Package (SiP) that combines two chips: the MPU-6500 three-axis gyroscope and three-axis accelerometer; and the AK8963 three-axis magnetometer. The MPU-6500 and MPU-9250 support I2C, up to 400 kHz, and SPI communication, up to 1 MHz for register setup and 20 MHz for data reading. The following selectable full scale sensor ranges are available:

Gyroscope Full Scale Range Accelerometer Full Scale Range Magnetometer Full Scale Range (MPU-9250 Only)
+/- 250 deg/s +/- 2g +/- 4800 uT
+/- 500 deg/s +/- 4g
+/- 1000 deg/s +/- 8g
+/- 2000 deg/s +/- 16g

The IMUs sample the gyros, accelerometers, and magnetometers with 16 bit analog to digital converters. It also features programmable digital filters, a precision clock, and an embedded temperature sensor.

Installation

Arduino

Use the Arduino Library Manager to install this library or clone to your Arduino/libraries folder.

For the MPU-6500, this library is added as:

#include "mpu6500.h"

For the MPU-9250, this library is added as:

#include "mpu9250.h"

Example Arduino executables are located in: examples/arduino/. Teensy 3.x, 4.x, and LC devices are used for testing under Arduino and this library should be compatible with other Arduino devices.

CMake

CMake is used to build this library, which is exported as a library target called invensense_imu.

For the MPU-6500, this library is added as:

#include "mpu6500.h"

For the MPU-9250, this library is added as:

#include "mpu9250.h"

The library can be also be compiled stand-alone using the CMake idiom of creating a build directory and then, from within that directory issuing:

cmake .. -DMCU=MK66FX1M0
make

This will build the library and example executables called i2c_example, spi_example, drdy_spi_example, and wom_example (MPU-9250 only). The example executable source files are located at examples/cmake. Notice that the cmake command includes a define specifying the microcontroller the code is being compiled for. This is required to correctly configure the code, CPU frequency, and compile/linker options. The available MCUs are:

  • MK20DX128
  • MK20DX256
  • MK64FX512
  • MK66FX1M0
  • MKL26Z64
  • IMXRT1062_T40
  • IMXRT1062_T41
  • IMXRT1062_MMOD

These are known to work with the same packages used in Teensy products. Also switching packages is known to work well, as long as it's only a package change.

The example targets create executables for communicating with the sensor using I2C or SPI communication, using the data ready interrupt, and using the wake on motion interrupt, respectively. Each target also has a _hex, for creating the hex file to upload to the microcontroller, and an _upload for using the Teensy CLI Uploader to flash the Teensy. Please note that instructions for setting up your build environment can be found in our build-tools repo.

Namespace

This library is within the namespace bfs.

InvensenseImu

This class provides methods for reading and writing to registers on these sensors. It's expected that this should work with at least the MPU-6000, MPU-6050, MPU-6500, MPU-9150, and MPU-9250; although, it may work for other sensors as well. Most users will likely prefer the sensor specific classes, below; however, this class may enable people to unlock greater functionality and use as a starting point for their own sensor drivers.

Methods

InvensenseImu() Default constructor, requires calling the Config method to setup the I2C or SPI bus and I2C address or SPI chip select pin.

InvensenseImu(TwoWire *i2c, const uint8_t addr) Creates a InvensenseImu object. This constructor is used for the I2C communication interface. A pointer to the I2C bus object is passed along with the I2C address of the sensor.

InvensenseImu(SPIClass *bus, uint8_t cs) Creates a InvensenseImu object. This constructor is used for the SPI communication interface. A pointer to the SPI bus object is passed along with the chip select pin of the sensor. Any pin capable of digital I/O can be used as a chip select pin.

void Config(TwoWire *bus, const uint8_t addr) This is required when using the default constructor and sets up the I2C bus and I2C address.

void Config(SPIClass *spi, const uint8_t cs) This is required when using the default constructor and sets up the SPI bus and chip select pin.

void Begin() Initializes communication with the sensor. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus.

bool WriteRegister(const uint8_t reg, const uint8_t data, const int32_t spi_clock) Writes register data to the sensor given the register address and the data. The SPI clock speed must be specified if SPI communication is used.

bool WriteRegister(const uint8_t reg, const uint8_t data) Overload of the above where I2C communication is used.

bool ReadRegisters(const uint8_t reg, const uint8_t count, const int32_t spi_clock, uint8_t * const data) Reads register data from the sensor given the register address, the number of registers to read, the SPI clock, and a pointer to store the data.

bool ReadRegisters(const uint8_t reg, const uint8_t count, uint8_t * const data) Overload of the above where I2C communication is used.

Mpu9250

This class works with the MPU-9250 and MPU-9255 IMUs.

Methods

Mpu9250() Default constructor, requires calling the Config method to setup the I2C or SPI bus and I2C address or SPI chip select pin.

Mpu9250(i2c_t3 *bus, I2cAddr addr) Creates a Mpu9250 object. This constructor is used for the I2C communication interface. A pointer to the I2C bus object is passed along with the I2C address of the sensor. The address will be I2C_ADDR_PRIM (0x68) if the AD0 pin is grounded and I2C_ADDR_SEC (0x69) if the AD0 pin is pulled high.

Mpu9250 mpu9250(&Wire, bfs::Mpu9250::I2C_ADDR_PRIM);

Mpu9250(SPIClass *bus, uint8_t cs) Creates a Mpu9250 object. This constructor is used for the SPI communication interface. A pointer to the SPI bus object is passed along with the chip select pin of the sensor. Any pin capable of digital I/O can be used as a chip select pin.

Mpu9250 mpu9250(&SPI, 2);

void Config(TwoWire *bus, const I2cAddr addr) This is required when using the default constructor and sets up the I2C bus and I2C address. The address will be I2C_ADDR_PRIM (0x68) if the AD0 pin is grounded and I2C_ADDR_SEC (0x69) if the AD0 pin is pulled high.

void Config(SPIClass *spi, const uint8_t cs) This is required when using the default constructor and sets up the SPI bus and chip select pin.

bool Begin() Initializes communication with the sensor and configures the default sensor ranges, sampling rates, and low pass filter settings. The default accelerometer range is +/- 16g and the default gyro range is +/- 2,000 deg/s. The default sampling rate is 1000 Hz and the low-pass filter is set to a cutoff frequency of 184 Hz. True is returned if communication is able to be established with the sensor and configuration completes successfully, otherwise, false is returned. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus.

Wire.begin();
Wire.setClock(400000);
bool status = mpu9250.Begin();
if (!status) {
  // ERROR
}

bool EnableDrdyInt() Enables the data ready interrupt. A 50 us interrupt will be triggered on the MPU-9250 INT pin when IMU data is ready. This interrupt is active high. This method returns true if the interrupt is successfully enabled, otherwise, false is returned.

bool status = mpu9250.EnableDrdyInt();
if (!status) {
  // ERROR
}

bool DisableDrdyInt() Disables the data ready interrupt. This method returns true if the interrupt is successfully disabled, otherwise, false is returned.

bool status = mpu9250.DisableDrdyInt();
if (!status) {
  // ERROR
}

bool ConfigAccelRange(const AccelRange range) Sets the accelerometer full scale range. Options are:

Range Enum Value
+/- 2g ACCEL_RANGE_2G
+/- 4g ACCEL_RANGE_4G
+/- 8g ACCEL_RANGE_8G
+/- 16g ACCEL_RANGE_16G

True is returned on succesfully setting the accelerometer range, otherwise, false is returned. The default range is +/-16g.

bool status = mpu9250.ConfigAccelRange(bfs::Mpu9250::ACCEL_RANGE_4G);
if (!status) {
  // ERROR
}

AccelRange accel_range() Returns the current accelerometer range.

AccelRange range = mpu9250.accel_range();

bool ConfigGyroRange(const GyroRange range) Sets the gyro full scale range. Options are:

Range Enum Value
+/- 250 deg/s GYRO_RANGE_250DPS
+/- 500 deg/s GYRO_RANGE_500DPS
+/- 1000 deg/s GYRO_RANGE_1000DPS
+/- 2000 deg/s GYRO_RANGE_2000DPS

True is returned on succesfully setting the gyro range, otherwise, false is returned. The default range is +/-2000 deg/s.

bool status = mpu9250.ConfigGyroRange(bfs::Mpu9250::GYRO_RANGE_1000DPS);
if (!status) {
  // ERROR
}

GyroRange gyro_range() Returns the current gyro range.

GyroRange range = mpu9250.gyro_range();

bool ConfigSrd(const uint8_t srd) Sets the sensor sample rate divider. The MPU-9250 samples the accelerometer and gyro at a rate, in Hz, defined by:

$$rate = 1000 / (srd + 1)$$

A srd setting of 0 means the MPU-9250 samples the accelerometer and gyro at 1000 Hz. A srd setting of 4 would set the sampling at 200 Hz. The IMU data ready interrupt is tied to the rate defined by the sample rate divider. The magnetometer is sampled at 100 Hz for sample rate divider values corresponding to 100 Hz or greater. Otherwise, the magnetometer is sampled at 8 Hz.

True is returned on succesfully setting the sample rate divider, otherwise, false is returned. The default sample rate divider value is 0, resulting in a 1000 Hz sample rate.

/* Set sample rate divider for 50 Hz */
bool status = mpu9250.sample_rate_divider(19);
if (!status) {
  // ERROR
}

uint8_t srd() Returns the current sample rate divider value.

uint8_t srd = mpu9250.srd();

bool ConfigDlpfBandwidth(const DlpfBandwidth dlpf) Sets the cutoff frequency of the digital low pass filter for the accelerometer, gyro, and temperature sensor. Available bandwidths are:

DLPF Bandwidth Enum Value
184 Hz DLPF_BANDWIDTH_184HZ
92 Hz DLPF_BANDWIDTH_92HZ
41 Hz DLPF_BANDWIDTH_41HZ
20 Hz DLPF_BANDWIDTH_20HZ
10 Hz DLPF_BANDWIDTH_10HZ
5 Hz DLPF_BANDWIDTH_5HZ

True is returned on succesfully setting the digital low pass filters, otherwise, false is returned. The default bandwidth is 184 Hz.

bool status = mpu9250.ConfigDlpfBandwidth(bfs::Mpu9250::DLPF_BANDWIDTH_20HZ);
if (!status) {
  // ERROR
}

DlpfBandwidth dlpf_bandwidth() Returns the current digital low pass filter bandwidth setting.

DlpfBandwidth dlpf = mpu9250.dlpf_bandwidth();

bool EnableWom(int16_t threshold_mg, const WomRate wom_rate) Enables the Wake-On-Motion interrupt. It places the MPU-9250 into a low power state, waking up at an interval determined by the WomRate. If the accelerometer detects motion in excess of the threshold, threshold_mg, it generates a 50us pulse from the MPU-9250 interrupt pin. The following enumerated WOM rates are supported:

WOM Sample Rate Enum Value
0.24 Hz WOM_RATE_0_24HZ
0.49 Hz WOM_RATE_0_49HZ
0.98 Hz WOM_RATE_0_98HZ
1.95 Hz WOM_RATE_1_95HZ
3.91 Hz WOM_RATE_3_91HZ
7.81 Hz WOM_RATE_7_81HZ
15.63 Hz WOM_RATE_15_63HZ
31.25 Hz WOM_RATE_31_25HZ
62.50 Hz WOM_RATE_62_50HZ
125 Hz WOM_RATE_125HZ
250 Hz WOM_RATE_250HZ
500 Hz WOM_RATE_500HZ

The motion threshold is given as a value between 4 and 1020 mg, which is internally mapped to a single byte, 1-255 value. This function returns true on successfully enabling Wake On Motion, otherwise returns false. Please see the wom_i2c example. The following is an example of enabling the wake on motion with a 40 mg threshold and a ODR of 31.25 Hz.

imu.EnableWom(40, bfs::Mpu9250::WOM_RATE_31_25HZ);

void Reset() Resets the MPU-9250.

bool Read() Reads data from the MPU-9250 and stores the data in the Mpu9250 object. Returns true if data is successfully read, otherwise, returns false.

/* Read the IMU data */
if (mpu9250.Read()) {
}

bool new_imu_data() Returns true if new data was returned from the accelerometer and gyro.

if (mpu9250.Read()) {
  bool new_data = mpu9250.new_imu_data();
}

bool new_mag_data() Returns true if new data was returned from the magnetometer. For MPU-9250 sample rates of 100 Hz and higher, the magnetometer is sampled at 100 Hz. For MPU-9250 sample rates less than 100 Hz, the magnetometer is sampled at 8 Hz, so it is not uncommon to receive new IMU data, but not new magnetometer data.

if (mpu9250.Read()) {
  bool new_mag = mpu9250.new_mag_data();
}

float accel_x_mps2() Returns the x accelerometer data from the Mpu9250 object in units of m/s/s. Similar methods exist for the y and z axis data.

/* Read the IMU data */
if (mpu9250.Read()) {
  float ax = mpu9250.accel_x_mps2();
  float ay = mpu9250.accel_y_mps2();
  float az = mpu9250.accel_z_mps2();
}

float gyro_x_radps() Returns the x gyro data from the Mpu9250 object in units of rad/s. Similar methods exist for the y and z axis data.

/* Read the IMU data */
if (mpu9250.Read()) {
  float gx = mpu9250.gyro_x_radps();
  float gy = mpu9250.gyro_y_radps();
  float gz = mpu9250.gyro_z_radps();
}

float mag_x_ut() Returns the x magnetometer data from the Mpu9250 object in units of uT. Similar methods exist for the y and z axis data.

/* Read the IMU data */
if (mpu9250.Read()) {
  float hx = mpu9250.mag_x_ut();
  float hy = mpu9250.mag_y_ut();
  float hz = mpu9250.mag_z_ut();
}

float die_temp_c() Returns the die temperature of the sensor in units of C.

/* Read the IMU data */
if (mpu9250.Read()) {
  float temp = mpu9250.die_temp_c();
}

Mpu6500

This class works with the MPU-6500 sensor.

Methods

Mpu6500() Default constructor, requires calling the Config method to setup the I2C or SPI bus and I2C address or SPI chip select pin.

Mpu6500(i2c_t3 *bus, I2cAddr addr) Creates a Mpu6500 object. This constructor is used for the I2C communication interface. A pointer to the I2C bus object is passed along with the I2C address of the sensor. The address will be I2C_ADDR_PRIM (0x68) if the AD0 pin is grounded and I2C_ADDR_SEC (0x69) if the AD0 pin is pulled high.

Mpu6500 mpu6500(&Wire, bfs::Mpu6500::I2C_ADDR_PRIM);

Mpu6500(SPIClass *bus, uint8_t cs) Creates a Mpu6500 object. This constructor is used for the SPI communication interface. A pointer to the SPI bus object is passed along with the chip select pin of the sensor. Any pin capable of digital I/O can be used as a chip select pin.

Mpu6500 mpu6500(&SPI, 2);

void Config(TwoWire *bus, const I2cAddr addr) This is required when using the default constructor and sets up the I2C bus and I2C address. The address will be I2C_ADDR_PRIM (0x68) if the AD0 pin is grounded and I2C_ADDR_SEC (0x69) if the AD0 pin is pulled high.

void Config(SPIClass *spi, const uint8_t cs) This is required when using the default constructor and sets up the SPI bus and chip select pin.

bool Begin() Initializes communication with the sensor and configures the default sensor ranges, sampling rates, and low pass filter settings. The default accelerometer range is +/- 16g and the default gyro range is +/- 2,000 deg/s. The default sampling rate is 1000 Hz and the low-pass filter is set to a cutoff frequency of 184 Hz. True is returned if communication is able to be established with the sensor and configuration completes successfully, otherwise, false is returned. The communication bus is not initialized within this library and must be initialized seperately; this enhances compatibility with other sensors that may on the same bus.

Wire.begin();
Wire.setClock(400000);
bool status = mpu6500.Begin();
if (!status) {
  // ERROR
}

bool EnableDrdyInt() Enables the data ready interrupt. A 50 us interrupt will be triggered on the MPU-9250 INT pin when IMU data is ready. This interrupt is active high. This method returns true if the interrupt is successfully enabled, otherwise, false is returned.

bool status = mpu6500.EnableDrdyInt();
if (!status) {
  // ERROR
}

bool DisableDrdyInt() Disables the data ready interrupt. This method returns true if the interrupt is successfully disabled, otherwise, false is returned.

bool status = mpu6500.DisableDrdyInt();
if (!status) {
  // ERROR
}

bool ConfigAccelRange(const AccelRange range) Sets the accelerometer full scale range. Options are:

Range Enum Value
+/- 2g ACCEL_RANGE_2G
+/- 4g ACCEL_RANGE_4G
+/- 8g ACCEL_RANGE_8G
+/- 16g ACCEL_RANGE_16G

True is returned on succesfully setting the accelerometer range, otherwise, false is returned. The default range is +/-16g.

bool status = mpu6500.ConfigAccelRange(bfs::Mpu6500::ACCEL_RANGE_4G);
if (!status) {
  // ERROR
}

AccelRange accel_range() Returns the current accelerometer range.

AccelRange range = mpu6500.accel_range();

bool ConfigGyroRange(const GyroRange range) Sets the gyro full scale range. Options are:

Range Enum Value
+/- 250 deg/s GYRO_RANGE_250DPS
+/- 500 deg/s GYRO_RANGE_500DPS
+/- 1000 deg/s GYRO_RANGE_1000DPS
+/- 2000 deg/s GYRO_RANGE_2000DPS

True is returned on succesfully setting the gyro range, otherwise, false is returned. The default range is +/-2000 deg/s.

bool status = mpu6500.ConfigGyroRange(bfs::Mpu6500::GYRO_RANGE_1000DPS);
if (!status) {
  // ERROR
}

GyroRange gyro_range() Returns the current gyro range.

GyroRange range = mpu6500.gyro_range();

bool ConfigSrd(const uint8_t srd) Sets the sensor sample rate divider. The MPU-9250 samples the accelerometer and gyro at a rate, in Hz, defined by:

$$rate = 1000 / (srd + 1)$$

A srd setting of 0 means the MPU-9250 samples the accelerometer and gyro at 1000 Hz. A srd setting of 4 would set the sampling at 200 Hz. The IMU data ready interrupt is tied to the rate defined by the sample rate divider. The magnetometer is sampled at 100 Hz for sample rate divider values corresponding to 100 Hz or greater. Otherwise, the magnetometer is sampled at 8 Hz.

True is returned on succesfully setting the sample rate divider, otherwise, false is returned. The default sample rate divider value is 0, resulting in a 1000 Hz sample rate.

/* Set sample rate divider for 50 Hz */
bool status = mpu6500.sample_rate_divider(19);
if (!status) {
  // ERROR
}

uint8_t srd() Returns the current sample rate divider value.

uint8_t srd = mpu6500.srd();

bool ConfigDlpfBandwidth(const DlpfBandwidth dlpf) Sets the cutoff frequency of the digital low pass filter for the accelerometer, gyro, and temperature sensor. Available bandwidths are:

DLPF Bandwidth Enum Value
184 Hz DLPF_BANDWIDTH_184HZ
92 Hz DLPF_BANDWIDTH_92HZ
41 Hz DLPF_BANDWIDTH_41HZ
20 Hz DLPF_BANDWIDTH_20HZ
10 Hz DLPF_BANDWIDTH_10HZ
5 Hz DLPF_BANDWIDTH_5HZ

True is returned on succesfully setting the digital low pass filters, otherwise, false is returned. The default bandwidth is 184 Hz.

bool status = mpu6500.ConfigDlpfBandwidth(bfs::Mpu6500::DLPF_BANDWIDTH_20HZ);
if (!status) {
  // ERROR
}

DlpfBandwidth dlpf_bandwidth() Returns the current digital low pass filter bandwidth setting.

DlpfBandwidth dlpf = mpu6500.dlpf_bandwidth();

bool Read() Reads data from the MPU-6500 and stores the data in the Mpu6500 object. Returns true if data is successfully read, otherwise, returns false.

/* Read the IMU data */
if (mpu6500.Read()) {
}

bool new_imu_data() Returns true if new data was returned from the accelerometer and gyro.

if (mpu6500.Read()) {
  bool new_data = mpu6500.new_imu_data();
}

float accel_x_mps2() Returns the x accelerometer data from the Mpu6500 object in units of m/s/s. Similar methods exist for the y and z axis data.

/* Read the IMU data */
if (mpu6500.Read()) {
  float ax = mpu6500.accel_x_mps2();
  float ay = mpu6500.accel_y_mps2();
  float az = mpu6500.accel_z_mps2();
}

float gyro_x_radps() Returns the x gyro data from the Mpu6500 object in units of rad/s. Similar methods exist for the y and z axis data.

/* Read the IMU data */
if (mpu6500.Read()) {
  float gx = mpu6500.gyro_x_radps();
  float gy = mpu6500.gyro_y_radps();
  float gz = mpu6500.gyro_z_radps();
}

float die_temp_c() Returns the die temperature of the sensor in units of C.

/* Read the IMU data */
if (mpu6500.Read()) {
  float temp = mpu6500.die_temp_c();
}

Sensor Orientation

This library transforms all data to a common axis system before it is returned. This axis system is shown below. It is a right handed coordinate system with the z-axis positive down, common in aircraft dynamics.

MPU-9250 Orientation

Caution! This axis system is shown relative to the MPU-6500 and MPU-9250 sensor. The sensor may be rotated relative to the breakout board.

invensense-imu's People

Contributors

flybrianfly avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

invensense-imu's Issues

Not working at all for me I2C

Arduino --> MPU9250
5V --> VCC
GND --> GND
A4 --> SDA
A5 --> SCL

IMU initialization unsuccessful
Check IMU wiring or try cycling power
Status: -1

Sensor recognized in the I2C scanner

wrong heading angle from the magnetometer

Hi , thanks for the great job you doing my problem is I'm getting wrong value for heading angle and I,m using your library and calibration method to calculate the headeing so can you help me?
thanks

calibrateAccel() not calculating bias/scale?

I've been trying to run the calibration code for the MPU9250, and the calibrateMag() method seems to work, but the calibrateAccel() method simply returns bias 0 and scale 1.
I am running the function 6 times, changing the orientation of the imu with +z, -z, +y, -y, +x, -x pointing up in that order, switching whenever the "switch" serial print appears and holding position until the next.
Any ideas?

Hardware running the code: ESP8266 WeMos D1 mini with MPU9250
Code I'm running:

/*
CalibrateMPU9250.ino
Brian R Taylor
[email protected]
Copyright (c) 2017 Bolder Flight Systems
Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
and associated documentation files (the "Software"), to deal in the Software without restriction, 
including without limitation the rights to use, copy, modify, merge, publish, distribute, 
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or 
substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include "MPU9250.h"
#include "EEPROM.h"

// an MPU-9250 object on SPI bus 0 with chip select 10
MPU9250 Imu(Wire, 0x68);
int status;

// EEPROM buffer and variable to save accel and mag bias 
// and scale factors
uint8_t EepromBuffer[48];
float value;

void setup() {
  // serial to display instructions
  Serial.begin(57600);
  while(!Serial) {}
  // start communication with IMU 
  status = Imu.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }
  // calibrating accelerometer
  Serial.println("Starting Accelerometer Calibration");
  Imu.calibrateAccel();
  Serial.println("Switch");
  delay(5000);
  Imu.calibrateAccel();
  Serial.println("Switch");
  delay(5000);
  Imu.calibrateAccel();
  Serial.println("Switch");
  delay(5000);
  Imu.calibrateAccel();
  Serial.println("Switch");
  delay(5000);
  Imu.calibrateAccel();
  Serial.println("Switch");
  delay(5000);
  Imu.calibrateAccel();
  Serial.println("Done");
  Serial.println("Starting Magnetometer Calibration");
  delay(5000);
  // calibrating magnetometer
  Imu.calibrateMag();
  // saving to EEPROM
  value = Imu.getAccelBiasX_mss();
  memcpy(EepromBuffer,&value,4);
  Serial.print(value);
  Serial.println(": abx");
  value = Imu.getAccelScaleFactorX();
  memcpy(EepromBuffer+4,&value,4);
  Serial.print(value);
  Serial.println(": asx");
  value = Imu.getAccelBiasY_mss();
  memcpy(EepromBuffer+8,&value,4);
  Serial.print(value);
  Serial.println(": aby");
  value = Imu.getAccelScaleFactorY();
  memcpy(EepromBuffer+12,&value,4);
  Serial.print(value);
  Serial.println(": asy");
  value = Imu.getAccelBiasZ_mss();
  memcpy(EepromBuffer+16,&value,4);
  Serial.print(value);
  Serial.println(": abz");
  value = Imu.getAccelScaleFactorZ();
  memcpy(EepromBuffer+20,&value,4);
  Serial.print(value);
  Serial.println(": asz");
  value = Imu.getMagBiasX_uT();
  memcpy(EepromBuffer+24,&value,4);
  Serial.print(value);
  Serial.println(": mbx");
  value = Imu.getMagScaleFactorX();
  memcpy(EepromBuffer+28,&value,4);
  Serial.print(value);
  Serial.println(": msx");
  value = Imu.getMagBiasY_uT();
  memcpy(EepromBuffer+32,&value,4);
  Serial.print(value);
  Serial.println(": mby");
  value = Imu.getMagScaleFactorY();
  memcpy(EepromBuffer+36,&value,4);
  Serial.print(value);
  Serial.println(": msy");
  value = Imu.getMagBiasZ_uT();
  memcpy(EepromBuffer+40,&value,4);
  Serial.print(value);
  Serial.println(": mbz");
  value = Imu.getMagScaleFactorZ();
  memcpy(EepromBuffer+44,&value,4);
  Serial.print(value);
  Serial.println(": msz");
  for (size_t i=0; i < sizeof(EepromBuffer); i++) {
    EEPROM.write(i,EepromBuffer[i]);
  }
  Serial.println("Done");
}

void loop() {}

How to use attachInterrupt with ESP32 in I2C mod

Hi, i want to print data with specific frequency, so i want to use Interrupt when new data is available then print, but use example sketch Interrupt_SPI and change SPI to I2C then upload sketch serialport is not anything print, i think the attachInterrupt is not work, below is my sketch code.

/*
Interrupt_SPI.ino
Brian R Taylor
[email protected]


```Copyright (c) 2017 Bolder Flight Systems

Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
and associated documentation files (the "Software"), to deal in the Software without restriction, 
including without limitation the rights to use, copy, modify, merge, publish, distribute, 
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or 
substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include "MPU9250.h"

// an MPU9250 object with the MPU-9250 sensor on SPI bus 0 and chip select pin 10
MPU9250 IMU(Wire,0x68);
int status;

void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  // enabling the data ready interrupt
  IMU.enableDataReadyInterrupt();
  // attaching the interrupt to microcontroller pin 1
  pinMode(1,INPUT);
  attachInterrupt(1,getIMU,RISING);
}

void loop() {}

void getIMU(){ 
  // read the sensor
  IMU.readSensor();
  // display the data
  Serial.print(IMU.getAccelX_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelY_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelZ_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroX_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroY_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroZ_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagX_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagY_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagZ_uT(),6);
  Serial.print("\t");
  Serial.println(IMU.getTemperature_C(),6);
}  

Mag bias and scale returning 0

After I run the calibrateMag() method the bias and scales are returning 0.000 and 1.000

I have tried waving it in a big figure 8, a small figure 8, a slow figure 8, fast figure 8 and leaving it still.

Any idea why?

Basic_I2C Example with ESP32

Hey, I'm trying to use the MPU9250 in combination with an ESP32 Dev Board instead of an Arduino. I already tested the connection with an I2C Scanner and it shows that there is a device connected with address 0x68 (SDA and SCL are Pins 21 and 22 on the ESP32).
But when I start the Basic_I2C Example, I always get status = -1 from IMU.begin(). My wiring should be correct.
Do you have any idea what the problem is and how I can get this working?

MPU9255 Data is wrong and recommend some improvements

This is great source for MPU9255 but I found error and unstability of sensor data in readsensor() when I printed raw data:
// display the data
Serial.print(_axcounts);
Serial.print(" \t");
Serial.print(_aycounts);
Serial.print(" \t");
Serial.print(_azcounts);
Serial.print(" \t");
Serial.print(_gxcounts);
Serial.print(" \t");
Serial.print(_gycounts);
Serial.print(" \t");
Serial.print(_gzcounts);
Serial.print(" \t");
Serial.print(_hxcounts);
Serial.print(" \t");
Serial.print(_hycounts);
Serial.print(" \t");
Serial.print(_hzcounts);
Serial.print(" \t");
Serial.println(_tcounts);
raw data printed like this:
30827 -59 -32145 32763 48 -22 27 10 32730 -31909
30825 32703 618 -5 -32702 32747 27 10 32730 859
30825 32703 618 -5 -32702 32747 27 10 32730 859
30813 -61 -32168 32767 54 -16 27 10 32730 856
30813 -61 -32168 32767 54 -16 27 10 32730 856
30836 -57 -32152 -1 -4 -4 27 10 32730 858
30836 -57 -32152 -1 -4 -4 27 10 32730 858

I also recommend custom SPI definition using:
int MPU9250::begin(uint8_t CLKPIN,uint8_t SOPIN,uint8_t SIPIN,uint8_t _csPIN)
// setting CS pin to output
pinMode(_csPin,OUTPUT);
// setting CS pin high
digitalWrite(_csPin,HIGH);
// setting SPI SO pin to output
pinMode(SOPIN,OUTPUT);
// setting SPI SI pin to input
pinMode(SIPIN,INPUT);
// setting SPI CLK pin to input
pinMode(CLKPIN,INPUT);
// begin SPI communication
// _spi->begin();
_spi->begin(CLKPIN,SOPIN,SIPIN,_csPIN);////SPI.begin(int8_t sck=SCK, int8_t miso=MISO, int8_t mosi=MOSI, int8_t ss=-1);

Add calibration functions

It would be quite useful to have them
I think you should add something to calibrate while running the sketch, a function to read the calibration data and a function to write calibration data

readSensor function don't work in interrupt

I'm calling readSensor function in interrupts (first in setup() call enableDataReadyInterrupt()) and using I2C connection. interrupt is received and go to function but then it hangs and running code is stoped. why did this happen?

#include "MPU9250.h"

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
int status;

void setup() {
// serial to display data
Serial.begin(115200);
while(!Serial) {}
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
else Serial.println("initiate ok");
// setting the accelerometer full scale range to +/-16G
IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
// setting the gyroscope full scale range to +/-500 deg/s
IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
// setting DLPF bandwidth to 20 Hz
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
// setting SRD to 19 for a 50 Hz update rate
IMU.setSrd(19);
// enabling the data ready interrupt
IMU.enableDataReadyInterrupt();
// attaching the interrupt to microcontroller pin 1
pinMode(2,INPUT);
attachInterrupt(digitalPinToInterrupt(2),getIMU,RISING);
}

void loop() {
// display the data
Serial.println("loop");
/* Serial.print(IMU.getAccelX_mss(),6);
Serial.print("\t");
Serial.print(IMU.getAccelY_mss(),6);
Serial.print("\t");
Serial.print(IMU.getAccelZ_mss(),6);
Serial.print("\t");
Serial.print(IMU.getGyroX_rads(),6);
Serial.print("\t");
Serial.print(IMU.getGyroY_rads(),6);
Serial.print("\t");
Serial.print(IMU.getGyroZ_rads(),6);
Serial.print("\t");
Serial.print(IMU.getMagX_uT(),6);
Serial.print("\t");
Serial.print(IMU.getMagY_uT(),6);
Serial.print("\t");
Serial.print(IMU.getMagZ_uT(),6);
Serial.print("\t");
Serial.println(IMU.getTemperature_C(),6);*/
}

void getIMU(){
// read the sensor
IMU.readSensor();
Serial.println("interrupts");
}

Problems working with madgwick quaternion filter

Hi,
I am working on a small project using MPU9250 to get rotational data of the joint. As I utilizting this library with Madgwick quaternion filter, I got a very strange result, which not only drift when stationary, but also when I'm moving the sensor. (a short video showing the result: https://youtu.be/sDa7cdnEAwQ)

I guess the code for the filter is correct since it is open source and online everywhere. (In my working repo lib: https://github.com/larryyyyy/MPU9250) I was stucking in this stage to get a good quaternion for so long like several weeks... hope to get some hins here. Thank you so much!

2x MPU9250 i2c Esp32

Hey Guys,
I am having issues with my esp32 and interfacing my two IMUs (MPU9250 with different addresses). Both Imus are connected to 21,22 pins of the Esp32. My setup works at times and then it doesn't work. Here's the story -

2 months ago - Got Everything working. Then it stopped. Replaced the IMUs, eventually after many resets, it began to work. Both sets of IMUs worked fine.
AND KEPT WORKING
1 week ago - IMU's stopped work (Previous night everything worked fine!!)

I am using a basic i2c scanner sketch to test my communication. When both IMUs are connected. I am getting 'no devices found 'errors. When I connect them alone, both addresses are picked up just fine. Tried the basic I2c sketches. I get the -1 status errors.

I am not using any pullup resistors. As I believe, they come with the breakout boards (generic gy boards). I don't think I need them as my setup used to worked fine for months!!. And If I do use pull resistors, what would the circuit look like given that two IMU's are being used, will the resistors be in parallel (Electronics noob here)

PLEASE HELP as these IMUs are essential to my thesis due soon!

No sample wiring diagram

This repository seems much helpful but there can be many other like me who feel afraid of wrong wiring so , in ReadMe file or here, it would be great to share a sample working wiring diagram using any one of the Arduino boards, preferably Arduino UNO, Arduino Mega, Arduino Nano, STM32F1 board, etc.

Getting Raw Sensor Values - Weird data returned

Hi Brian.
I am trying to incorporate a function to return the raw data in counts and getting weird results:

1550104011431,1550104013211,220550,1550104015131,41,40,28.28
1550104011431,1550104013211,220550,1550104015131,41,40,28.28

just a couple of samples. Its suppose to represent ax, ay, az, gx, gy, gz, tempC.

Tried it on three different 9250 sensors with the same results. The advanced_i2c function works fine. Here is the function I am using:

int MPU9250::readSensorCounts(int16_t* rawValues)
{
    _useSPIHS = true; // use the high speed SPI for data readout

  // grab the data from the MPU9250
  if (readRegisters(ACCEL_OUT, 21, _buffer) < 0) {
    return -1;
  }
    rawValues[0] = (((int16_t)_buffer[0]) << 8) | _buffer[1];  // combine into 16 bit values
    rawValues[1] = (((int16_t)_buffer[2]) << 8) | _buffer[3];
    rawValues[2] = (((int16_t)_buffer[4]) << 8) | _buffer[5];

    rawValues[3] = (((int16_t)_buffer[8]) << 8) | _buffer[9];
    rawValues[4] = (((int16_t)_buffer[10]) << 8) | _buffer[11];
    rawValues[5] = (((int16_t)_buffer[12]) << 8) | _buffer[13];
	
	rawValues[6] = (((int16_t)_buffer[15]) << 8) | _buffer[14];
	rawValues[7] = (((int16_t)_buffer[17]) << 8) | _buffer[16];
	rawValues[8] = (((int16_t)_buffer[19]) << 8) | _buffer[18];
    
    rawValues[9] = (((int16_t)_buffer[6]) << 8) | _buffer[7];
    
  return 1;
}

Don't know what I am missing here.

Thanks
Mike

FIFO and WoM connection

I have a question if there is a possibility to connect two examples together (FIFO_SPI.ino and WoM_I2C.ino) so it would send the content of FIFO only if there's motion detected.

Thanks for tips.

WHOAMI byte

Hi all,

i am using MPU9250 with an arduino nano and arduino IDE 1.8.9.
When I'm trying to use the BASIC_I2C sketch there is an error with status -5...

IMU initialization unsuccessful
Check IMU wiring or try cycling power
Status: -5

I have checked the error in the MPU9250.cpp:
// check the WHO AM I byte, expected value is 0x71 (decimal 113) or 0x73 (decimal 115)
if((whoAmI() != 113)&&(whoAmI() != 115)){
return -5;
}

But i don't know how to deal with this error and to run the sketch properly..
I'm happy about your advice!

No matching function call to MPU9250 :: Solved

I am trying to compile Basic_I2C and am receiving the following error

No Matching function for call to 'MPU9250_::MPU9250_(TwoWire&, int)'

I am using Arduino uno, Arduino IDE 1.8.7 , I installed via the library manager and the IDE installed it for me. I opened the sketch via the examples menu

Accelerometer and Gyroscope readings are reversed.

Hello,

Thank you all for the information on this topic.

I have modified the advanced I2C to add a filter to stabilize the data and wrote test code to graph each axis (one program per axis per reading) in real time for testing. I've found that my Accelerometer readings are reading the Gyro (it is still displaying with the Accelerometer units) and my Gyro is reading acceleration in radians.

My functions are correct (I did not put the wrong one in). I am using an Arduino Uno with a sparkfun MPU-9250. I believe this has something to do with the header file, but I am not experienced enough to identify what I would need to interchange. Can I get some help on this?

Let me know what I can provide to be helpful.

Thank you,
Nick

Magnetometer values freeze after some time

Hello all,

Setting:

I am working on a system where multiple MPU9250 IMUs (6, to be specific) are connected to an ESP32 MCU via SPI. Using SPI as the communication bus is crucial because the I2C pins are occupied. If it's relevant, sensors are divided as 3 sensors to VSPI and 3 sensors to HSPI.

Problem:

The Mag values freeze for multiple sensors after being initialized without any problem. The sketch follows the initialization sequence given in the example Basic_SPI from the examples directory.

Moreover, freezing of the mag values is not deterministic between different runs (some mags freeze after a few seconds, some after a minute and so on), proving it very hard to detect what is going on. I have tried keeping every sensor steady (i.e. without any motion, so that the SPI connections remain stable), but the problem persists. Also - Accel and Gyro continue to give the data as expected, so the SPI connection between the MCU and IMUs cannot be an issue.

Can you please provide some pointers as to how should I try debugging this behavior of the sensors?

Error at initialisation

Hi and thank you for this library,

for some reason I encounter an error when calling "IMU.begin()". The status returns -18, so writing the control register 1 of the magnetometer to continuous measurement mode does not work.

Also, if I uncomment this section from the cpp file, the next error occurs at the gyro-calibration (IMU.begin() returns -20).

Do you have any idea what that could be based on?

Thank you for any help.

Dom

Measurements like a toothsaw

Hi!

I am trying to read a MPU9250 following the same configurations as the Advanced_I2C example (ACCEL_RANGE_8G, GYRO_RANGE_500DPS, DLPF_BANDWIDTH_20HZ, SRC=19) but I am having weird measurements. Apparently, only one of every two measurements is correct:

Accel( -0.002394, 0.603370, 0.002394)
Accel( -0.035915, 0.615341, -9.907711)
Accel( -0.002394, 2.449393, 0.002394)
Accel( -0.047886, 0.644073, -9.843065)
Accel( -0.002394, 0.687171, 0.002394)
Accel( -0.019155, 0.691960, -9.888557)
Accel( -0.038309, 0.610553, -20.837799)

The same happens with the magnetic field but not with the gyroscope and after a couple of minutes, my MPU stops responding.

Any ideas on why this could be happening?

More context: when I run the "Advanced_I2C" example as is, everything works correctly. My implementation is in code where I am reading a pressure sensor as well and sending everything (acceleration, gyroscope, magnetometer, pressure) via a LoRa transmitter.

Calculation of Magnetometer Heading (relative to magnetic north / 0 - 360°)

I require some help calculating the heading relative to magnetic north with this library and an MPU9250.

float heading_m = (atan2(IMU.getMagY_uT(), IMU.getMagX_uT()) * 180) / PI;
does not work as with other magnetometer examples ( Adafruit Example for Heading) 😞

As far as I understand, the readings gathered by the library do not represent vectors as it is the case with the Adafruit code. Adding a function to this library to calculate the heading would be extremely useful!

Thanks in advance!

Can't compile the basic SPI example

Hi,

I'm using Arduino 1.8.5
I'm trying to compile https://github.com/bolderflight/MPU9250/blob/master/examples/Basic_SPI/Basic_SPI.ino code. but I got a compile error,

Basic_SPI:27: error: 'SPI' was not declared in this scope
MPU9250 IMU(SPI,10);
^
/tmp/arduino_modified_sketch_937275/Basic_SPI.ino: In function 'void setup()':
Basic_SPI:36: error: 'class MPU9250' has no member named 'begin'
status = IMU.begin();
^
/tmp/arduino_modified_sketch_937275/Basic_SPI.ino: In function 'void loop()':
Basic_SPI:48: error: 'class MPU9250' has no member named 'readSensor'
IMU.readSensor();
^
Basic_SPI:50: error: 'class MPU9250' has no member named 'getAccelX_mss'
Serial.print(IMU.getAccelX_mss(),6);
^
Basic_SPI:52: error: 'class MPU9250' has no member named 'getAccelY_mss'
Serial.print(IMU.getAccelY_mss(),6);
^
Basic_SPI:54: error: 'class MPU9250' has no member named 'getAccelZ_mss'
Serial.print(IMU.getAccelZ_mss(),6);
^
Basic_SPI:56: error: 'class MPU9250' has no member named 'getGyroX_rads'
Serial.print(IMU.getGyroX_rads(),6);
^
Basic_SPI:58: error: 'class MPU9250' has no member named 'getGyroY_rads'
Serial.print(IMU.getGyroY_rads(),6);
Multiple libraries were found for "MPU9250.h"
^
Used: /home/tk/Arduino/libraries/MPU9250-master
Basic_SPI:60: error: 'class MPU9250' has no member named 'getGyroZ_rads'
Not used: /home/tk/Arduino/libraries/MPU9250-new
Serial.print(IMU.getGyroZ_rads(),6);
^
Basic_SPI:62: error: 'class MPU9250' has no member named 'getMagX_uT'
Serial.print(IMU.getMagX_uT(),6);
^
Basic_SPI:64: error: 'class MPU9250' has no member named 'getMagY_uT'
Serial.print(IMU.getMagY_uT(),6);
^
Basic_SPI:66: error: 'class MPU9250' has no member named 'getMagZ_uT'
Serial.print(IMU.getMagZ_uT(),6);
^
Basic_SPI:68: error: 'class MPU9250' has no member named 'getTemperature_C'
Serial.println(IMU.getTemperature_C(),6);
^
exit status 1
'SPI' was not declared in this scope

can you please help me.

Thank You.

ESP32 and SPI connection: corrupt accelerometer and magnetometer values

Hi,

Issue:
when using the library with an ESP32 connected via SPI, accelerometer and magnetometer values are corrupt. Example: Serial.print(IMU->getAccelX_mss(),6) prints "ovf".

Reason:
When not performing a calibration, numerous values for accelerometer biases such as "_axb" etc. defined in MPU9250.h are not initialized.

Using this library on MPU6050

I tried using this MPU9250 library on my MPU6050 because I found this library to be simpler than the ones used for the MPU6050. However it didn't work, though some people said it would.

So, is there anyway to use this library on MPU6050?
Thank you

ACCEL_RANGE is ignored

When I set the Accel Range with IMU.setAccelRange(MPU9250::ACCEL_RANGE_4G); the statement is ignored and all readings I get are limited to 20 m/s/s (2G).
I have tried different ranges but always get only maximum 2G readings.

Converting to use with Jeff Rowbergs Lib

Hi
I am trying to convert your master mode method and incorporate into the MPU6050 lib that I use for the FreeIMU library. Did a conversion but for the life of me I can not get it working. Was wondering if you could take a look and let me know what you think. What happens it that it does not appear to be connecting to the mPU9250. I am using Kris Winer's MPU9250 mini board. By the way would the t2 library and bus work with non-teensy boards.

Here is the code that I use to initialize the 9250:
`void MPU60X0::initialize9250MasterMode(){
#include "AK8963.h"

uint8_t buff[3];
uint8_t data[7];
int16_t _magScaleX, _magScaleY, _magScaleZ;	
bool test;

//set dev address for magnetometer
magDevAddr = 0x0C;

// SPI Configuration
if (bSPI) {
  SPI.begin();
	pinMode(devAddr, OUTPUT);
	digitalWrite(devAddr, HIGH);
	reset();
	delay(100);
	switchSPIEnabled(true);
	delay(1);
} else {
	switchSPIEnabled(false);
}

Serial.println("Passed SPI");
setStandbyDisable();
setSleepEnabled(false);

// select clock source to gyro
setClockSource(MPU60X0_CLOCK_PLL_XGYRO);
Serial.println(getClockSource());

// enable I2C master mode
setI2CMasterModeEnabled(1); 
Serial.println(getI2CMasterModeEnabled());
// set the I2C bus speed to 400 kHz
setMasterClockSpeed(13);
Serial.println(getMasterClockSpeed());
// set AK8963 to Power Down
test = writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN);
Serial.println(test);

// reset the MPU9250
reset();

// wait for MPU-9250 to come back up
delay(1);

// reset the AK8963
writeAKRegister(AK8963_RA_CNTL2, AK8963_CNTL2_RESET);

// select clock source to gyro
setClockSource(MPU60X0_CLOCK_PLL_XGYRO);

// check the WHO AM I byte, expected value is 0x71 (decimal 113)
if( getDeviceID() != 113 ){
    Serial.print(getDeviceID()); Serial.print(", "); Serial.println("9250 WIA Does not Match");
}

// enable accelerometer and gyro
setStandbyDisable();

/* setup the accel and gyro ranges */
setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);	// set gyro range to +/- 2000 deg/second
setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);		// set accel range to +- 2g

// enable I2C master mode
setI2CMasterModeEnabled(true); 

// set the I2C bus speed to 400 kHz
setMasterClockSpeed(13);

// check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
readAKRegisters(AK8963_RA_WIA, sizeof(buff), &buff[0]);
if(  buff[0] != 72 ){
    Serial.print(buff[0]); Serial.print(", ");Serial.println("AK does not match");
}

/* get the magnetometer calibration */

// set AK8963 to Power Down
writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN);

delay(100); // long wait between AK8963 mode changes

// set AK8963 to FUSE ROM access
writeAKRegister(AK8963_RA_CNTL2, AK8963_MODE_FUSEROM);

delay(100); // long wait between AK8963 mode changes

// read the AK8963 ASA registers and compute magnetometer scale factors
readAKRegisters(AK8963_RA_ASAX, sizeof(buff), &buff[0]);
//_magScaleX = ((((float)buff[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
//_magScaleY = ((((float)buff[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
//_magScaleZ = ((((float)buff[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 
_magScaleX = buff[0];
_magScaleX = buff[1];
_magScaleX = buff[2];
Serial.print(_magScaleX,8); Serial.print(", "); Serial.print(_magScaleY,8); 
Serial.print(", "); Serial.println(_magScaleZ,8);

// set AK8963 to Power Down
writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN);
delay(100); // long wait between AK8963 mode changes  

// set AK8963 to 16 bit resolution, 100 Hz update rate
writeRegister(AK8963_RA_CNTL1, 0x16);
delay(100); // long wait between AK8963 mode changes

// select clock source to gyro
setClockSource(MPU60X0_CLOCK_PLL_XGYRO); 

// instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);

// successful init, return 0
Serial.println("FINISHED");

}

void MPU60X0::getMotion9Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz)
{
uint8_t buff[21];
int16_t axx, ayy, azz, gxx, gyy, gzz;

readRegister(MPU60X0_RA_ACCEL_XOUT_H, sizeof(buff), &buff[0]); // grab the data from the MPU9250

axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
ayy = (((int16_t)buff[2]) << 8) | buff[3];
azz = (((int16_t)buff[4]) << 8) | buff[5];

gxx = (((int16_t)buff[8]) << 8) | buff[9];
gyy = (((int16_t)buff[10]) << 8) | buff[11];
gzz = (((int16_t)buff[12]) << 8) | buff[13];

*hx = (((int16_t)buff[15]) << 8) | buff[14];  
*hy = (((int16_t)buff[17]) << 8) | buff[16];
*hz = (((int16_t)buff[19]) << 8) | buff[18];

//*ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
//*ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
//*az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;

//*gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
//*gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
//*gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;

}

/* get accelerometer, gyro, and magnetometer data given pointers to store values /
void MPU60X0::getMotion9(float
ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz){
int16_t accel[3];
int16_t gyro[3];
int16_t mag[3];

getMotion9Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &mag[0], &mag[1], &mag[2]);

*ax = ((float) accel[0]); // typecast and scale to values
*ay = ((float) accel[1]);
*az = ((float) accel[2]);

*gx = ((float) gyro[0]);
*gy = ((float) gyro[1]);
*gz = ((float) gyro[2]);

*hx = ((float) mag[0]);
*hy = ((float) mag[1]);
*hz = ((float) mag[2]);

}

//========================================================
void MPU60X0::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest){
Wire.beginTransmission(devAddr); // open the device
Wire.write(subAddress); // specify the starting register address
Wire.endTransmission(false);

    Wire.requestFrom(devAddr, count); // specify the number of bytes to receive

}

/* writes a register to the AK8963 given a register address and data */
bool MPU60X0::writeAKRegister(uint8_t subAddress, uint8_t data){
uint8_t count = 1;
uint8_t buff[1];
writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, AK8963_DEFAULT_ADDRESS); // set slave 0 to the AK8963 and set for write
writeRegister(MPU60X0_RA_I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
writeRegister(MPU60X0_RA_I2C_SLV0_DO,data); // store the data for write
writeRegister(MPU60X0_RA_I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and send 1 byte

// read the register and confirm
readAKRegisters(subAddress, sizeof(buff), &buff[0]);

if(buff[0] == data) {
	return true;
}
else{
	return false;
}

}

/* reads registers from the AK8963 /
void MPU60X0::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t
dest){

writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
writeRegister(MPU60X0_RA_I2C_SLV0_REG,  subAddress); // set the register to the desired AK8963 sub address
writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and request the bytes
delay(1); // takes some time for these registers to fill
readRegister(MPU60X0_RA_EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers

}

void MPU60X0::writeRegister(uint8_t subAddress, uint8_t data){

Wire.beginTransmission(devAddr); // open the device
Wire.write(subAddress); // write the register address
Wire.write(data); // write the data
Wire.endTransmission();

delay(10); // need to slow down how fast I write to MPU9250

}

void MPU60X0::writeMagRegister(uint8_t subAddress, uint8_t data){

Wire.beginTransmission(magDevAddr); // open the device
Wire.write(subAddress); // write the register address
Wire.write(data); // write the data
Wire.endTransmission();

delay(10); // need to slow down how fast I write to MPU9250

}
/** Verify the I2C/SPI connection.

  • Make sure the device is connected and responds as expected.
  • @return True if connection is valid, false otherwise
    */
    bool MPU60X0::testConnection() {
    return getDeviceID() == 0b110100 || getDeviceID() == 0x71;
    }
    `

TwoWire does not name a type

Hello, I'm using this library on arduino 1.8.5 but master branch gives me this error:

MPU9250.h:123:5: error: 'TwoWire' does not name a type
TwoWire *_i2c;
^
exit status 1

Magnetometer readings freeze when SRD is above 34

Starting with the example script Interrupt_SPI, if I change the SRD value to something up to 34, everything goes smoothly. However, setting it to 35 or higher causes the magnetometer readings to freeze (i.e., the rest of the readings change, but not the magnetometer ones).

Magnetometer calibration breaks if biases and scales were set before

During calibration, the samples from the magnetometer are corrected using the scales and biases already stored.

e.g.: line 897:

_hxfilt = (_hxfilt * ((float)_coeff - 1) + (getMagX_uT() / _hxs + _hxb)) / ((float)_coeff);

This results in the new biases implicitly being relative to the old ones (which breaks them). A way to test it is to calibrate twice on a row, which results in different values.

I think the same line should be:

_hxfilt = (_hxfilt * ((float)_coeff - 1) + getMagX_uT()) / ((float)_coeff);

instead.

Amazing lib btw :)

Accelerometer calibration

When i try to calibrate the accelerometer i call the function for each axis and when i show the results all of the biases are 0 and the scale for each one is 1. Is that normal?

CALIB Gyro -- dont move until I say stop!!! done!
X: 0.004778
Y: 0.019271
Z: -0.003822
Gyro calib done
********** ACC calib **************
Enter when ready for dir 1 X+..
Enter when ready for dir 2 X-..
Enter when ready for dir 3 Y+..
Enter when ready for dir 4 Y-..
Enter when ready for dir 5 Z+..
Enter when ready for dir 6 Z-..
Acc calib done
Vals:
X: 0.000000/1.000000 (bias/scale)
Y: 0.000000/1.000000 (bias/scale)
Z: 0.000000/1.000000 (bias/scale)
CALIB MAG -- move in figure 8s until I say stop!!! done!
X: 28.451765/0.883743
Y: 47.327587/1.067171
Z: -51.208195/1.073661

Code used:

Serial.println(F("********** ACC calib **************"));

char * dirs[6] = { "X+", "X-", "Y+", "Y-", "Z+", "Z-"};

for (uint8_t i = 0; i < 6; i++) {
Serial.print(F("Enter when ready for dir "));
Serial.print((int)(i + 1));
Serial.print(' ');
Serial.print(dirs[i]);
while (! Serial.available() ) {
delay(10);
}

while (Serial.available()) {
  Serial.read();
  delay(5);
  Serial.print('.');
}
Serial.println();
IMU.calibrateAccel();

}

Serial.println(F("Acc calib done"));

Serial.println(F("Vals: "));

Serial.print(F("X: "));
Serial.print(IMU.getAccelBiasX_mss(), 6);
Serial.print('/');
Serial.println(IMU.getAccelScaleFactorX(), 6);

Serial.print(F("Y: "));
Serial.print(IMU.getAccelBiasY_mss(), 6);
Serial.print('/');
Serial.println(IMU.getAccelScaleFactorY(), 6);

Serial.print(F("Z: "));
Serial.print(IMU.getAccelBiasZ_mss(), 6);
Serial.print('/');
Serial.println(IMU.getAccelScaleFactorZ(), 6);

Reset SRD in calibration methods

Hi,

Thanks for the library! Well done and very robust and easy to debug code.

I am having the following problem regarding the Sensor Calibration routines, all of them (calibrateGyro(), calibrateAccel(), calibrateMag()).
In the beginning of each of them, we set the SRD to be 19 such that we have a frequency of 50 Hz, or a sample coming every 20ms.
My understanding is that this is required to perform correctly the sensor setup.

However, In the end of each of the calibration methods, we try to set the SRD back to the previous value, the one known as the protected _srd value.
For me, trying to do this always fails, as if the SRD method doesn't work when the SRD is set to 19.
Following this problem, the behavior i am observing is that if i run one of the calibration methods, i stay at an SRD of 19, regardless of the value i actually set, and the setup fails (returning a negative number; -6 for calibeGyro and calibAccel, and -2 for calibMag)

Commenting the instructions to set the SRD to 19, everything works fine and it looks like the sensor is calibrated and generates stable values.

My setup is:
I am using an Arduino Nano for the MCU to interface the sensors. And a non-latching interrupt based sampling, such that an ISR is waiting for an interrupt and then instructs the MCU to go and fetch the data.
Everything works well until i try to change the calibration.

Any help would be appreciated.
Thank you!

Data Recording and Storage at 1Khz (New to Arduino)

Hi there,

I have an issue with a project i'm working on.

I have an MPU-9250 and SD Card Module (With a compatible SanDisk 16GB SD Card) connected to an official Arduino UNO board.

I want to be able to record and store the data at a 1KHz sample rate. I believe two issues are perhaps the limitation of bolderflight package, as well as myself using print to read/write to the data, and not storing binary and using a buffer. If anyone could look into this that would be amazing, I'll be very active in checking this thread.

Thank you,

I've attached the file of code I've currently been using.
Arduino Example Issue.pdf

calibrateAccel

Is it works? It's every time show me that bias is 0, and scale is 1. But i can read peak 10.7mss on X and min -9.0 or another => real bias ~ -0.9. I checked sourses, but i can't find something wrong.

Can't compile for Arduino Nano nor Arduino Uno

When trying to compile the example Basic_SPI for the Arduino Nano or the Arduino Uno boards, I get the error
Global variables use 5368 bytes (262%) of dynamic memory, leaving -3320 bytes for local variables. Maximum is 2048 bytes.

So, is this library not compatible with these boards? In that case, it should be mentioned in the docs.

Edit: the FIFO implemented in the library takes up a lot of space. If the FIFO is removed, everything runs just fine (except for the FIFO related functions, of course). Maybe the FIFO could be allocated dynamically, with a warning that it should only be used by high end boards, such as the Due.

readSensor() stops giving back values after a couple minutes

Hi, I'm using the library with the ESP32 and it works fine for a few minutes but then it only returns -1 and I have to restart the program. I tried the basic and the advanced i2c example as well. Do you have any suggestion what could be the problem? Or if this is unavoidable is there a way to restart the imu faster? Because right now the begin function does calibration and everything which takes a couple seconds and I want it to go back reading data as fast as possible. Thanks!

Right order of axis for AHRS

Hello, anyone here tried to use this library coordinate system to get working quaternion with madwick? I have seen this implementation, but it seems that it uses NED reference frame. How can i convert your reference system to NED?

mFilterMadwick.update( elapsed, acceleration.x, acceleration.y, acceleration.z, gyroCalibrated.x, gyroCalibrated.y, gyroCalibrated.z, mCompassCalibrated.x, mCompassCalibrated.y, mCompassCalibrated.z );

Accelometer Z axis

The Z axis is not changing its value but I turn it at the right position and it is only changing if I turn it at the X or the Y axis then its the opposit of the x/y value. Do you know something about this problem?
(The gyroscop and the magnetometer are working correctly)

Sorry for my bad english but I hope you will understand my problem.

Does MPU9255 FIFO can enable Interrupt?

MPU9255 has 512 bytes FIFO and always old data will be replaced with new data. I could not find a solution to trigger Interrupt once FIFO is full.
If there is not way to use Interrupt, how to check FIFO size with SPI and Arduino.
Thanks

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.