Skip to content

Instantly share code, notes, and snippets.

@botamochi6277

botamochi6277/DualMPU9250Basic.ino

Created December 1, 2018 09:15
Show Gist options
  • Save botamochi6277/7bfc4e15443cfbaa3ab9882f6a953868 to your computer and use it in GitHub Desktop.
Save botamochi6277/7bfc4e15443cfbaa3ab9882f6a953868 to your computer and use it in GitHub Desktop.
Download ZIP
Dual MPU9250 libraries for M5stack
Raw
DualMPU9250Basic.ino
/**
* MPU9250 Basic Example Code for Dual MPU9250
* Copyright (c) 2014 Kris Winer
* Copyright (c) 2018 botamochi6277
* license: Beerware - Use this code however you'd like. If you
* find it useful you can buy me a beer some time.
*/
#include <M5Stack.h>
#include "utility/MPU9250.h"
#include "utility/quaternionFilters.h"
#define processing_out false
// AHRS: Attitude Heading Reference System(姿勢推定)
// ref: https://myenigma.hatenablog.com/entry/2015/11/09/183738
#define AHRS false // Set to false for basic data read
// #define SerialDebug true // Set to true to get Serial output for debugging
#define LCD
MPU9250 imu0;// 0x68
MPU9250 imu1;// 0x69
// Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances
void setup() {
M5.begin();
Wire.begin();
imu1.setAddress(0x69);
// IMU
if (!imu0.init()) {
// ERROR
M5.Lcd.setTextSize(1);
M5.Lcd.setTextColor(RED , BLACK);
M5.Lcd.fillScreen(BLACK); // clears the screen and buffer
M5.Lcd.setCursor(0, 10);
M5.Lcd.print("Could not connect to MPU9250 in M5Stack");
while (1);
}
if (!imu1.init()) {
// ERROR
M5.Lcd.setTextSize(1);
M5.Lcd.setTextColor(RED , BLACK);
M5.Lcd.fillScreen(BLACK); // clears the screen and buffer
M5.Lcd.setCursor(0, 10);
M5.Lcd.print("Could not connect to MPU9250 in GROVE module");
while (1);
}
M5.Lcd.setTextSize(1);
M5.Lcd.setTextColor(GREEN , BLACK);
M5.Lcd.fillScreen(BLACK);
}
void loop() {
imu0.update(false);
imu1.update(false);
if (!AHRS) {
imu0.delt_t = millis() - imu0.count;
if (imu0.delt_t > 500) {
#ifdef LCD
M5.Lcd.setTextSize(1);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextColor(GREEN , BLACK);
M5.Lcd.setCursor(0, 0); M5.Lcd.print("MPU9250");
M5.Lcd.setCursor(0, 20); M5.Lcd.print("0x68");
M5.Lcd.setCursor(0, 32); M5.Lcd.print(" x y z ");
M5.Lcd.setCursor(0, 48); M5.Lcd.print((int)(1000 * imu0.ax));
M5.Lcd.setCursor(32, 48); M5.Lcd.print((int)(1000 * imu0.ay));
M5.Lcd.setCursor(64, 48); M5.Lcd.print((int)(1000 * imu0.az));
M5.Lcd.setCursor(96, 48); M5.Lcd.print("mg");
M5.Lcd.setCursor(0, 64); M5.Lcd.print((int)(imu0.gx));
M5.Lcd.setCursor(32, 64); M5.Lcd.print((int)(imu0.gy));
M5.Lcd.setCursor(64, 64); M5.Lcd.print((int)(imu0.gz));
M5.Lcd.setCursor(96, 64); M5.Lcd.print("o/s");
// M5.Lcd.setCursor(0, 96); M5.Lcd.print((int)(imu0.mx));
// M5.Lcd.setCursor(32, 96); M5.Lcd.print((int)(imu0.my));
// M5.Lcd.setCursor(64, 96); M5.Lcd.print((int)(imu0.mz));
// M5.Lcd.setCursor(96, 96); M5.Lcd.print("mG");
M5.Lcd.setCursor(0, 20 + 80); M5.Lcd.print("0x68");
M5.Lcd.setCursor(0, 32 + 80); M5.Lcd.print(" x y z ");
M5.Lcd.setCursor(0, 48 + 80); M5.Lcd.print((int)(1000 * imu1.ax));
M5.Lcd.setCursor(32, 48 + 80); M5.Lcd.print((int)(1000 * imu1.ay));
M5.Lcd.setCursor(64, 48 + 80); M5.Lcd.print((int)(1000 * imu1.az));
M5.Lcd.setCursor(96, 48 + 80); M5.Lcd.print("mg");
M5.Lcd.setCursor(0, 64 + 80); M5.Lcd.print((int)(imu1.gx));
M5.Lcd.setCursor(32, 64 + 80); M5.Lcd.print((int)(imu1.gy));
M5.Lcd.setCursor(64, 64 + 80); M5.Lcd.print((int)(imu1.gz));
M5.Lcd.setCursor(96, 64 + 80); M5.Lcd.print("o/s");
M5.Lcd.setCursor(0, 80 + 80); M5.Lcd.print("Gyro T ");
#endif // LCD
imu0.count = millis();
// digitalWrite(myLed, !digitalRead(myLed)); // toggle led
} // end of if (IMU.delt_t > 500)
}
// strike/shock/impact detection
if (imu0.isStrike()) {
M5.Speaker.tone(1000, 100);
} else {
M5.Speaker.mute();
}
}
Raw
MPU9250.cpp
#include "MPU9250.h"
#include "quaternionFilters.h"
//==============================================================================
//====== Set of useful function to access acceleration. gyroscope, magnetometer,
//====== and temperature data
//==============================================================================
MPU9250::MPU9250() {
MPU9250_address_ = 0x68;
AK8963_address_ = 0x0C;
// Specify sensor full scale
Gscale = GFS_250DPS;
Ascale = AFS_2G;
threshold_ = 1.8;
// Choose either 14-bit or 16-bit magnetometer resolution
Mscale = MFS_16BITS;
// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
Mmode = 0x02;
}
void MPU9250::setAddress(uint8_t MPU9250_address,
uint8_t AK8963_address) {
MPU9250_address_ = MPU9250_address;// 0x68 or 0x69s
AK8963_address_ = AK8963_address;
}
uint8_t MPU9250::address(bool is_inertia) {
if (is_inertia) {
return MPU9250_address_;
} else {
return AK8963_address_;
}
}
void MPU9250::update(bool is_mag) {
// If intPin goes high, all data registers have new data
// On interrupt, check if data ready interrupt
if (readByte(MPU9250_address_, INT_STATUS) & 0x01) {
readAccelData(accelCount); // Read the x/y/z adc values
getAres();
// Now we'll calculate the accleration value into actual g's
// This depends on scale being set
ax = (float)accelCount[0] * aRes; // - accelBias[0];
ay = (float)accelCount[1] * aRes; // - accelBias[1];
az = (float)accelCount[2] * aRes; // - accelBias[2];
readGyroData(gyroCount); // Read the x/y/z adc values
getGres();
// Calculate the gyro value into actual degrees per second
// This depends on scale being set
gx = (float)gyroCount[0] * gRes;
gy = (float)gyroCount[1] * gRes;
gz = (float)gyroCount[2] * gRes;
// update magnetic values
if (is_mag) {
readMagData(magCount); // Read the x/y/z adc values
getMres();
// User environmental x-axis correction in milliGauss, should be
// automatically calculated
magbias[0] = + 470.;
// User environmental x-axis correction in milliGauss TODO axis??
magbias[1] = + 120.;
// User environmental x-axis correction in milliGauss
magbias[2] = + 125.;
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental
// corrections
// Get actual magnetometer value, this depends on scale being set
mx = (float)magCount[0] * mRes * magCalibration[0] -
magbias[0];
my = (float)magCount[1] * mRes * magCalibration[1] -
magbias[1];
mz = (float)magCount[2] * mRes * magCalibration[2] -
magbias[2];
}
} // end of if (readByte(MPU9250_address_, INT_STATUS) & 0x01)
// Must be called before updating quaternions!
updateTime();
// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of
// the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis
// (+ up) of accelerometer and gyro! We have to make some allowance for this
// orientationmismatch in feeding the output to the quaternion filter. For the
// MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward
// along the x-axis just like in the LSM9DS0 sensor. This rotation can be
// modified to allow any convenient orientation convention. This is ok by
// aircraft orientation standards! Pass gyro rate as rad/s
// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
MahonyQuaternionUpdate(ax, ay, az, gx * DEG_TO_RAD,
gy * DEG_TO_RAD, gz * DEG_TO_RAD, my,
mx, mz, deltat);
}
void MPU9250::setRange(uint8_t a_scale, uint8_t g_scale) {
switch (a_scale) {
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
case 2:
Ascale = AFS_2G;
threshold_ = a_scale * 0.9;
break;
case 4:
Ascale = AFS_4G;
threshold_ = a_scale * 0.9;
break;
case 8:
Ascale = AFS_8G;
threshold_ = a_scale * 0.9;
break;
case 16:
Ascale = AFS_16G;
threshold_ = a_scale * 0.9;
break;
default :
Ascale = AFS_2G;
break;
}
switch (g_scale) {
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case 250:
Gscale = GFS_250DPS;
break;
case 500:
Gscale = GFS_500DPS;
break;
case 1000:
Gscale = GFS_1000DPS;
break;
case 2000:
Gscale = GFS_2000DPS;
break;
default :
Gscale = GFS_250DPS;
break;
}
}
bool MPU9250::isStrike() {
if ( (abs(ax) > threshold_) ||
(abs(ay) > threshold_) ||
(abs(az) > threshold_)) {
return true;
} else {
return false;
}
}
bool MPU9250::init() {
char c = readByte(MPU9250_address_, WHO_AM_I_MPU9250);
// MPU9250: 0x71
// MPU9255: 0x73
// WHO_AM_I should always be 0x68
if (c == 0x71 || c == 0x73) {
// Start by performing self test and reporting values
MPU9250SelfTest(SelfTest);
// Calibrate gyro and accelerometers, load biases in bias registers
calibrateMPU9250(gyroBias, accelBias);
initMPU9250();
return true;
} else {
return false;
}
}
void MPU9250::getMres() {
switch (Mscale) {
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
mRes = 10.*4912. / 8190.; // Proper scale to return milliGauss
break;
case MFS_16BITS:
mRes = 10.*4912. / 32760.0; // Proper scale to return milliGauss
break;
}
}
void MPU9250::getGres() {
switch (Gscale) {
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS:
gRes = 250.0 / 32768.0;
break;
case GFS_500DPS:
gRes = 500.0 / 32768.0;
break;
case GFS_1000DPS:
gRes = 1000.0 / 32768.0;
break;
case GFS_2000DPS:
gRes = 2000.0 / 32768.0;
break;
}
}
void MPU9250::getAres() {
switch (Ascale) {
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
aRes = 2.0 / 32768.0;
break;
case AFS_4G:
aRes = 4.0 / 32768.0;
break;
case AFS_8G:
aRes = 8.0 / 32768.0;
break;
case AFS_16G:
aRes = 16.0 / 32768.0;
break;
}
}
void MPU9250::readAccelData(int16_t * destination) {
uint8_t rawData[6]; // x/y/z accel register data stored here
readBytes(MPU9250_address_, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}
void MPU9250::readGyroData(int16_t * destination) {
uint8_t rawData[6]; // x/y/z gyro register data stored here
readBytes(MPU9250_address_, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}
void MPU9250::readMagData(int16_t * destination) {
// x/y/z gyro register data, ST2 register stored here, must read ST2 at end of
// data acquisition
uint8_t rawData[7];
// Wait for magnetometer data ready bit to be set
if (readByte(AK8963_address_, AK8963_ST1) & 0x01) {
// Read the six raw data and ST2 registers sequentially into data array
readBytes(AK8963_address_, AK8963_XOUT_L, 7, &rawData[0]);
uint8_t c = rawData[6]; // End data read by reading ST2 register
// Check if magnetic sensor overflow set, if not then report data
if (!(c & 0x08)) {
// Turn the MSB and LSB into a signed 16-bit value
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];
// Data stored as little Endian
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
}
}
}
int16_t MPU9250::readTempData() {
uint8_t rawData[2]; // x/y/z gyro register data stored here
readBytes(MPU9250_address_, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value
}
// Calculate the time the last update took for use in the quaternion filters
void MPU9250::updateTime() {
Now = micros();
// Set integration time by time elapsed since last filter update
deltat = ((Now - lastUpdate) / 1000000.0f);
lastUpdate = Now;
sum += deltat; // sum for averaging filter update rate
sumCount++;
}
void MPU9250::initAK8963(float * destination) {
// First extract the factory calibration for each magnetometer axis
uint8_t rawData[3]; // x/y/z gyro calibration data stored here
writeByte(AK8963_address_, AK8963_CNTL, 0x00); // Power down magnetometer
delay(10);
writeByte(AK8963_address_, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
delay(10);
readBytes(AK8963_address_, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
destination[0] = (float)(rawData[0] - 128) / 256. + 1.; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128) / 256. + 1.;
destination[2] = (float)(rawData[2] - 128) / 256. + 1.;
writeByte(AK8963_address_, AK8963_CNTL, 0x00); // Power down magnetometer
delay(10);
// Configure the magnetometer for continuous read and highest resolution
// set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
// and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
writeByte(AK8963_address_, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
delay(10);
}
void MPU9250::initMPU9250() {
// wake up device
writeByte(MPU9250_address_, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
delay(100); // Wait for all registers to reset
// get stable time source
writeByte(MPU9250_address_, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
delay(200);
// Configure Gyro and Thermometer
// Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
// minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
// be higher than 1 / 0.0059 = 170 Hz
// DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
// With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU9250_address_, CONFIG, 0x03);
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU9250_address_, SMPLRT_DIV, 0x04);
// Use a 200 Hz rate; a rate consistent with the filter update rate
// determined inset in CONFIG above
// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
uint8_t c = readByte(MPU9250_address_, GYRO_CONFIG); // get current GYRO_CONFIG register value
// c = c & ~0xE0; // Clear self-test bits [7:5]
c = c & ~0x02; // Clear Fchoice bits [1:0]
c = c & ~0x18; // Clear AFS bits [4:3]
c = c | Gscale << 3; // Set full scale range for the gyro
// c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
writeByte(MPU9250_address_, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
// Set accelerometer full-scale range configuration
c = readByte(MPU9250_address_, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
// c = c & ~0xE0; // Clear self-test bits [7:5]
c = c & ~0x18; // Clear AFS bits [4:3]
c = c | Ascale << 3; // Set full scale range for the accelerometer
writeByte(MPU9250_address_, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
// Set accelerometer sample rate configuration
// It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
// accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
c = readByte(MPU9250_address_, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
writeByte(MPU9250_address_, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
// but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
// clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
writeByte(MPU9250_address_, INT_PIN_CFG, 0x22);
writeByte(MPU9250_address_, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
delay(100);
}
// Function which accumulates gyro and accelerometer data after device
// initialization. It calculates the average of the at-rest readings and then
// loads the resulting offsets into accelerometer and gyro bias registers.
void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) {
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
// reset device
// Write a one to bit 7 reset bit; toggle reset device
writeByte(MPU9250_address_, PWR_MGMT_1, 0x80);
delay(100);
// get stable time source; Auto select clock source to be PLL gyroscope
// reference if ready else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_address_, PWR_MGMT_1, 0x01);
writeByte(MPU9250_address_, PWR_MGMT_2, 0x00);
delay(200);
// Configure device for bias calculation
writeByte(MPU9250_address_, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU9250_address_, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU9250_address_, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(MPU9250_address_, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU9250_address_, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(MPU9250_address_, USER_CTRL, 0x0C); // Reset FIFO and DMP
delay(15);
// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU9250_address_, CONFIG, 0x01); // Set low-pass filter to 188 Hz
writeByte(MPU9250_address_, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
writeByte(MPU9250_address_, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU9250_address_, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
// Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU9250_address_, USER_CTRL, 0x40); // Enable FIFO
writeByte(MPU9250_address_, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
writeByte(MPU9250_address_, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
readBytes(MPU9250_address_, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
readBytes(MPU9250_address_, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] );
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] );
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] );
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] );
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]);
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
if (accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (int32_t) accelsensitivity;}
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1] / 4) & 0xFF;
data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2] / 4) & 0xFF;
// Push gyro biases to hardware registers
writeByte(MPU9250_address_, XG_OFFSET_H, data[0]);
writeByte(MPU9250_address_, XG_OFFSET_L, data[1]);
writeByte(MPU9250_address_, YG_OFFSET_H, data[2]);
writeByte(MPU9250_address_, YG_OFFSET_L, data[3]);
writeByte(MPU9250_address_, ZG_OFFSET_H, data[4]);
writeByte(MPU9250_address_, ZG_OFFSET_L, data[5]);
// Output scaled gyro biases for display in the main program
gyroBias[0] = (float) gyro_bias[0] / (float) gyrosensitivity;
gyroBias[1] = (float) gyro_bias[1] / (float) gyrosensitivity;
gyroBias[2] = (float) gyro_bias[2] / (float) gyrosensitivity;
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
readBytes(MPU9250_address_, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_address_, YA_OFFSET_H, 2, &data[0]);
accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_address_, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
for (ii = 0; ii < 3; ii++) {
if ((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
}
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1] / 8);
accel_bias_reg[2] -= (accel_bias[2] / 8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
writeByte(MPU9250_address_, XA_OFFSET_H, data[0]);
writeByte(MPU9250_address_, XA_OFFSET_L, data[1]);
writeByte(MPU9250_address_, YA_OFFSET_H, data[2]);
writeByte(MPU9250_address_, YA_OFFSET_L, data[3]);
writeByte(MPU9250_address_, ZA_OFFSET_H, data[4]);
writeByte(MPU9250_address_, ZA_OFFSET_L, data[5]);
// Output scaled accelerometer biases for display in the main program
accelBias[0] = (float)accel_bias[0] / (float)accelsensitivity;
accelBias[1] = (float)accel_bias[1] / (float)accelsensitivity;
accelBias[2] = (float)accel_bias[2] / (float)accelsensitivity;
}
// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250::MPU9250SelfTest(float * destination) { // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
uint8_t selfTest[6];
int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
float factoryTrim[6];
uint8_t FS = 0;
writeByte(MPU9250_address_, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
writeByte(MPU9250_address_, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
writeByte(MPU9250_address_, GYRO_CONFIG, 1 << FS); // Set full scale range for the gyro to 250 dps
writeByte(MPU9250_address_, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
writeByte(MPU9250_address_, ACCEL_CONFIG, 1 << FS); // Set full scale range for the accelerometer to 2 g
for ( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
readBytes(MPU9250_address_, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
readBytes(MPU9250_address_, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
// Configure the accelerometer for self-test
writeByte(MPU9250_address_, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
writeByte(MPU9250_address_, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
delay(25); // Delay a while to let the device stabilize
for ( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
readBytes(MPU9250_address_, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
readBytes(MPU9250_address_, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Configure the gyro and accelerometer for normal operation
writeByte(MPU9250_address_, ACCEL_CONFIG, 0x00);
writeByte(MPU9250_address_, GYRO_CONFIG, 0x00);
delay(25); // Delay a while to let the device stabilize
// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
selfTest[0] = readByte(MPU9250_address_, SELF_TEST_X_ACCEL); // X-axis accel self-test results
selfTest[1] = readByte(MPU9250_address_, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
selfTest[2] = readByte(MPU9250_address_, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
selfTest[3] = readByte(MPU9250_address_, SELF_TEST_X_GYRO); // X-axis gyro self-test results
selfTest[4] = readByte(MPU9250_address_, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
selfTest[5] = readByte(MPU9250_address_, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
// Retrieve factory self-test value from self-test code reads
factoryTrim[0] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
factoryTrim[1] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
factoryTrim[2] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
factoryTrim[3] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = (float)(2620 / 1 << FS) * (pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
// To get percent, must multiply by 100
for (int i = 0; i < 3; i++) {
destination[i] = 100.0 * ((float)(aSTAvg[i] - aAvg[i])) / factoryTrim[i]; // Report percent differences
destination[i + 3] = 100.0 * ((float)(gSTAvg[i] - gAvg[i])) / factoryTrim[i + 3]; // Report percent differences
}
}
// Wire.h read and write protocols
void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}
uint8_t MPU9250::readByte(uint8_t address, uint8_t subAddress) {
uint8_t data; // `data` will store the register data
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count,
uint8_t * dest) {
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
uint8_t i = 0;
Wire.requestFrom(address, count); // Read bytes from slave register address
while (Wire.available()) {
dest[i++] = Wire.read();
} // Put read results in the Rx buffer
}
Raw
MPU9250.h
/*
Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or
a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire
library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast
I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
*/
#ifndef _MPU9250_H_
#define _MPU9250_H_
#include <SPI.h>
#include <Wire.h>
// See also MPU-9250 Register Map and Descriptions, Revision 4.0,
// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above
// document; the MPU9250 and MPU9150 are virtually identical but the latter has
// a different register map
// https://cdn.sparkfun.com/assets/learn_tutorials/5/5/0/MPU-9250-Register-Map.pdf
//Magnetometer Registers
// #define AK8963_ADDRESS 0x0C
#define WHO_AM_I_AK8963 0x00 // should return 0x48
#define INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
#define SELF_TEST_X_GYRO 0x00
#define SELF_TEST_Y_GYRO 0x01
#define SELF_TEST_Z_GYRO 0x02
/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
#define Y_FINE_GAIN 0x04
#define Z_FINE_GAIN 0x05
#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC 0x07
#define YA_OFFSET_H 0x08
#define YA_OFFSET_L_TC 0x09
#define ZA_OFFSET_H 0x0A
#define ZA_OFFSET_L_TC 0x0B */
#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
#define SELF_TEST_Z_ACCEL 0x0F
#define SELF_TEST_A 0x10
#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG2 0x1D
#define LP_ACCEL_ODR 0x1E
#define WOM_THR 0x1F
// Duration counter threshold for motion interrupt generation, 1 kHz rate,
// LSB = 1 ms
#define MOT_DUR 0x20
// Zero-motion detection threshold bits [7:0]
#define ZMOT_THR 0x21
// Duration counter threshold for zero motion interrupt generation, 16 Hz rate,
// LSB = 64 ms
#define ZRMOT_DUR 0x22
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34
#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39 // Check DMP interrupt
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2 0x6C
#define DMP_BANK 0x6D // Activates a specific bank in the DMP
#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
#define DMP_REG_1 0x70
#define DMP_REG_2 0x71
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E
// Using the MPU-9250 breakout board, ADO is set to 0
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
// #define ADO 0
// #if ADO
// #define MPU9250_ADDRESS 0x69 // Device address when ADO = 1
// #else
// #define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
// AK8963_ADDRESS: 0x0C, 0x0D, 0x0E and 0x0F depending on CAD0/1 pins
// https://strawberry-linux.com/pub/AK8963.pdf
// #define AK8963_ADDRESS 0x0C // Address of magnetometer
// #endif // ADO
class MPU9250 {
protected:
// to use two MPU9250, add address member variables to this class.
uint8_t MPU9250_address_;
uint8_t AK8963_address_;
// Set initial input parameters
enum Ascale {
AFS_2G = 0,
AFS_4G,
AFS_8G,
AFS_16G
};
enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};
enum Mscale {
MFS_14BITS = 0, // 0.6 mG per LSB
MFS_16BITS // 0.15 mG per LSB
};
// Specify sensor full scale
uint8_t Gscale;
uint8_t Ascale;
// Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mscale;
// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
uint8_t Mmode;
public:
float pitch, yaw, roll;
float temperature; // Stores the real internal chip temperature in Celsius
int16_t tempCount; // Temperature raw count output
uint32_t delt_t = 0; // Used to control display output rate
uint32_t count = 0, sumCount = 0; // used to control display output rate
float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
// Scale resolutions per LSB for the sensors
float aRes, gRes, mRes;
// Variables to hold latest sensor data values
float ax, ay, az, gx, gy, gz, mx, my, mz;
// Factory mag calibration and mag bias
float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0};
// Bias corrections for gyro and accelerometer
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};
float SelfTest[6];
// Stores the 16-bit signed accelerometer sensor output
int16_t accelCount[3];
float threshold_; // threashold for detecting strike
public:
MPU9250();
void setAddress(uint8_t MPU9250_address = 0x68,
uint8_t AK8963_address = 0x0c);
uint8_t address(bool is_inertia = true);
void update(bool is_mag = true);
void setRange(uint8_t a_scale, uint8_t g_scale);
bool isStrike();
bool init();
void getMres();
void getGres();
void getAres();
void readAccelData(int16_t *);
void readGyroData(int16_t *);
void readMagData(int16_t *);
int16_t readTempData();
void updateTime();
void initAK8963(float *);
void initMPU9250();
void calibrateMPU9250(float * gyroBias, float * accelBias);
void MPU9250SelfTest(float * destination);
void writeByte(uint8_t, uint8_t, uint8_t);
uint8_t readByte(uint8_t, uint8_t);
void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
}; // class MPU9250
#endif // _MPU9250_H_
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment