admin管理员组文章数量:1122846
I integrated an STM32F103RT6TR with an MPU6050 and HMC5883L to calculate acceleration, gyroscope, pitch, roll, and yaw values. While everything else is working fine, the yaw value is drifting. Can someone help me resolve this issue?
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;
}
In the main function, I initialize the MPU6050 and HMC5883L, then calibrate the MPU6050. Inside the while loop, I call HMC5883L_Read_Mag() followed by MPU6050_Read_All(&hi2c1, accel, gyro, angles).
I integrated an STM32F103RT6TR with an MPU6050 and HMC5883L to calculate acceleration, gyroscope, pitch, roll, and yaw values. While everything else is working fine, the yaw value is drifting. Can someone help me resolve this issue?
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;
}
In the main function, I initialize the MPU6050 and HMC5883L, then calibrate the MPU6050. Inside the while loop, I call HMC5883L_Read_Mag() followed by MPU6050_Read_All(&hi2c1, accel, gyro, angles).
- What's the incoming format from the accelerometer? Signed 16 bit 2's complement big endian? – Lundin Commented Nov 25, 2024 at 15:58
1 Answer
Reset to default 0SO is not to recommend libraries but math related to the 6 (or 9)axis is much more complicated than your simple equations. Integration of data from many MEMS sensors is not an easy task. Even noise removal from the raw data is quite a complicated DSP-related task.
In am using with success https://www.st.com/en/embedded-software/x-cube-mems1.html.
https://github.com/STMicroelectronics/X-CUBE-MEMS1/tree/main
本文标签： embeddedquotYaw Drift Issue in STM32F103 Integration with MPU6050 and HMC5883LquotStack Overflow
版权声明：本文标题：embedded - "Yaw Drift Issue in STM32F103 Integration with MPU6050 and HMC5883L" - Stack Overflow 内容由网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：http://www.betaflare.com/web/1736299785a1930582.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。
发表评论