Comments (40)
kriswiner
commented on July 28, 2024
I have this board also, and it has worked well for me. You need to make sure that both VDD and VDDIO are powered, and that you properly configure the chip select (it must be HIGH for I2C IIRC). There are 10K pullups on the board for I2C; just make sure there are no other pullups in the circuit. What micrcontroller are you using>? If you can read the whoami register then you probably have the hardware set up properly. You have to power up the sensor (set sleep bit in 0x6B to 0) and enable bypass mode( set register 0x37 to 0x22) if you want to read the magnetometer. My code (https://github.com/kriswiner/MPU-9250/blob/master/MPU9250BasicAHRS.ino) on GitHub does all this and should work with this breakout board.
Kris
From: sverdlovsk
Sent: Friday, November 28, 2014 8:13 AM
To: kriswiner/MPU-9250
Subject: [MPU-9250] Device doesn't seems to be working properly. (#5)
I have this board: http://www.embeddedmasters.com/PortalProductDetail.aspx?ProdId=552444#.VHidcTHF-os
So far I have made attempts to initialize based on my personal understanding of the datasheet. As the result I'm successfully reading WhoAmI register and accelerometr output registers. The issue that AX, AY an AZ do not change at all except after reset...Reading configuration registers back doesn't make sense - all 0.
The second attempt was to use other initialization routines found on the web. Basically the same result.
The third was porting this code. No luck.
Any suggestions?
Thanks.
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Thank you for fast reply.
Pull ups was the first issue: the device didn't ACK, so they are removed.The schematic for this board is at least confusing and no designation on the board itself. VDD pins on one side connector are joined connected to 3.3V. The GND pin goes to ground and SDA and SCL from other side connector are going to my board. For this exercise I'm using STM32F4DISCOVERY. The reason I've mentioned my ability to read WhoAmI and you've noticed that, that my communication is working...
Here is the ported version of your code:
// Accelerometer and gyroscope self test; check calibration wrt factory settings
bool MPU9250SelfTest (float * destination) { // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
uint8_t data[6] = {0, 0, 0, 0, 0, 0}, val;
uint8_t selfTest[6];
int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
float factoryTrim[6];
uint8_t FS = 0;
i2c_err_e *err;
// Set gyro sample rate to 1 kHz
val = 0;
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
// Set gyro sample rate to 1 kHz and DLPF to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
// Set full scale range for the gyro to 250 dps
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
// Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(val), &val, err)) return false;
// Set full scale range for the accelerometer to 2 g
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
// Configure the accelerometer for self-test
// Enable self test on all three axes and set accelerometer range to +/- 2 g
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Configure the gyro and accelerometer for normal operation
val = 0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_ACCEL, sizeof(val), &val, err)) return false;
selfTest[0] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_ACCEL, sizeof(val), &val, err)) return false;
selfTest[1] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_ACCEL, sizeof(val), &val, err)) return false;
selfTest[2] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_GYRO, sizeof(val), &val, err)) return false;
selfTest[3] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_GYRO, sizeof(val), &val, err)) return false;
selfTest[4] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_GYRO, sizeof(val), &val, err)) return false;
selfTest[5] = val; // X-axis accel self-test results
// Retrieve factory self-test value from self-test code reads
factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
// 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 ii = 0; ii < 3; ii++) {
destination[ii] = 100.0*((float)(aSTAvg[ii] - aAvg[ii]))/factoryTrim[ii]; // Report percent differences
destination[ii+3] = 100.0*((float)(gSTAvg[ii] - gAvg[ii]))/factoryTrim[ii+3]; // Report percent differences
}
return true;
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
bool calibrateMPU9250 (float * dest1, float * dest2) {
uint8_t val, data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
i2c_err_e *err;
// reset device, reset all registers, clear gyro and accelerometer bias registers
val = 0x80; // Write a one to bit 7 reset bit; toggle reset device
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
val = 0x1;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_2, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure device for bias calculation
val = 0; // Disable all interrupts
if (!mpu9250_wr_reg(MPU9250_INT_ENABLE, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
val = 0; // Turn on internal clock source
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0; // Disable I2C master
if (!mpu9250_wr_reg(MPU9250_I2C_MST_CTRL, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO and I2C master modes
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0xC; // Reset FIFO and DMP
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure MPU9250 gyro and accelerometer for bias calculation
val = 1; // Set low-pass filter to 188 Hz
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set sample rate to 1 kHz
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
val = 0; // Set gyro full-scale to 250 degrees per second, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set accelerometer full-scale to 2 g, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Configure FIFO to capture accelerometer and gyro data for bias calculation
val = 0x40; // Enable FIFO
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0x78; // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(80); // accumulate 40 samples in 80 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
val = 0; // Disable gyro and accelerometer sensors for FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
// read FIFO sample count
if (!mpu9250_rd_reg(MPU9250_FIFO_COUNTH, sizeof(uint16_t), data, err)) return false;
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count/12; // How many sets of full gyro and accelerometer data for averaging
if (packet_count) {
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
// read data for averaging
if (!mpu9250_rd_reg(MPU9250_FIFO_R_W, sizeof(data), data, err)) return false;
accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3] );
accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5] );
gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7] );
gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9] );
gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[0] += (int32_t)accel_temp[0];
accel_bias[1] += (int32_t)accel_temp[1];
accel_bias[2] += (int32_t)accel_temp[2];
gyro_bias[0] += (int32_t)gyro_temp[0];
gyro_bias[1] += (int32_t)gyro_temp[1];
gyro_bias[2] += (int32_t)gyro_temp[2];
}
// Normalize sums to get average count biases
accel_bias[0] /= (int32_t)packet_count;
accel_bias[1] /= (int32_t)packet_count;
accel_bias[2] /= (int32_t)packet_count;
gyro_bias[0] /= (int32_t)packet_count;
gyro_bias[1] /= (int32_t)packet_count;
gyro_bias[2] /= (int32_t)packet_count;
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;
// Push gyro biases to hardware registers
/*
val = data[0];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[1];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[2];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[3];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[4];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[5];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_L, sizeof(val), val, err)) return false;
*/
// construct gyro bias in deg/s for later manual subtraction
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
}
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
// Read factory accelerometer trim values
if (!mpu9250_rd_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
// If temperature compensation bit is set, record that fact in mask_bit
for (ii = 0; ii < 3; ii++) if (accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01;
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*
if (!mpu9250_wr_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), &data[0], err)) return false;
if (!mpu9250_wr_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), &data[2], err)) return false;
if (!mpu9250_wr_reg(MPU9250_ZA_OFFSET_H, sizeof(uint16_t), &data[4], err)) return false;
*/
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
return true;
}
The first function reads all zeros, the second reads packet_count as 0...
What else? Bad device?
Thank you.
from mpu9250.
kriswiner
commented on July 28, 2024
I assume you also have VDDIO connected to 3V3.
I don’t see in this ported code where you are clearing the sleep bit in the 0x6B register. The device is default asleep and must be turned on.
If you just write 0x00 to register 0x6b, you should be able to get non-zero accel data. Try this.
Kris
From: sverdlovsk
Sent: Friday, November 28, 2014 10:58 AM
To: kriswiner/MPU-9250
Cc: Kris Winer
Subject: Re: [MPU-9250] Device doesn't seems to be working properly. (#5)
Thank you for fast reply.
Pull ups was the first issue: the device didn't ACK, so they are removed.The schematic for this board is at least confusing and no designation on the board itself. VDD pins on one side connector are joined connected to 3.3V. The GND pin goes to ground and SDA and SCL from other side connector are going to my board. For this exercise I'm using STM32F4DISCOVERY. The reason I've mentioned my ability to read WhoAmI and you've noticed that, that my communication is working...
Here is the ported version of your code:
// Accelerometer and gyroscope self test; check calibration wrt factory settings
bool MPU9250SelfTest (float * destination) { // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
uint8_t data[6] = {0, 0, 0, 0, 0, 0}, val;
uint8_t selfTest[6];
int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
float factoryTrim[6];
uint8_t FS = 0;
i2c_err_e *err;
// Set gyro sample rate to 1 kHz
val = 0;
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
// Set gyro sample rate to 1 kHz and DLPF to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
// Set full scale range for the gyro to 250 dps
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
// Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(val), &val, err)) return false;
// Set full scale range for the accelerometer to 2 g
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
// Configure the accelerometer for self-test
// Enable self test on all three axes and set accelerometer range to +/- 2 g
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Configure the gyro and accelerometer for normal operation
val = 0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_ACCEL, sizeof(val), &val, err)) return false;
selfTest[0] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_ACCEL, sizeof(val), &val, err)) return false;
selfTest[1] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_ACCEL, sizeof(val), &val, err)) return false;
selfTest[2] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_GYRO, sizeof(val), &val, err)) return false;
selfTest[3] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_GYRO, sizeof(val), &val, err)) return false;
selfTest[4] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_GYRO, sizeof(val), &val, err)) return false;
selfTest[5] = val; // X-axis accel self-test results
// Retrieve factory self-test value from self-test code reads
factoryTrim[0] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
factoryTrim[1] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
factoryTrim[2] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
factoryTrim[3] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = (float)(2620/1<<FS)(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
// 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 ii = 0; ii < 3; ii++) {
destination[ii] = 100.0_((float)(aSTAvg[ii] - aAvg[ii]))/factoryTrim[ii]; // Report percent differences
destination[ii+3] = 100.0_((float)(gSTAvg[ii] - gAvg[ii]))/factoryTrim[ii+3]; // Report percent differences
}
return true;
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
bool calibrateMPU9250 (float * dest1, float * dest2) {
uint8_t val, data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
i2c_err_e *err;
// reset device, reset all registers, clear gyro and accelerometer bias registers
val = 0x80; // Write a one to bit 7 reset bit; toggle reset device
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
val = 0x1;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_2, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure device for bias calculation
val = 0; // Disable all interrupts
if (!mpu9250_wr_reg(MPU9250_INT_ENABLE, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
val = 0; // Turn on internal clock source
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0; // Disable I2C master
if (!mpu9250_wr_reg(MPU9250_I2C_MST_CTRL, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO and I2C master modes
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0xC; // Reset FIFO and DMP
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure MPU9250 gyro and accelerometer for bias calculation
val = 1; // Set low-pass filter to 188 Hz
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set sample rate to 1 kHz
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
val = 0; // Set gyro full-scale to 250 degrees per second, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set accelerometer full-scale to 2 g, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Configure FIFO to capture accelerometer and gyro data for bias calculation
val = 0x40; // Enable FIFO
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0x78; // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(80); // accumulate 40 samples in 80 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
val = 0; // Disable gyro and accelerometer sensors for FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
// read FIFO sample count
if (!mpu9250_rd_reg(MPU9250_FIFO_COUNTH, sizeof(uint16_t), data, err)) return false;
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count/12; // How many sets of full gyro and accelerometer data for averaging
if (packet_count) {
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
// read data for averaging
if (!mpu9250_rd_reg(MPU9250_FIFO_R_W, sizeof(data), data, err)) return false;
accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3] );
accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5] );
gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7] );
gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9] );
gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[0] += (int32_t)accel_temp[0];
accel_bias[1] += (int32_t)accel_temp[1];
accel_bias[2] += (int32_t)accel_temp[2];
gyro_bias[0] += (int32_t)gyro_temp[0];
gyro_bias[1] += (int32_t)gyro_temp[1];
gyro_bias[2] += (int32_t)gyro_temp[2];
}
// Normalize sums to get average count biases
accel_bias[0] /= (int32_t)packet_count;
accel_bias[1] /= (int32_t)packet_count;
accel_bias[2] /= (int32_t)packet_count;
gyro_bias[0] /= (int32_t)packet_count;
gyro_bias[1] /= (int32_t)packet_count;
gyro_bias[2] /= (int32_t)packet_count;
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;
// Push gyro biases to hardware registers
/*
val = data[0];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[1];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[2];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[3];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[4];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[5];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_L, sizeof(val), val, err)) return false;
*/
// construct gyro bias in deg/s for later manual subtraction
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
}
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
// Read factory accelerometer trim values
if (!mpu9250_rd_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
// If temperature compensation bit is set, record that fact in mask_bit
for (ii = 0; ii < 3; ii++) if (accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01;
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*
if (!mpu9250_wr_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), &data[0], err)) return false;
if (!mpu9250_wr_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), &data[2], err)) return false;
if (!mpu9250_wr_reg(MPU9250_ZA_OFFSET_H, sizeof(uint16_t), &data[4], err)) return false;
*/
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
return true;
}
The first function reads all zeros, the second reads packet_count as 0...
What else? Bad device?
Thank you.
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
'''
void be2le_uint16 (uint8_t *ptr) {
uint8_t tmp;
tmp = ptr[0];
ptr[0] = ptr[1];
ptr[1] = tmp;
}
bool read_data (uint8_t *buf, i2c_err_e *err) {
mpu9250_rd_reg (MPU9250_ACCEL_XOUT_H, 6, buf, err);
be2le_uint16 (buf);
be2le_uint16 (&buf[2]);
be2le_uint16 (&buf[4]);
return true;
}
bool init_mpu9250 (i2c_err_e *err) {
uint32_t bob;
uint8_t tmp, data, len, buf[6];
uint8_t init1_msg[] =
{
(1<<MPU9250_PWR_MGMT_1_H_RESET),
};
uint8_t init2_msg[] = {
(1<<MPU9250_PWR_MGMT_1_CLKSEL),
};
uint8_t init3_msg[] = {
(1<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ),
};
uint8_t init4_msg[] = {
199,
};
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init1_msg), init1_msg, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source :Auto selects the best available clock source – PLL if ready,
// else use the Internal oscillator
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init2_msg), init2_msg, err)) return false;
// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 5 Hz, respectively;
// DLPF_CFG = bits 2:0 = 110;
if (!mpu9250_wr_reg( MPU9250_CONFIG, sizeof(init3_msg), init3_msg, err)) return false;
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
// Use a 5 Hz rate; the same rate set in CONFIG above
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(init4_msg), init4_msg, err)) return false;
// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
#if 1
if (!mpu9250_rd_reg(MPU9250_GYRO_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_GYRO_CONFIG_XGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_YGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_ZGYRO_CTEN));
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_GYRO_CONFIG_GYRO_FS_SEL_CLR);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Set full scale range for the accelerometer
data |= (MPU9250_GYRO_CONFIG_GYRO_FS_SEL_250DPS<<MPU9250_GYRO_CONFIG_GYRO_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err);
#endif
// Set accelerometer configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_ACCEL_CONFIG_AX_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AY_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AZ_ST_EN));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_ACCEL_CONFIG_FS_SEL_CLR);
// Set full scale range for the accelerometer
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_FS_SEL_2G<<MPU9250_ACCEL_CONFIG_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#endif
// Set accelerometer sample rate configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG_2, sizeof(data), &data, err)) return false;
tmp = data & ~((MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_CLR<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG)+(1<<MPU9250_ACCEL_CONFIG_2_A_FCHOICE_B));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG);
#else
tmp = (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG); // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
#endif
#if 0
// 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
#endif
return true;
}
'''
Didn't help much.
Yes, I'm reading no zero data but as before nothing changes with motion of the board.
from mpu9250.
kriswiner
commented on July 28, 2024
When I have such trouble I return to my trusty bus pirate. Then I can just check that I can read registers and that the values make sense.
In this case, I would suggest that you verify the status of the PWR_MGMT_1 (0x6B) register to make sure the device is powered up. This should be the only action required to start getting accel data.
In fact, I suggest you write a very simple program that does nothing but clear the sleep bit (write 0x00 to register 0x6B) and read accel data. This has to work if you can read the whoami register.
If it doesn’t you might have a bad board but this is very unlikely; embedded masters does very high quality work.
Kris
From: sverdlovsk
Sent: Friday, November 28, 2014 11:50 AM
To: kriswiner/MPU-9250
Cc: Kris Winer
Subject: Re: [MPU-9250] Device doesn't seems to be working properly. (#5)
'''
void be2le_uint16 (uint8_t *ptr) {
uint8_t tmp;
tmp = ptr[0];
ptr[0] = ptr[1];
ptr[1] = tmp;
}
bool read_data (uint8_t *buf, i2c_err_e *err) {
mpu9250_rd_reg (MPU9250_ACCEL_XOUT_H, 6, buf, err);
be2le_uint16 (buf);
be2le_uint16 (&buf[2]);
be2le_uint16 (&buf[4]);
return true;
}
bool init_mpu9250 (i2c_err_e *err) {
uint32_t bob;
uint8_t tmp, data, len, buf[6];
uint8_t init1_msg[] =
{
(1<<MPU9250_PWR_MGMT_1_H_RESET),
};
uint8_t init2_msg[] = {
(1<<MPU9250_PWR_MGMT_1_CLKSEL),
};
uint8_t init3_msg[] = {
(1<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ),
};
uint8_t init4_msg[] = {
199,
};
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init1_msg), init1_msg, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source :Auto selects the best available clock source – PLL if ready,
// else use the Internal oscillator
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init2_msg), init2_msg, err)) return false;
// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 5 Hz, respectively;
// DLPF_CFG = bits 2:0 = 110;
if (!mpu9250_wr_reg( MPU9250_CONFIG, sizeof(init3_msg), init3_msg, err)) return false;
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
// Use a 5 Hz rate; the same rate set in CONFIG above
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(init4_msg), init4_msg, err)) return false;
// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
#if 1
if (!mpu9250_rd_reg(MPU9250_GYRO_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_GYRO_CONFIG_XGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_YGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_ZGYRO_CTEN));
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_GYRO_CONFIG_GYRO_FS_SEL_CLR);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Set full scale range for the accelerometer
data |= (MPU9250_GYRO_CONFIG_GYRO_FS_SEL_250DPS<<MPU9250_GYRO_CONFIG_GYRO_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err);
#endif
// Set accelerometer configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_ACCEL_CONFIG_AX_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AY_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AZ_ST_EN));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_ACCEL_CONFIG_FS_SEL_CLR);
// Set full scale range for the accelerometer
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_FS_SEL_2G<<MPU9250_ACCEL_CONFIG_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#endif
// Set accelerometer sample rate configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG_2, sizeof(data), &data, err)) return false;
tmp = data & ~((MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_CLR<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG)+(1<<MPU9250_ACCEL_CONFIG_2_A_FCHOICE_B));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG);
#else
tmp = (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG); // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
#endif
#if 0
// 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
#endif
return true;
}
'''
Didn't help much.
Yes, I'm reading no zero data but as before nothing changes with motion of the board.
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
This should be sufficient to read accelerometer?
'''
bool simple_mpu9250 (i2c_err_e *err) {
uint8_t val;
val = 0x80;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
for (;;) {
if (!read_data(data_in, &i2c_err)) gpio_set(LED_RED_PORT,LED_RED_PIN);
else gpio_clr(LED_RED_PORT,LED_RED_PIN);
}
return true;
}
'''
Not workie..
from mpu9250.
kriswiner
commented on July 28, 2024
I don’t know exactly what your code is doing but if writing 0x00 to PWR_MGMT_1 doesn’t allow accel reads then try another board.
From: sverdlovsk
Sent: Friday, November 28, 2014 12:20 PM
To: kriswiner/MPU-9250
Cc: Kris Winer
Subject: Re: [MPU-9250] Device doesn't seems to be working properly. (#5)
This should be sufficient to read accelerometer?
'''
bool simple_mpu9250 (i2c_err_e *err) {
uint8_t val;
val = 0x80;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
for (;;) {
if (!read_data(data_in, &i2c_err)) gpio_set(LED_RED_PORT,LED_RED_PIN);
else gpio_clr(LED_RED_PORT,LED_RED_PIN);
}
return true;
}
'''
Not workie..
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Well, obviously I don't have another piece of hw ready...Here is another clue: it doesn't matter what I'm writing to MPU9250_PWR_MGMT_1 I'm always reading back 1. Also the same behavior is with ACCEL_CONFIG register and GYRO_CONFIG - the data I'm reading back are always 0!
May by chip is 👎?
from mpu9250.
kriswiner
commented on July 28, 2024
Sounds like your I2C write is not working properly. If you write 0x00 in PWR_MGMT_1 and you read back 0x40 or anything other than 0x00 either your write doesn’t work or the board/chip is ‘bad’. I think the probability is quite high it is the former.
Can you talk to and get data from any other I2C sensor with your hardware?
Kris
From: sverdlovsk
Sent: Friday, November 28, 2014 3:59 PM
To: kriswiner/MPU-9250
Cc: Kris Winer
Subject: Re: [MPU-9250] Device doesn't seems to be working properly. (#5)
Well, obviously I don't have another piece of hw ready...Here is another clue: it doesn't matter what I'm writing to MPU9250_PWR_MGMT_1 I'm always reading back 1. Also the same behavior is with ACCEL_CONFIG register and GYRO_CONFIG - the data I'm reading back are always 0!
May by chip is ?
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
I2C is fine. I'm talking with many other sensors and have a scope as an alternative judge! BTW otherwise how would I got ACKs all the way?
Monday will try not to forget to put screenshot from the scope writing 0 to PWR_MGMT_1. Will you bet your $.02? :-)
from mpu9250.
kriswiner
commented on July 28, 2024
OK, if I2C is fine why can’t you write to the MPU9250? If you write 0x00 to register 0x6B and you then read 0x00 in register 0x6B are you still getting accel data 0x00 or unchanging?
I am having trouble understanding this problem because with my EMSENSR MPU9250 and the thirty other MPU9250 boards I have built if I can read the WHOAMI register and write 0x00 to register 0x6B then I can read the accel/gyro data without doing anything else.
So I am a bit perplexed by your trouble and don’t know what else to suggest.
Sorry I can’t be of more help.
Kris
From: sverdlovsk
Sent: Friday, November 28, 2014 4:13 PM
To: kriswiner/MPU-9250
Cc: Kris Winer
Subject: Re: [MPU-9250] Device doesn't seems to be working properly. (#5)
I2C is fine. I'm talking with many other sensors and have a scope as an alternative judge! BTW otherwise how would I got ACKs all the way?
—
Reply to this email directly or view it on GitHub.
from mpu9250.
sverdlovsk
commented on July 28, 2024
If I knew the answer...My bet is that chip itself is not functioning properly. Whatever I'm writing to 0x6B I'm reading not 0 but 1 all the time and you're correct accel data are not 0 but do not change like one time read or something...
from mpu9250.
robotage
commented on July 28, 2024
Hello
kriswiner
have you tried Sebastian Madgwick's IMU fusion filter , Running on the STM32F407ZGT6 / 168 MHz CORTEX-M4 microcontroller ,
or have you tried it on raspberry pi,
do you think we can get more than 5000Hz :)
from mpu9250.
kriswiner
commented on July 28, 2024
Yes, I was getting about that with Madgwick on the STM32F401 Nucleo board. See:
https://github.com/kriswiner/MPU-6050/wiki/Affordable-9-DoF-Sensor-Fusion
from mpu9250.
robotage
commented on July 28, 2024
interesting :)
i don't have STM32F401 Nucleo board , how can i run it on STM32F407ZGT6 olimex board.
i send you an massage to mbed.org
from mpu9250.
kriswiner
commented on July 28, 2024
I think you can just compile the code that runs on mbed in the usual way. You might have to make a few adjustments for your Olimex board, but hooking up the sensor to I2C, running the code, and getting serial output are pretty standard.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Seems that I've solved the issue. My I2C wr/rd do NOT have stop condition - I don't want to release the bus. I just added stop in my wr/rd functions and data are changing now...
from mpu9250.
kriswiner
commented on July 28, 2024
Great! It's always the little things. But how were you able to read other sensors using the same I2C read/write routines???
from mpu9250.
sverdlovsk
commented on July 28, 2024
Well, it isn't over yet. The data don't make sense. Attached two screenshots writing and reading back reg 0x1C. Does anyone see anything wrong except that actually nothing were written?
from mpu9250.
kriswiner
commented on July 28, 2024
I'm not sure what I am looking at. Are you able to verify that the register contents you write can be read back?
from mpu9250.
sverdlovsk
commented on July 28, 2024
I'm writing to ACCEL_CFG(0x1C) - 8 and immediately reading the same register back - no change :-(
Have no idea what is wrong. I don't see anything wrong with with I2C, do you?
from mpu9250.
sverdlovsk
commented on July 28, 2024
Why I can't write to this( and probably any other) register?
bool init_mpu9250 (i2c_err_e *err) {
uint8_t val;
val = 0x80;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(10);
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
do {
val = 8; ms_delay(100);
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
} while (val != 8);
return true;
from mpu9250.
kriswiner
commented on July 28, 2024
I just want to make sure; you CAN read the WHOAMI register, right? But you CAN'T write to any register?
This is very strange...
I am assuming that this function:
mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)
has been tested and shown to work in other applications.
Can you scan for I2C addresses and verify that you can detect the MPU9250. This sounds like a basic I2C address error problem.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Yes, I'm reading WhoAmI with no trouble.
I'm ASSUMING that I can't write to any register. This code is the test to prove the point.
The functions mpu9250_rd_reg(or wr_reg) are using i2c routine, tested in other applications. WhoAmI is read by the same routine as well.
I currently have only mpu9250 on the i2c bus.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Well, at this point I'm finally able to write to the registers. This device does NOT support repeated start condition in write mode &^%$#@! I don't understand why didn't they describe all noncompliance with I2C standard?! Still accelerometer data don't make sense. Will continue this fight tomorrow
from mpu9250.
sverdlovsk
commented on July 28, 2024
If finally works...
This device is I2C compatible and NOT I2C compliant!!! Be aware of this fact, the datasheet doesn't say a word about this fact.
from mpu9250.
kriswiner
commented on July 28, 2024
So was the problem in the end the repeated start not being supported by the MPU9250 ASIC?
I don't use this feature of I2C so have never had this problem.
from mpu9250.
sverdlovsk
commented on July 28, 2024
- This device requires STOP to finish transaction.
- Repeated start is a necessary part of reading but is NOT supported at writing...
Once again: they should say that MPU-9250 is not I2C compliant but the only clue I found was the picture on the bottom of the page 34 of Product specification document. But this is just clue, not the statement.
Not sure what other discoveries to expect...
Ordered LSM9DS1 breakout board.
from mpu9250.
kriswiner
commented on July 28, 2024
I hope your experience with the LSM9DS1 is more satisfying. Please let me know what I can do to help!
from mpu9250.
sverdlovsk
commented on July 28, 2024
No matter what I'm really appreciate your help. Hope my pain will save somebody else from suffering with bad documentation.
My "noise" test is calculating sqrt(ax_ax + ay_ay + az*az) and see how is this value changing when I'm playing with the accelerometer. Doesn't look very good. Will try to tune configuration.
Do you have code for converting temperature from raw hex to C? It is noisy but I can't estimate the value yet.
from mpu9250.
kriswiner
commented on July 28, 2024
You might want to 'play' with the bandwidth and ODR to reduce some of the noise on the accel. I usually use 200 Hz ODR and 41 Hz bandwidth.
For the MPU9250 gyro temperature:
tempCount = readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade
// Print temperature in degrees Centigrade
Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of a degree C
from mpu9250.
sverdlovsk
commented on July 28, 2024
Thanks again.
Could you please publish complete config(ini) file including the magnetometer like { uint8_t reg, uint8_t val;} I think that that would be great!
from mpu9250.
kriswiner
commented on July 28, 2024
I think what you need can be found here, no?
https://github.com/kriswiner/MPU-9250
from mpu9250.
sverdlovsk
commented on July 28, 2024
This device has stopped talking to me :-(
from mpu9250.
kriswiner
commented on July 28, 2024
Are you sue it is the device and not the microcontroller that is causing these troubles?
from mpu9250.
sverdlovsk
commented on July 28, 2024
Kris, that is exactly why I put this screenshot! Master is clocking, device has to ACK, but it isn't...
from mpu9250.
kriswiner
commented on July 28, 2024
What's different from yesterday when all seemed to be working. Maybe you DO have a bum MPU9250!
from mpu9250.
sverdlovsk
commented on July 28, 2024
OK,
One of those two functions is making this device comatoze. Power reset for few seconds returns it to life. Those actually 3 functions is the difference.
Publishing it here for curious people
bool init_mpu9250 (i2c_err_e *err) {
#if 1
uint8_t val;
val = 0x80;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(10);
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
// ms_delay(10);
do {
val = 8; ms_delay(100);
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// if (!mpu9250_rd_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
} while (val != 8);
#else
uint8_t tmp, data, len, buf[6];
uint8_t init1_msg[] =
{
(1<<MPU9250_PWR_MGMT_1_H_RESET),
};
uint8_t init2_msg[] = {
(1<<MPU9250_PWR_MGMT_1_CLKSEL),
};
uint8_t init3_msg[] = {
(1<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ),
};
uint8_t init4_msg[] = {
199,
};
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init1_msg), init1_msg, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source :Auto selects the best available clock source – PLL if ready,
// else use the Internal oscillator
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(init2_msg), init2_msg, err)) return false;
// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 5 Hz, respectively;
// DLPF_CFG = bits 2:0 = 110;
if (!mpu9250_wr_reg( MPU9250_CONFIG, sizeof(init3_msg), init3_msg, err)) return false;
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
// Use a 5 Hz rate; the same rate set in CONFIG above
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(init4_msg), init4_msg, err)) return false;
// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
#if 1
if (!mpu9250_rd_reg(MPU9250_GYRO_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_GYRO_CONFIG_XGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_YGYRO_CTEN)+(1<<MPU9250_GYRO_CONFIG_ZGYRO_CTEN));
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_GYRO_CONFIG_GYRO_FS_SEL_CLR);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Set full scale range for the accelerometer
data |= (MPU9250_GYRO_CONFIG_GYRO_FS_SEL_250DPS<<MPU9250_GYRO_CONFIG_GYRO_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(tmp), &tmp, err);
#endif
// Set accelerometer configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG, sizeof(data), &data, err)) return false;
// Clear self-test bits [7:5]
tmp = data & ~((1<<MPU9250_ACCEL_CONFIG_AX_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AY_ST_EN)+(1<<MPU9250_ACCEL_CONFIG_AZ_ST_EN));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
// Clear AFS bits [4:3]
tmp = data & ~(MPU9250_ACCEL_CONFIG_FS_SEL_CLR);
// Set full scale range for the accelerometer
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_FS_SEL_2G<<MPU9250_ACCEL_CONFIG_FS_SEL);
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#else
tmp = 0; // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(tmp), &tmp, err)) return false;
#endif
// Set accelerometer sample rate configuration
#if 1
if (!mpu9250_rd_reg(MPU9250_ACCEL_CONFIG_2, sizeof(data), &data, err)) return false;
tmp = data & ~((MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_CLR<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG)+(1<<MPU9250_ACCEL_CONFIG_2_A_FCHOICE_B));
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
data |= (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG);
#else
tmp = (MPU9250_ACCEL_CONFIG_2_A_DLPFCFG_5HZ<<MPU9250_ACCEL_CONFIG_2_A_DLPFCFG); // should be the same as above
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(tmp), &tmp, err)) return false;
#endif
#if 0
// 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
#endif
#endif
return true;
}
void be2le_uint16 (uint8_t *ptr) {
uint8_t tmp;
tmp = ptr[0];
ptr[0] = ptr[1];
ptr[1] = tmp;
}
bool read_data (uint8_t *buf, i2c_err_e *err) {
mpu9250_rd_reg (MPU9250_ACCEL_XOUT_H, 14, buf, err);
be2le_uint16 (buf);
be2le_uint16 (&buf[2]);
be2le_uint16 (&buf[4]);
be2le_uint16 (&buf[6]);
be2le_uint16 (&buf[8]);
be2le_uint16 (&buf[10]);
be2le_uint16 (&buf[12]);
return true;
}
// Accelerometer and gyroscope self test; check calibration wrt factory settings
bool MPU9250SelfTest (float * destination) { // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
uint8_t data[6] = {0, 0, 0, 0, 0, 0}, val;
uint8_t selfTest[6];
int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
float factoryTrim[6];
uint8_t FS = 0;
i2c_err_e *err;
// Set gyro sample rate to 1 kHz
val = 0;
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
// Set gyro sample rate to 1 kHz and DLPF to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
// Set full scale range for the gyro to 250 dps
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
// Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
val = 2;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG_2, sizeof(val), &val, err)) return false;
// Set full scale range for the accelerometer to 2 g
val = 1<<FS;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average current readings
aAvg[ii] /= 200;
gAvg[ii] /= 200;
}
// Configure the accelerometer for self-test
// Enable self test on all three axes and set accelerometer range to +/- 2 g
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0xE0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
for (int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_ACCEL_XOUT_H, sizeof(data), data, err)) return false;
aSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
aSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
aSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
// Read the six raw data registers into data array
if (!mpu9250_rd_reg(MPU9250_GYRO_XOUT_H, sizeof(data), data, err)) return false;
gSTAvg[0] += (int16_t)(((int16_t)data[0] << 8) | data[1]) ; // Turn the MSB and LSB into a signed 16-bit value
gSTAvg[1] += (int16_t)(((int16_t)data[2] << 8) | data[3]) ;
gSTAvg[2] += (int16_t)(((int16_t)data[4] << 8) | data[5]) ;
}
for (int ii = 0; ii < 3; ii++) { // Get average of 200 values on each axis and store as average self-test readings
aSTAvg[ii] /= 200;
gSTAvg[ii] /= 200;
}
// Configure the gyro and accelerometer for normal operation
val = 0;
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Enable self test on all three axes and set gyro range to +/- 250 degrees/s
val = 0;
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
ms_delay(25); // Delay 25ms a while to let the device stabilize
// Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_ACCEL, sizeof(val), &val, err)) return false;
selfTest[0] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_ACCEL, sizeof(val), &val, err)) return false;
selfTest[1] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_ACCEL, sizeof(val), &val, err)) return false;
selfTest[2] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_X_GYRO, sizeof(val), &val, err)) return false;
selfTest[3] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Y_GYRO, sizeof(val), &val, err)) return false;
selfTest[4] = val; // X-axis accel self-test results
if (!mpu9250_rd_reg(MPU9250_SELF_TEST_Z_GYRO, sizeof(val), &val, err)) return false;
selfTest[5] = val; // X-axis accel self-test results
// Retrieve factory self-test value from self-test code reads
factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
// 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 ii = 0; ii < 3; ii++) {
destination[ii] = 100.0*((float)(aSTAvg[ii] - aAvg[ii]))/factoryTrim[ii]; // Report percent differences
destination[ii+3] = 100.0*((float)(gSTAvg[ii] - gAvg[ii]))/factoryTrim[ii+3]; // Report percent differences
}
return true;
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
bool calibrateMPU9250 (float * dest1, float * dest2) {
uint8_t val, data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g
i2c_err_e *err;
// reset device, reset all registers, clear gyro and accelerometer bias registers
val = 0x80; // Write a one to bit 7 reset bit; toggle reset device
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
// Delay 100 ms for PLL to get established on x-axis gyro;
ms_delay(100);
// get stable time source
// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
val = 0x1;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0;
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_2, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure device for bias calculation
val = 0; // Disable all interrupts
if (!mpu9250_wr_reg(MPU9250_INT_ENABLE, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
val = 0; // Turn on internal clock source
if (!mpu9250_wr_reg(MPU9250_PWR_MGMT_1, sizeof(val), &val, err)) return false;
val = 0; // Disable I2C master
if (!mpu9250_wr_reg(MPU9250_I2C_MST_CTRL, sizeof(val), &val, err)) return false;
val = 0; // Disable FIFO and I2C master modes
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0xC; // Reset FIFO and DMP
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(100);
// Configure MPU9250 gyro and accelerometer for bias calculation
val = 1; // Set low-pass filter to 188 Hz
if (!mpu9250_wr_reg(MPU9250_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set sample rate to 1 kHz
if (!mpu9250_wr_reg(MPU9250_SMPLRT_DIV, sizeof(val), &val, err)) return false;
val = 0; // Set gyro full-scale to 250 degrees per second, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_GYRO_CONFIG, sizeof(val), &val, err)) return false;
val = 0; // Set accelerometer full-scale to 2 g, maximum sensitivity
if (!mpu9250_wr_reg(MPU9250_ACCEL_CONFIG, sizeof(val), &val, err)) return false;
// Configure FIFO to capture accelerometer and gyro data for bias calculation
val = 0x40; // Enable FIFO
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
val = 0x78; // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
if (!mpu9250_wr_reg(MPU9250_USER_CTRL, sizeof(val), &val, err)) return false;
ms_delay(80); // accumulate 40 samples in 80 milliseconds = 480 bytes
// At end of sample accumulation, turn off FIFO sensor read
val = 0; // Disable gyro and accelerometer sensors for FIFO
if (!mpu9250_wr_reg(MPU9250_FIFO_EN, sizeof(val), &val, err)) return false;
// read FIFO sample count
if (!mpu9250_rd_reg(MPU9250_FIFO_COUNTH, sizeof(uint16_t), data, err)) return false;
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count/12; // How many sets of full gyro and accelerometer data for averaging
if (packet_count) {
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
// read data for averaging
if (!mpu9250_rd_reg(MPU9250_FIFO_R_W, sizeof(data), data, err)) return false;
accel_temp[0] = (int16_t)(((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t)(((int16_t)data[2] << 8) | data[3] );
accel_temp[2] = (int16_t)(((int16_t)data[4] << 8) | data[5] );
gyro_temp[0] = (int16_t)(((int16_t)data[6] << 8) | data[7] );
gyro_temp[1] = (int16_t)(((int16_t)data[8] << 8) | data[9] );
gyro_temp[2] = (int16_t)(((int16_t)data[10] << 8) | data[11]);
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[0] += (int32_t)accel_temp[0];
accel_bias[1] += (int32_t)accel_temp[1];
accel_bias[2] += (int32_t)accel_temp[2];
gyro_bias[0] += (int32_t)gyro_temp[0];
gyro_bias[1] += (int32_t)gyro_temp[1];
gyro_bias[2] += (int32_t)gyro_temp[2];
}
// Normalize sums to get average count biases
accel_bias[0] /= (int32_t)packet_count;
accel_bias[1] /= (int32_t)packet_count;
accel_bias[2] /= (int32_t)packet_count;
gyro_bias[0] /= (int32_t)packet_count;
gyro_bias[1] /= (int32_t)packet_count;
gyro_bias[2] /= (int32_t)packet_count;
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;
// Push gyro biases to hardware registers
/*
val = data[0];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[1];
if (!mpu9250_wr_reg(MPU9250_XG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[2];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[3];
if (!mpu9250_wr_reg(MPU9250_YG_OFFSET_L, sizeof(val), val, err)) return false;
val = data[4];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_H, sizeof(val), val, err)) return false;
val = data[5];
if (!mpu9250_wr_reg(MPU9250_ZG_OFFSET_L, sizeof(val), val, err)) return false;
*/
// construct gyro bias in deg/s for later manual subtraction
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
}
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
// Read factory accelerometer trim values
if (!mpu9250_rd_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
if (!mpu9250_rd_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), data, err)) return false;
accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
// If temperature compensation bit is set, record that fact in mask_bit
for (ii = 0; ii < 3; ii++) if (accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01;
// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
/*
if (!mpu9250_wr_reg(MPU9250_XA_OFFSET_H, sizeof(uint16_t), &data[0], err)) return false;
if (!mpu9250_wr_reg(MPU9250_YA_OFFSET_H, sizeof(uint16_t), &data[2], err)) return false;
if (!mpu9250_wr_reg(MPU9250_ZA_OFFSET_H, sizeof(uint16_t), &data[4], err)) return false;
*/
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
return true;
}
In the init function compiling condition should be changed...
from mpu9250.
kriswiner
commented on July 28, 2024
Looks like you are only waiting 10 ms for the PLL to get established in this init function, maybe 100 would be better.
from mpu9250.
sverdlovsk
commented on July 28, 2024
Well, and even if you're correct then why this POS stops ACKing?
BTW POS stands for Piece Of Silicon...
from mpu9250.
Related Issues (20)
- 5 different MPU9250 and not a single one 100% working (broken axis) HOT 5
- Pitch range only from -90 to 90 HOT 1
- How to increase data read rate?
- Reading ACCEL and Gyro Registers are wrong? HOT 6
- MPU9250 custom board didn‘t work HOT 4
- Alternative to MPU9250 and BNO055 HOT 9
- How can I get Ak8963 Address from MPU9250 HOT 1
- MPU9250 magnetometer not accessible by any read HOT 4
- The Magnetometer adress doesn't show up at MPU9250 with RPI3 HOT 3
- Azimuth HOT 10
- Computing Beta and Zeta Values HOT 9
- 9250 Who am I 0x75 ? HOT 5
- Sanity Check of mpu9250-derived values? Am I in the right units? HOT 19
- MPU9250 interrupt pin not working HOT 7
- Yaw values are not precise HOT 9
- Can such sensor be used for indoors inertial navigation ? HOT 1
- Calibration - figure of eight HOT 2
- Reset FIFO and DMP HOT 5
- Sensitivity Adjustment HOT 3
- Limitations/Considerations when 3D-tracking a fast rotating object for 1 second HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from mpu9250.