diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 409192e..3a25686 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -511,7 +511,7 @@ 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 @@ -519,16 +519,16 @@ void MPU9250::MPU9250SelfTest(float * destination) 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 @@ -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) @@ -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) @@ -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++) @@ -768,7 +815,7 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) else { return readByteSPI(registerAddress); - } + } } else { @@ -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) @@ -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) { diff --git a/src/MPU9250.h b/src/MPU9250.h index 34cbafb..72de87e 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -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 @@ -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 @@ -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); @@ -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; @@ -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 ); @@ -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 *); diff --git a/src/quaternionFilters.cpp b/src/quaternionFilters.cpp index 87094fe..6c944c2 100644 --- a/src/quaternionFilters.cpp +++ b/src/quaternionFilters.cpp @@ -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 @@ -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 @@ -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;