STM32F103与MPU6050、HMC5883L集成的偏航角漂移问题求助

我已将STM32F103RT6TR与MPU6050、HMC5883L进行集成，用于计算加速度、陀螺仪数据以及俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。目前其余数据均正常输出，但偏航角存在漂移现象，恳请各位协助解决该问题。

相关代码实现 #

void MPU6050_Calibrate(I2C_HandleTypeDef *hi2c)
{
    uint8_t rawData[14];
    int16_t accel_raw[3], gyro_raw[3];
    int samples = 2000;

    printf("Calibrating...\n");

    for (int i = 0; i < samples; i++)
    {
        HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, ACCEL_XOUT_H, 1, rawData, 14, HAL_MAX_DELAY);

        accel_raw[0] = (int16_t)(rawData[0] << 8 | rawData[1]);
        accel_raw[1] = (int16_t)(rawData[2] << 8 | rawData[3]);
        accel_raw[2] = (int16_t)(rawData[4] << 8 | rawData[5]);
        gyro_raw[0] = (int16_t)(rawData[8] << 8 | rawData[9]);
        gyro_raw[1] = (int16_t)(rawData[10] << 8 | rawData[11]);
        gyro_raw[2] = (int16_t)(rawData[12] << 8 | rawData[13]);

        accel_offset[0] += accel_raw[0];
        accel_offset[1] += accel_raw[1];
        accel_offset[2] += accel_raw[2];
        gyro_offset[0] += gyro_raw[0];
        gyro_offset[1] += gyro_raw[1];
        gyro_offset[2] += gyro_raw[2];

        //printf("Calibration %d\n",i);
    }

    // Average offsets
    for (int i = 0; i < 3; i++)
    {
        accel_offset[i] /= samples;
        gyro_offset[i] /= samples;
    }
    accel_offset[2] -= ACCEL_SCALE;  // Gravity adjustment
    printf("Calibration Done...\n");
}
// Read all data from MPU6050
void MPU6050_Read_All(I2C_HandleTypeDef *hi2c, float *accel, float *gyro, float *angles)
{
    uint8_t rawData[14];
    int16_t accel_raw[3], gyro_raw[3];
    float accel_angle[2];  // Roll and Pitch from accelerometer

    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_time)/1000.0;  // time difference in seconds

    if(dt == 0)
        dt = 0.01; // fallback for very fast loops

    // Read accelerometer and gyroscope data
    HAL_I2C_Mem_Read(hi2c, MPU6050_ADDR, ACCEL_XOUT_H, 1, rawData, 14, HAL_MAX_DELAY);

    // Convert raw data
    accel_raw[0] = (int16_t)(rawData[0] << 8 | rawData[1]);
    accel_raw[1] = (int16_t)(rawData[2] << 8 | rawData[3]);
    accel_raw[2] = (int16_t)(rawData[4] << 8 | rawData[5]);
    gyro_raw[0] = (int16_t)(rawData[8] << 8 | rawData[9]);
    gyro_raw[1] = (int16_t)(rawData[10] << 8 | rawData[11]);
    gyro_raw[2] = (int16_t)(rawData[12] << 8 | rawData[13]);

    // Scale the values
    accel[0] = (accel_raw[0] - accel_offset[0]) / ACCEL_SCALE;
    accel[1] = (accel_raw[1] - accel_offset[1]) / ACCEL_SCALE;
    accel[2] = (accel_raw[2] - accel_offset[2]) / ACCEL_SCALE;

    gyro[0] = (gyro_raw[0] - gyro_offset[0]) / GYRO_SCALE;
    gyro[1] = (gyro_raw[1] - gyro_offset[1]) / GYRO_SCALE;
    gyro[2] = (gyro_raw[2] - gyro_offset[2]) / GYRO_SCALE;

    // Calculate angles from accelerometer
    accel_angle[0] = atan2f(accel[1], sqrtf(accel[0] * accel[0] + accel[2] * accel[2])) * 180.0 / M_PI;  // Roll
    accel_angle[1] = atan2f(-accel[0], sqrtf(accel[1] * accel[1] + accel[2] * accel[2])) * 180.0 / M_PI;  // Pitch

    // Apply Kalman filter for roll and pitch
    angles[0] = Kalman_Filter(accel_angle[0], gyro[0], dt, &roll_state, &roll_bias);
    angles[1] = Kalman_Filter(accel_angle[1], gyro[1], dt, &pitch_state, &pitch_bias);

    float magnetometer_yaw = atan2f(mag_y, mag_x) * 180.0 / M_PI;  // Magnetometer-based yaw
    yaw = 0.98 * (yaw + gyro[2] * dt) + 0.02 * magnetometer_yaw;  // Complementary filter
    angles[2] = yaw;

    last_time = current_time;


}

void HMC5883L_Read_Mag(void)
{
    uint8_t mag_data[6];
    int16_t raw_x, raw_y, raw_z;

    // Read magnetometer data
    HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDR, HMC5883L_DATA_X_MSB, 1, mag_data, 6, HAL_MAX_DELAY);

    // Combine high and low bytes
    raw_x = (int16_t)(mag_data[0] << 8 | mag_data[1]);
    raw_z = (int16_t)(mag_data[2] << 8 | mag_data[3]);
    raw_y = (int16_t)(mag_data[4] << 8 | mag_data[5]);

    // Convert to Gauss (apply scale if necessary)
    mag_x = (float)raw_x * MAG_SCALE;
    mag_y = (float)raw_y * MAG_SCALE;
}
// Kalman filter implementation
float Kalman_Filter(float accel_angle, float gyro_rate, float dt, float *state, float *bias)

{
    static float P[2][2] = { {1, 0}, {0, 1} };  // Initial error covariance
    const float Q_angle = 0.001, Q_bias = 0.003, R_measure = 0.03;  // Tuning parameters

    float rate = gyro_rate - *bias;
    *state += dt * rate;

    // Predict error covariance
    P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    // Kalman gain
    float S = P[0][0] + R_measure;
    float K[2] = { P[0][0] / S, P[1][0] / S };

    // Update state with measurement
    float y = accel_angle - *state;  // Measurement residual
    *state += K[0] * y;
    *bias += K[1] * y;

    // Update error covariance
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];
    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;

    return *state;
}

主函数执行流程 #

• 初始化MPU6050和HMC5883L
• 对MPU6050进行校准
• 主循环内先调用HMC5883L_Read_Mag()，再调用MPU6050_Read_All(&hi2c1, accel, gyro, angles)

内容的提问来源于stack exchange，提问作者Pugazhazhagan T A