MPU9250-SelfTest Fail

Below is the correct sequence to execute self test function.
/**
* @brief Trigger gyro/accel/compass self-test.
* On success/error, the self-test returns a mask representing the sensor(s)
* that failed. For each bit, a one (1) represents a "pass" case; conversely,
* a zero (0) indicates a failure.
*
* n The mask is defined as follows:
* n Bit 0: Gyro.
* n Bit 1: Accel.
* n Bit 2: Compass.
*
* n Currently, the hardware self-test is unsupported for MPU6500. However,
* this function can still be used to obtain the accel and gyro biases.
*
* n This function must be called with the device either face-up or face-down
* (z-axis is parallel to gravity).
* @param[out] gyro Gyro biases in q16 format.
* @param[out] accel Accel biases (if applicable) in q16 format.
* @return Result mask (see above).
*/
int mpu_run_self_test(long *gyro, long *accel)
{
#ifdef MPU6050
const unsigned char tries = 2;
long gyro_st[3], accel_st[3];
unsigned char accel_result, gyro_result;
#ifdef AK89xx_SECONDARY
unsigned char compass_result;
#endif
int ii;
#endif
int result;
unsigned char accel_fsr, fifo_sensors, sensors_on;
unsigned short gyro_fsr, sample_rate, lpf;
unsigned char dmp_was_on;

if (st.chip_cfg.dmp_on) {
mpu_set_dmp_state(0);
dmp_was_on = 1;
} else
dmp_was_on = 0;

/* Get initial settings. */
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_lpf(&lpf);
mpu_get_sample_rate(&sample_rate);
sensors_on = st.chip_cfg.sensors;
mpu_get_fifo_config(&fifo_sensors);

/* For older chips, the self-test will be different. */
#if defined MPU6050
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro, accel, 0))
break;
if (ii == tries) {
/* If we reach this point, we most likely encountered an I2C error.
* We'll just report an error for all three sensors.
*/
result = 0;
goto restore;
}
for (ii = 0; ii < tries; ii++)
if (!get_st_biases(gyro_st, accel_st, 1))
break;
if (ii == tries) {
/* Again, probably an I2C error. */
result = 0;
goto restore;
}
accel_result = accel_self_test(accel, accel_st);
gyro_result = gyro_self_test(gyro, gyro_st);

result = 0;
if (!gyro_result)
result |= 0x01;
if (!accel_result)
result |= 0x02;

#ifdef AK89xx_SECONDARY
compass_result = compass_self_test();
if (!compass_result)
result |= 0x04;
#endif
restore:
#elif defined MPU6500
/* For now, this function will return a "pass" result for all three sensors
* for compatibility with current test applications.
*/
get_st_biases(gyro, accel, 0);
result = 0x7;
#endif
/* Set to invalid values to ensure no I2C writes are skipped. */
st.chip_cfg.gyro_fsr = 0xFF;
st.chip_cfg.accel_fsr = 0xFF;
st.chip_cfg.lpf = 0xFF;
st.chip_cfg.sample_rate = 0xFFFF;
st.chip_cfg.sensors = 0xFF;
st.chip_cfg.fifo_enable = 0xFF;
st.chip_cfg.clk_src = INV_CLK_PLL;
mpu_set_gyro_fsr(gyro_fsr);
mpu_set_accel_fsr(accel_fsr);
mpu_set_lpf(lpf);
mpu_set_sample_rate(sample_rate);
mpu_set_sensors(sensors_on);
mpu_configure_fifo(fifo_sensors);

if (dmp_was_on)
mpu_set_dmp_state(1);

return result;
}