Device doesn't seems to be working properly.

[sverdlovsk]

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.

[kriswiner]

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.

[sverdlovsk]

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.

[kriswiner]

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.

[sverdlovsk]

''' 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.

[kriswiner]

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.

[sverdlovsk]

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..

[kriswiner]

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.

[sverdlovsk]

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 :-1:?

[kriswiner]

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.

[sverdlovsk]

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? :-)

[kriswiner]

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.

[sverdlovsk]

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...

[robotage]

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 :)

[kriswiner]

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

[robotage]

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

[kriswiner]

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.

[sverdlovsk]

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...

[kriswiner]

Great! It's always the little things. But how were you able to read other sensors using the same I2C read/write routines???

[sverdlovsk]

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?

[kriswiner]

I'm not sure what I am looking at. Are you able to verify that the register contents you write can be read back?

[sverdlovsk]

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?

[sverdlovsk]

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;

[kriswiner]

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.

[sverdlovsk]

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.

[sverdlovsk]

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

[sverdlovsk]

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.

[kriswiner]

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.

[sverdlovsk]

1. This device requires STOP to finish transaction.
2. 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.

[kriswiner]

I hope your experience with the LSM9DS1 is more satisfying. Please let me know what I can do to help!

[sverdlovsk]

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.

[kriswiner]

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

[sverdlovsk]

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!

[kriswiner]

I think what you need can be found here, no?

https://github.com/kriswiner/MPU-9250

[sverdlovsk]

This device has stopped talking to me :-(

[kriswiner]

Are you sue it is the device and not the microcontroller that is causing these troubles?

[sverdlovsk]

Kris, that is exactly why I put this screenshot! Master is clocking, device has to ACK, but it isn't...

[kriswiner]

What's different from yesterday when all seemed to be working. Maybe you DO have a bum MPU9250!

[sverdlovsk]

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...

[kriswiner]

Looks like you are only waiting 10 ms for the PLL to get established in this init function, maybe 100 would be better.

[sverdlovsk]

Well, and even if you're correct then why this POS stops ACKing? BTW POS stands for Piece Of Silicon...