Drift problem MPU6000

Hello,

Can anyone help me with this drift issue on MPU6000? Here is the description:

Platform used: AVR32
Sensor used: MPU-6000
Software version: Embedded MotionDriver 5.1.2 (we also tried the older versions 5.11 and even the Embedded MotionApps v2.02)
Background: The MPU-6000 is integrated on the mainboard of of of our video monitor. We use this sensor to get 6-axis quaternion data from the DMP and then calculate the gravity data from the quaternion. Finally, we calculate the the inclination angle from gravity data. The configuration codes for the sensors and the calculation method of gravity data will be attached in "Sensor configuration codes". We also calculate the inclination angle from acceleration data for comparison.
Here we suppose that the inclination angle calculated from 6-axis gravity data is theta_grav and that calculated from acceleration is theta_accel.
Problem description:
Step 1: When I start the monitor (the MPU-6000 is started with the monitor) at a normal room temperature, that means under 30°C, there's no difference between theta_grav and theta_accel and the sensor works well. In this case, I can restart the monitor without any error between the theta_grav and theta_accel. I only need to make sure that the temperature of the monitor is not too high at the moment of its restarting.
Step 2: I let the monitor remain started for a period of time until the temperature of monitor and sensor reach a relatively high level, 45°C for example. There's always no error if I don't restart the monitor. But once I restart it at this temperature, an error of about 2° occurs on theta_grav. In this case, if I wait for some minutes or several hours, the theta_grav could drift to the correct value at a random moment. That's why our clients saw a drift during use.
Attention: there's never un error for theta_accel. Its value is always correct.

Sensor configuration codes:
Sensor initialization codes:
test_error = mpu_init(&int_param);
if (test_error!=0)flag_first_time = TRUE;

/* Get/set hardware configuration. Start gyro. */
/* Wake up all sensors. */
test_error = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if (test_error!=0)flag_first_time = TRUE;
/* Push both gyro and accel data into the FIFO. */
test_error = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if (test_error!=0)flag_first_time = TRUE;
test_error = mpu_set_sample_rate(200/*DEFAULT_MPU_HZ 200*/);
if (test_error!=0)flag_first_time = TRUE;

/* Initialize HAL state variables. */
memset(&hal, 0, sizeof(hal));
hal.sensors = ACCEL_ON | GYRO_ON;
hal.report = PRINT_QUAT | PRINT_ACCEL | PRINT_GYRO;

test_error = dmp_load_motion_driver_firmware();
if (test_error!=0)flag_first_time = TRUE;
test_error = dmp_set_orientation(
inv_orientation_matrix_to_scalar(gyro_orientation));
if (test_error!=0)flag_first_time = TRUE;
test_error = dmp_register_tap_cb(tap_cb);
if (test_error!=0)flag_first_time = TRUE;
hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL;
test_error = dmp_enable_feature(hal.dmp_features);
if (test_error!=0)flag_first_time = TRUE;
test_error = dmp_set_fifo_rate(20/*DEFAULT_MPU_HZ*/);
if (test_error!=0)flag_first_time = TRUE;
test_error = mpu_set_dmp_state(1);
if (test_error!=0)flag_first_time = TRUE;
hal.dmp_on = 1;


Codes for reading quaternion and calculating inclination angle:

//read mpu data from fifo bias
test_error = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
vTEMPO32tempo(5);

//calculate gravity data from quaternion
data_grav[0] =
inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
data_grav[1] =
inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
data_grav[2] =
(inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) -
1073741824L;
int ii;
for (ii = 0; ii < 3; ii++)
data_grav[ii] >>= 14;
//calculate inclination angle from gravity data
inclination_angle_x= 1800.0*(asin(gravity_vector_x/65536.0))/3.141593;

Please help me solve this issue as soon as possible. If it can not be solved soon, we'll have to change sensor manufacturer. Thank you.

Best Regards,
Yijun DENG
Development Engineer
Transvideo France