MPU6050 inconsistent Gyro data

Question

I'm using an MPU6050 for a custom flight controller and I am facing an issue when reading the values from the IMU. The data shown below represents the angular velocity in the y-axis. When the angular velocity is increasing or decreasing the overall trend seems to be increasing/decreasing but there are random drops to 0 degrees / second. The end goal is to read from the accelerometer and gyroscope and calculate the current angular position of the device but the inconsistent data may produce garbage angles.

The data below is taken when I am rotating the MPU6050 about its x-axis.

Is this behaviour normal for a gyroscope or am I doing something wrong. If this is normal, is there a filter that I can apply to get more accurate values?

MPU6050 Datasheet: https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf

MPU6050 Register Map: https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf

Specs:

Clock speed: 16MHz internal

Communication: I2C (100KHz)

MCU: Nucleo 64 STM32f446RE

Data:

Sample of Data

#    DPS
223 -6.442748
224 -2.076336
225 0.732824
226 1.664122
227 1.832061
228 -1.740458
229 -0.549618
230 5.679389
231 8.351145
232 -0.442748
233 12.045801
234 15.78626
235 19.725191
236 28.106871
237 -0.473282
238 31.709925
239 31.450382
240 -0.70229
241 -1.526718
242 29.007633
243 -1.862595
244 32.152672
245 -0.580153
246 -1.725191
247 40.229008
248 -0.687023
249 -1.587786
250 48.580154
251 51.61832
252 -0.381679
253 -0.183206
254 63.343513

Code:

while (1){
    MPU_config();
    MPU_getData();
}

void MPU_config(){
    //turns off SLEEP and CYCLE mode (register 0x6B)
    i2c_txData = 0x0;
    HAL_I2C_Mem_Write(&hi2c1,i2c_devAddr,
    (uint16_t)PWR_MGMT_1,1,&i2c_txData,1,HAL_MAX_DELAY);
    // Gyro Configuration register (0x1B)
    i2c_txData = 0x08; // fullscale +-500dps (0x08)
    HAL_I2C_Mem_Write(&hi2c1,i2c_devAddr,
    (uint16_t)GYRO_CONFIG,1,&i2c_txData,1,HAL_MAX_DELAY);
    // Accelerometer Configuration (0x1C)
    i2c_txData = 0x08; // fullscale +-4g
    HAL_I2C_Mem_Write(&hi2c1,i2c_devAddr,
    (uint16_t)ACCEL_CONFIG,1,&i2c_txData,1,HAL_MAX_DELAY);
}

void MPU_getData(){
    uint8_t ugyrox_h=0, ugyrox_l=0;
    signed char gyrox_h=0, gyrox_l=0;

    //0x0064 (100) random delay value
   HAL_I2C_Mem_Read(&hi2c1,i2c_devAddr|0x1,GYRO_XOUT_H,1,&ugyrox_h,1,0x0064);
   HAL_I2C_Mem_Read(&hi2c1,i2c_devAddr|0x1,GYRO_XOUT_L,1,&ugyrox_l,1,0x0064);

    // gyro data in MPU is in 2s complement
    gyrox_h = (signed char) ugyrox_h;
    gyrox_l = (signed char) ugyrox_l;

    //gyro_500 = 1.0f/65.5f (gyro_sensitivity: 500dps)
    gyroData = ((float) ((gyrox_h<<8) | gyrox_l))*gyro_500;

    printf("%x \t %x \t %f \n ",gyrox_h,gyrox_l,gyroData);
}

Gyro Configuration FS_SEL = 1, for 500dps (page 14 of register map PDF)

Sensitivity factor is 65.5, "gyro_500" is 1/65.5 in the code (page 12 of datasheet)

Answer 1

So I found a way around the issue by filtering the data. Since there are random dips to 0, I replace a 0 with the previous value which was valid. This works out since it is safe to assume that the actual value when the gyro outputted 0 was close to the previous value which was valid. So its more accurate to keep that previous value instead of using 0.

In addition to this, I also used the average of the last 15 samples of data as the output.

Code:

I need to clean up the code, but here it is... WINDOW_SIZE is set to 15 for the graph below

void MPU_getData(){
    uint8_t ugyrox_h=0, ugyrox_l=0, i;
    signed char gyrox_h=0, gyrox_l=0;
    int16_t tot=0;

    if(run == WINDOW_SIZE) run = 0;

    //0x0064 (100) random delay value

    HAL_I2C_Mem_Read(&hi2c1,i2c_devAddr|0x1,GYRO_XOUT_H,1,&ugyrox_h,1,0x0064);

  HAL_I2C_Mem_Read(&hi2c1,i2c_devAddr|0x1,GYRO_XOUT_L,1,&ugyrox_l,1,0x0064);

    // gyro data in MPU is in 2s complement
    gyrox_h = (signed char) ugyrox_h;
    gyrox_l = (signed char) ugyrox_l;

    //gyro_500 = 1.0f/65.5f (gyro_sensitivity: 500dps)
    gyroData = ((float) ((gyrox_h<<8) | gyrox_l))*gyro_2000;

    if( (gyroData < 2) && (gyroData > -2) ){
        gyroData = prevGyro;
    }

    window[run] = (int)gyroData;

    for(i=0; i<WINDOW_SIZE; i++){
        tot += window[i];
    }

    ++run;
    prevGyro = gyroData;
    printf("%x \t %x \t %d \n ",gyrox_h,gyrox_l,(tot/WINDOW_SIZE));
}

Data: