Embedded / IoT
Intermediate
Arduino / ESP32 Wire Library
I2C 버스를 통한 MPU6050 6축 자이로 센서 데이터 취득 및 상보 필터(Complementary Filter)
가속도계의 고주파 노이즈와 자이로스코프의 저주파 드리프트(Drift)를 상보 필터로 결합하여 롤(Roll)과 피치(Pitch) 각도를 정확히 추정하는 펌웨어 알고리즘.
#Embedded #I2C #MPU6050 #Sensor #Robotics
핵심 개념가속도 센서는 진동에 취약하지만 장기적으로 중력 기준선이 정확하고, 자이로 센서는 단기 반응이 빠르지만 적분 오차가 누적되어 각도가 흐릅니다. 두 센서를 $\theta = \alpha(\theta + \omega dt) + (1 - \alpha)\theta_{acc}$ ($\alpha \approx 0.96$) 상보 필터로 합성하면 칼만 필터급의 깨끗한 자세각을 얻을 수 있습니다.
1. 상보 필터 각도 추정 C++ 코드
#include <Wire.h>
const int MPU_ADDR = 0x68;
float pitch = 0.0f;
float lastTime = 0.0f;
void setup() {
Wire.begin(21, 22); // ESP32 SDA=21, SCL=22
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x6B); // 전원 관리 레지스터
Wire.write(0x00); // 슬립 해제
Wire.endTransmission(true);
}
void loop() {
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B); // Accel X 레지스터부터 읽기 시작
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDR, 6, true);
int16_t ax = Wire.read() << 8 | Wire.read();
int16_t ay = Wire.read() << 8 | Wire.read();
int16_t az = Wire.read() << 8 | Wire.read();
float accPitch = atan2(-ax, sqrt(ay * ay + az * az)) * (180.0f / PI);
// 자이로 각속도 읽기 생략... (gyroRateY)
float dt = (millis() - lastTime) / 1000.0f;
lastTime = millis();
// 상보 필터 (자이로 96% + 가속도 4%)
pitch = 0.96f * (pitch + gyroRateY * dt) + 0.04f * accPitch;
}