MPU-9250 sample rate lower than the one selected in register map - i2c

Question

I'm using Arduino UNO and I2C protocol to read data from MPU 9250 in arduino IDE. But the sampling rate is lower than the one I selected, indeed, I try to use 1KHz but the maximum speed I can reach is by reading only one of the sensors and is 25 Hz. Do you know I can improve the speed of sampling?
This is my code:
float ax, ay, az, gx, gy, gz;//Some original data of accelerometer, gyroscope and magenetometer
float mx, my, mz;
float magCalibration[3]={0, 0 ,0};
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
// Specify sensor full scale
uint8_t Gscale = 0; //GFS_250DPS;
uint8_t Ascale = 0; //AFS_2G;
uint8_t Mscale = 1; // Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
// scale resolutions per LSB for the sensors
float gRes = 250./32768.0; // *(PI/180.0); degrees/s
float mRes = 1000.*4219./32760.0; // nT
float aRes = 2./32768.0; //g
float lastUpdate=0;
float Now, deltat;
void setup() {
Wire.begin();//Initialize
Serial.begin(115200);//Initialize the serial
//MyBlue.begin(9600);
initMPU9250(); //Inititalize the accelerometer
initAK8963(magCalibration);//Initialize the magnetometer
//int tickEvent1=t.every(timeChange, get_motion);
//int tickEvent2=t.every(500, printout) ;//print out the serial 50 after this line
}
void loop() {
get_motion();
printout();
Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
Serial.println(deltat);
}
void printout()
{
Serial.print(ax);
Serial.print(',');
Serial.print(ay);
Serial.print(',');
Serial.println(az);
Serial.print(',');
Serial.print(gx);
Serial.print(',');
Serial.print(gy);
Serial.print(',');
Serial.println(gz);
Serial.print(',');
Serial.print(mx);
Serial.print(',');
Serial.print(my);
Serial.print(',');
Serial.println(mz);
}
void get_motion()
{
if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
ax = (float)accelCount[0] * aRes;// * 9.81;
ay = (float)accelCount[1] * aRes; //* 9.81;
az = (float)accelCount[2] * aRes; //* 9.81;
readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
gx = (float)gyroCount[0] * gRes;
gy = (float)gyroCount[1] * gRes;
gz = (float)gyroCount[2] * gRes;
readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0] * mRes * magCalibration[0];
my = (float)magCount[1] * mRes * magCalibration[1];
mz = (float)magCount[2] * mRes * magCalibration[2];
}
}
void 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 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 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
//read mag
writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
delay(10);
writeByte(AK8963_ADDRESS, 0x0A, 0x01); //enable the magnetometer
delay(100);
if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 6, &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)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] ;
//}
}
}
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
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 initMPU9250()
{
// wake up device
//writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80);
delay(100);
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, 0x01);
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // 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 GFS_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 & ~0x03; // Clear Fchoice bits [1:0]
c = c & ~0x18; // Clear GFS 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 | 0x02; // 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);
}
// Wire.h read and write protocols
void 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 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 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
}

Answer 1

With the settings you have in initMPU9250, I don't see any reason not to get 1kHz sampling rate. However, Serial.print may be slowing you down. I suggest reducing the rate of your Serial dumps to 0.5s intervals to see if the situation improves.
EDIT:
I just noticed a few things that are wrong in your readMagData function.
Move these statements to the appropriate place in initAK8963:
//read mag
writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
delay(10);
writeByte(AK8963_ADDRESS, 0x0A, 0x01); //enable the magnetometer
delay(100);
because performing those actions on every data read will slow you down to below 10 Hz. They are configuration commands that are meant to be done once upon init of the sensor.
Reading the AK8963 data requires actually reading an extra register in order for the read to trigger. Change the 6 to a 7 in this line like so:
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);