From 3a483190f09db322835adc7632f1561a200fe11c Mon Sep 17 00:00:00 2001 From: Robert Oostenveld Date: Sun, 12 May 2019 12:32:44 +0200 Subject: [PATCH 1/2] stylistic clean up and improvements, no fundamental changes - cleaned up whitespace at end of lines - mention mpu9250 and the alternative mpu9255 that also works - give better user feedback during magnetometer calibration phase --- src/MPU9250.cpp | 33 ++++++++++++++++++--------------- src/MPU9250.h | 9 ++------- src/quaternionFilters.cpp | 7 ++++--- 3 files changed, 24 insertions(+), 25 deletions(-) diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 409192e..7ddb775 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 @@ -633,14 +633,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 +650,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 +771,7 @@ uint8_t MPU9250::readByte(uint8_t deviceAddress, uint8_t registerAddress) else { return readByteSPI(registerAddress); - } + } } else { @@ -790,17 +793,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 +819,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..d8d2339 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,7 +214,6 @@ 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 @@ -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); 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; From 7785facabab7282b581519a0022385b56a896abc Mon Sep 17 00:00:00 2001 From: Robert Oostenveld Date: Tue, 14 May 2019 22:57:13 +0200 Subject: [PATCH 2/2] implemented magContinuousCalMPU9250 method for MPU9250 object This can be regularly called in the main loop() to update the magnetometer calibration while the sketch is already functional. This means that calibration does not block the function for 15 seconds. --- src/MPU9250.cpp | 44 ++++++++++++++++++++++++++++++++++++++++++++ src/MPU9250.h | 18 ++++++++++++------ 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/MPU9250.cpp b/src/MPU9250.cpp index 7ddb775..3a25686 100644 --- a/src/MPU9250.cpp +++ b/src/MPU9250.cpp @@ -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) diff --git a/src/MPU9250.h b/src/MPU9250.h index d8d2339..72de87e 100644 --- a/src/MPU9250.h +++ b/src/MPU9250.h @@ -217,7 +217,7 @@ class MPU9250 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 @@ -242,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; @@ -269,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 ); @@ -289,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 *);