MPU6050 Quaternion calculation with stm32

Question

I've been interested in mpu6050 for a while. I can read gyro data but i need help how to convert it to quaternion. This is pretty easy on Arduino, jrowberg's library is really great. But i am working with a STM32F4.

I also checked the mpu6050's datasheet. I was able to compile such a code for calculations.

definations like this:

typedef struct {
    float Temperature;

    int16_t Gyro_X_RAW;
    int16_t Gyro_Y_RAW;
    int16_t Gyro_Z_RAW;
    float Gx;
    float Gy;
    float Gz;
} MPU6050_t;

typedef struct {
    float w;
    float x; 
    float y; 
    float z; 
} Quaternion;

Quaternion q = {1.0, 0.0, 0.0, 0.0}; 
MPU6050_t MPU6050;

code i found to read gyro data:

void MPU6050_Read_Gyro(I2C_HandleTypeDef *I2Cx, MPU6050_t *DataStruct)
{
    uint8_t Rec_Data[6];

    // Read 6 BYTES of data starting from GYRO_XOUT_H register

    HAL_I2C_Mem_Read(I2Cx, MPU6050_ADDR, GYRO_XOUT_H_REG, 1, Rec_Data, 6, 100);

    DataStruct->Gyro_X_RAW = (int16_t)(Rec_Data[0] << 8 | Rec_Data[1]);
    DataStruct->Gyro_Y_RAW = (int16_t)(Rec_Data[2] << 8 | Rec_Data[3]);
    DataStruct->Gyro_Z_RAW = (int16_t)(Rec_Data[4] << 8 | Rec_Data[5]);

    /*** convert the RAW values into dps (�/s)
         we have to divide according to the Full scale value set in FS_SEL
         I have configured FS_SEL = 0. So I am dividing by 131.0
         for more details check GYRO_CONFIG Register              ****/

    DataStruct->Gx = DataStruct->Gyro_X_RAW / 131.0;
    DataStruct->Gy = DataStruct->Gyro_Y_RAW / 131.0;
    DataStruct->Gz = DataStruct->Gyro_Z_RAW / 131.0;

}

Infinite loop:

while (1)
  {
      MPU6050_Read_Temp(&hi2c1, &MPU6050);
      MPU6050_Read_Gyro(&hi2c1, &MPU6050);
      float gyroX = MPU6050.Gx;
      float gyroY = MPU6050.Gy;
      float gyroZ = MPU6050.Gz;
      calculateQuaternion(&q, gyroX, gyroY, gyroZ, dt);

      char buffer[50];    
      uint8_t size = sprintf(buffer,"quat\t%.2f\t%.2f\t%.2f\t%.2f\r\n", q.w, q.x, q.y, q.z);      
      HAL_StatusTypeDef status = HAL_UART_Transmit(&huart3, (uint8_t*)buffer, size, 1000);
      HAL_Delay(1000);
  }

calculateQuaternion:

void calculateQuaternion(Quaternion *q, float gyroX, float gyroY, float gyroZ, float dt) {

    float dThetaX = gyroX * dt;
    float dThetaY = gyroY * dt;
    float dThetaZ = gyroZ * dt;

    float dq0 = 1.0;
    float dq1 = dThetaX * 0.5;
    float dq2 = dThetaY * 0.5;
    float dq3 = dThetaZ * 0.5;

    q->w += dq0 * q->w - dq1 * q->x - dq2 * q->y - dq3 * q->z;
    q->x += dq0 * q->x + dq1 * q->w + dq2 * q->z - dq3 * q->y;
    q->y += dq0 * q->y - dq1 * q->z + dq2 * q->w + dq3 * q->x;
    q->z += dq0 * q->z + dq1 * q->y - dq2 * q->x + dq3 * q->w;

    float norm = sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}

But when I check in debug mode, there is not much change in the gyro data, even when the sensor is fixed, the quaternion data continues to change.

serial port output

Can you help me correct if I am doing a mistake in the calculation? I would appreciate it if you could suggest a library or example that I can use.