Skip to content

stylistic clean-up and improvements in user feedback for magnetometer calibration #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

stylistic clean-up and improvements in user feedback for magnetometer calibration #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3a48319
stylistic clean up and improvements, no fundamental changes
robertoostenveld May 12, 2019
7785fac
implemented magContinuousCalMPU9250 method for MPU9250 object
robertoostenveld May 14, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 62 additions & 15 deletions src/MPU9250.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -511,24 +511,24 @@ void MPU9250::MPU9250SelfTest(float * destination)
int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
float factoryTrim[6];
uint8_t FS = GFS_250DPS;

writeByte(_I2Caddr, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
writeByte(_I2Caddr, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
writeByte(_I2Caddr, GYRO_CONFIG, FS<<3); // Set full scale range for the gyro to 250 dps
writeByte(_I2Caddr, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
writeByte(_I2Caddr, ACCEL_CONFIG, FS<<3); // 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(_I2Caddr, 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]) ;
aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;

readBytes(_I2Caddr, 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]) ;
gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
}

// Get average of 200 values and store as average current readings
Expand Down Expand Up @@ -617,6 +617,50 @@ void MPU9250::MPU9250SelfTest(float * destination)
}
}

void MPU9250::magContinuousCalMPU9250(int16_t * mag_temp, float * bias_dest, float * scale_dest)
{
int32_t mag_bias[3] = {0, 0, 0},
mag_scale[3] = {0, 0, 0};
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];
}
}
// Get hard iron correction
// Get 'average' x mag bias in counts
mag_bias[0] = (mag_max[0] + mag_min[0]) / 2;
// Get 'average' y mag bias in counts
mag_bias[1] = (mag_max[1] + mag_min[1]) / 2;
// Get 'average' z mag bias in counts
mag_bias[2] = (mag_max[2] + mag_min[2]) / 2;

// Save mag biases in G for main program
bias_dest[0] = (float)mag_bias[0] * mRes * factoryMagCalibration[0];
bias_dest[1] = (float)mag_bias[1] * mRes * factoryMagCalibration[1];
bias_dest[2] = (float)mag_bias[2] * mRes * factoryMagCalibration[2];

// Get soft iron correction estimate
// Get average x axis max chord length in counts
mag_scale[0] = (mag_max[0] - mag_min[0]) / 2;
// Get average y axis max chord length in counts
mag_scale[1] = (mag_max[1] - mag_min[1]) / 2;
// Get average z axis max chord length in counts
mag_scale[2] = (mag_max[2] - mag_min[2]) / 2;

float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
avg_rad /= 3.0;

scale_dest[0] = avg_rad / ((float)mag_scale[0]);
scale_dest[1] = avg_rad / ((float)mag_scale[1]);
scale_dest[2] = avg_rad / ((float)mag_scale[2]);
}

// Function which accumulates magnetometer data after device initialization.
// It calculates the bias and scale in the x, y, and z axes.
void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest)
Expand All @@ -633,14 +677,14 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest)

Serial.println(F("Mag Calibration: Wave device in a figure 8 until done!"));
Serial.println(
F(" 4 seconds to get ready followed by 15 seconds of sampling)"));
F(" 4 seconds to get ready followed by 15 seconds of sampling"));
delay(4000);

// shoot for ~fifteen seconds of mag data
// at 8 Hz ODR, new mag data is available every 125 ms
if (Mmode == M_8HZ)
{
sample_count = 128;
sample_count = 120;
}
// at 100 Hz ODR, new mag data is available every 10 ms
if (Mmode == M_100HZ)
Expand All @@ -650,6 +694,9 @@ void MPU9250::magCalMPU9250(float * bias_dest, float * scale_dest)

for (ii = 0; ii < sample_count; ii++)
{
if ((ii % (sample_count/15)) == 0)
Serial.println(F("keep waving..."));

readMagData(mag_temp); // Read the mag data

for (int jj = 0; jj < 3; jj++)
Expand Down Expand Up @@ -768,7 +815,7 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress)
else
{
return readByteSPI(registerAddress);
}
}
}
else
{
Expand All @@ -790,17 +837,17 @@ uint8_t MPU9250::readMagByteSPI(uint8_t registerAddress)
uint32_t count = 0;
while(((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 100000)) // Checks against the I2C_SLV4_DONE bit in the I2C master status register
{
I2C_MASTER_STATUS = readByteSPI(54);
I2C_MASTER_STATUS = readByteSPI(54);
}
if(count > 10000)
{
Serial.println(F("Timed out"));
}




return readByteSPI(53); // Read the data that is in the SLV4_DI register


return readByteSPI(53); // Read the data that is in the SLV4_DI register
}

uint8_t MPU9250::writeMagByteSPI(uint8_t registerAddress, uint8_t data)
Expand All @@ -816,7 +863,7 @@ uint8_t MPU9250::writeMagByteSPI(uint8_t registerAddress, uint8_t data)
uint32_t count = 0;
while(((I2C_MASTER_STATUS & 0b01000000) == 0) && (count++ < 10000)) // Checks against the I2C_SLV4_DONE bit in the I2C master status register
{
I2C_MASTER_STATUS = readByteSPI(54);
I2C_MASTER_STATUS = readByteSPI(54);
}
if(count > 10000)
{
Expand Down
27 changes: 14 additions & 13 deletions src/MPU9250.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,7 @@
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 for MPU9250 or 0x73 for MPU9255
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
Expand Down Expand Up @@ -214,11 +214,10 @@ class MPU9250
M_100HZ = 0x06 // 100 Hz continuous magnetometer
};


TwoWire * _wire; // Allows for use of various I2C ports
uint8_t _I2Caddr = MPU9250_ADDRESS_AD0; // Use AD0 by default

SPIClass * _spi; // Allows for use of different SPI ports
SPIClass * _spi; // Allows for use of different SPI ports
int8_t _csPin; // SPI chip select pin

uint32_t _interfaceSpeed; // Stores the desired I2C or SPi clock rate
Expand All @@ -229,13 +228,9 @@ class MPU9250
uint8_t Ascale = AFS_2G;
// Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mscale = MFS_16BITS;

// 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
// Choose either 8 Hz or 100 Hz continuous magnetometer data read
uint8_t Mmode = M_8HZ;




uint8_t writeByteWire(uint8_t, uint8_t, uint8_t);
uint8_t writeByteSPI(uint8_t, uint8_t);
uint8_t writeMagByteSPI(uint8_t subAddress, uint8_t data);
Expand All @@ -247,9 +242,10 @@ class MPU9250
void select();
void deselect();
void setupMagForSPI();
// TODO: Remove this next line
public:
uint8_t ak8963WhoAmI_SPI();

// TODO: Remove this next line
public:
uint8_t ak8963WhoAmI_SPI();

public:
float pitch, yaw, roll;
Expand All @@ -274,11 +270,15 @@ class MPU9250
float gyroBias[3] = {0, 0, 0},
accelBias[3] = {0, 0, 0},
magBias[3] = {0, 0, 0},
magScale[3] = {0, 0, 0};
magScale[3] = {1, 1, 1};
float selfTest[6];
// Stores the 16-bit signed accelerometer sensor output
int16_t accelCount[3];

// For continous calibration
int16_t mag_max[3] = {-32768, -32768, -32768},
mag_min[3] = {32767, 32767, 32767};

// Public method declarations
MPU9250( int8_t csPin, SPIClass &spiInterface = SPI, uint32_t spi_freq = SPI_DATA_RATE);
MPU9250( uint8_t address = MPU9250_ADDRESS_AD0, TwoWire &wirePort = Wire, uint32_t clock_frequency = 100000 );
Expand All @@ -294,7 +294,8 @@ class MPU9250
void initMPU9250();
void calibrateMPU9250(float * gyroBias, float * accelBias);
void MPU9250SelfTest(float * destination);
void magCalMPU9250(float * dest1, float * dest2);
void magCalMPU9250(float * bias_dest, float * scale_dest);
void magContinuousCalMPU9250(int16_t * mag_temp, float * bias_dest, float * scale_dest);
uint8_t writeByte(uint8_t, uint8_t, uint8_t);
uint8_t readByte(uint8_t, uint8_t);
uint8_t readBytes(uint8_t, uint8_t, uint8_t, uint8_t *);
Expand Down
7 changes: 4 additions & 3 deletions src/quaternionFilters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@

#include "quaternionFilters.h"

// These are the free parameters in the Mahony filter and fusion scheme, Kp
// for proportional feedback, Ki for integral
// These are the free parameters in the Mahony filter and fusion scheme
// Kp for proportional feedback, Ki for integral
#define Kp 2.0f * 5.0f
#define Ki 0.0f

Expand All @@ -32,6 +32,7 @@ static float GyroMeasDrift = PI * (0.0f / 180.0f);
// the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and
// fusion scheme.

static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta
// Compute zeta, the other free parameter in the Madgwick scheme usually
// set to a small or zero value
Expand Down Expand Up @@ -212,7 +213,7 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];

// Integrate rate of change of quaternion
pa = q2;
pb = q3;
Expand Down