Kris Winer
Added mag calibration and interrupt-based data ready
Dependencies: BLE_API mbed-src nRF51822
Diff: MPU9250.h
- Revision:
- 4:8d11bfc7cac0
- Parent:
- 3:fe46f14f5aef
--- a/MPU9250.h Mon Jun 15 21:34:33 2015 +0000
+++ b/MPU9250.h Thu Sep 22 01:21:24 2016 +0000
@@ -191,18 +191,24 @@
//Set up I2C, (SDA,SCL)
//I2C i2c(I2C_SDA, I2C_SCL);
-I2C i2c(P0_20, P0_22);
+//I2C i2c(P0_0, P0_1); // nRF51 FC
+//I2C i2c(P0_6, P0_7); // nRF52 QFN dev board
+I2C i2c(P0_6, P0_5); // nRF52 CIAA dev board
-DigitalOut myled(LED1);
+//DigitalOut myled(P0_19); // mbed kit
+//DigitalOut myled(P0_18); // nRF51822 module
+DigitalOut myled(P0_24); // nRF52832 module
// Pin definitions
-int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins
+bool newData = false;
+bool newMagData = false;
+int16_t MPU9250Data[7]; // used to read all 14 bytes at once from the MPU9250 accel/gyro
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias
-float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
+float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}, magScale[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
@@ -235,7 +241,7 @@
//====== Set of useful function to access acceleratio, gyroscope, and temperature data
//===================================================================================================================
- void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
char data_write[2];
data_write[0] = subAddress;
@@ -272,10 +278,10 @@
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
- mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
+ mRes = 10.0*4912.0/8190.0; // Proper scale to return milliGauss
break;
case MFS_16BITS:
- mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
+ mRes = 10.0*4912.0/32760.0; // Proper scale to return milliGauss
break;
}
}
@@ -288,16 +294,16 @@
// 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;
+ gRes = 250.0f/32768.0f;
break;
case GFS_500DPS:
- gRes = 500.0/32768.0;
+ gRes = 500.0f/32768.0f;
break;
case GFS_1000DPS:
- gRes = 1000.0/32768.0;
+ gRes = 1000.0f/32768.0f;
break;
case GFS_2000DPS:
- gRes = 2000.0/32768.0;
+ gRes = 2000.0f/32768.0f;
break;
}
}
@@ -310,20 +316,32 @@
// 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;
+ aRes = 2.0f/32768.0f;
break;
case AFS_4G:
- aRes = 4.0/32768.0;
+ aRes = 4.0f/32768.0f;
break;
case AFS_8G:
- aRes = 8.0/32768.0;
+ aRes = 8.0f/32768.0f;
break;
case AFS_16G:
- aRes = 16.0/32768.0;
+ aRes = 16.0f/32768.0f;
break;
}
}
+void readMPU9250Data(int16_t * destination)
+{
+ uint8_t rawData[14]; // x/y/z accel register data stored here
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 14, &rawData[0]); // Read the 14 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] ;
+ destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
+ destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
+ destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
+ destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
+}
void readAccelData(int16_t * destination)
{
@@ -346,15 +364,13 @@
void readMagData(int16_t * destination)
{
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
- if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
uint8_t c = rawData[6]; // End data read by reading ST2 register
if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
- destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
- destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
- destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
+ destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
+ destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
}
- }
}
int16_t readTempData()
@@ -374,12 +390,15 @@
void initAK8963(float * destination)
{
// First extract the factory calibration for each magnetometer axis
- uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ uint8_t rawData[3] = {0, 0, 0}; // x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
wait(0.01);
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
wait(0.01);
- readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ // readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ rawData[0] = readByte(AK8963_ADDRESS, AK8963_ASAX);
+ rawData[1] = readByte(AK8963_ADDRESS, AK8963_ASAY);
+ rawData[2] = readByte(AK8963_ADDRESS, AK8963_ASAZ);
destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
@@ -438,8 +457,8 @@
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, 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
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12); // INT is 50 microsecond pulse and any read to clear
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
@@ -589,13 +608,57 @@
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
+void magcalMPU9250(float * dest1, float * dest2)
+{
+ uint16_t ii = 0, sample_count = 0;
+ int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
+ int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
+
+ // shoot for ~fifteen seconds of mag data
+ if(Mmode == 0x02) sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms
+
+ for(ii = 0; ii < sample_count; ii++) {
+ readMagData(mag_temp); // Read the mag data
+
+ for (int jj = 0; jj < 3; jj++) {
+ if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
+ if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
+ }
+
+ if(Mmode == 0x02) wait(0.135); // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) wait(0.012); // at 100 Hz ODR, new mag data is available every 10 ms
+ }
+
+ // Get hard iron correction
+ mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
+ mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
+ mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
+
+ dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program
+ dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];
+ dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];
+
+ // Get soft iron correction estimate
+ mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts
+ mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts
+ mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts
+
+ float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
+ avg_rad /= 3.0;
+
+ dest2[0] = avg_rad/((float)mag_scale[0]);
+ dest2[1] = avg_rad/((float)mag_scale[1]);
+ dest2[2] = avg_rad/((float)mag_scale[2]);
+}
+
// Accelerometer and gyroscope self test; check calibration wrt factory settings
void 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];
+ int32_t gAvg[3] = {0, 0, 0}, aAvg[3] = {0, 0, 0}, aSTAvg[3] = {0, 0, 0}, gSTAvg[3] = {0, 0, 0};
float factoryTrim[6];
uint8_t FS = 0;
@@ -670,8 +733,8 @@
// 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
+ destination[i] = 100.0f*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.0f; // Report percent differences
+ destination[i+3] = 100.0f*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.0f; // Report percent differences
}
}
@@ -717,7 +780,7 @@
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
- norm = sqrt(ax * ax + ay * ay + az * az);
+ norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
@@ -725,7 +788,7 @@
az *= norm;
// Normalise magnetometer measurement
- norm = sqrt(mx * mx + my * my + mz * mz);
+ norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
@@ -749,7 +812,7 @@
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
- norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
+ norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
@@ -767,7 +830,7 @@
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
- norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
@@ -802,7 +865,7 @@
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
- norm = sqrt(ax * ax + ay * ay + az * az);
+ norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
@@ -810,7 +873,7 @@
az *= norm;
// Normalise magnetometer measurement
- norm = sqrt(mx * mx + my * my + mz * mz);
+ norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
@@ -820,7 +883,7 @@
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
- bx = sqrt((hx * hx) + (hy * hy));
+ bx = sqrtf((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
@@ -863,7 +926,7 @@
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
// Normalise quaternion
- norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+ norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;