MPU6050 六軸陀螺儀加速度計完整教學：I2C 暫存器讀取、DMP 姿態解算與 FIFO 資料串流

MPU6050 六軸慣性感測器

MPU6050 是 InvenSense 推出的 MEMS 六軸慣性感測器，整合 3 軸陀螺儀與 3 軸加速度計於單一晶片，內建 DMP（Digital Motion Processor）硬體姿態融合引擎，廣泛用於無人機飛控、手機螢幕旋轉、機器人平衡、VR/AR 頭盔等需要即時姿態追蹤的應用。

規格一覽

硬體接線

注意：大部分 MPU6050 模組（如 GY-521）已內建 I2C 上拉電阻，不需額外添加。

I2C 暫存器讀取

MPU6050 使用標準 I2C 協定，7-bit 位址 0x68（AD0=LOW）。設定暫存器和讀取感測資料都透過 I2C 進行：

關鍵暫存器速查

Arduino 程式設計

基礎讀取：原始陀螺儀與加速度資料

// ESP32 MPU6050 基礎讀取
#include <Wire.h>

#define MPU6050_ADDR 0x68

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22);  // SDA, SCL

    // 初始化 MPU6050
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x6B);          // PWR_MGMT_1
    Wire.write(0x00);          // 喚醒（清除 Sleep 位元）
    Wire.endTransmission(true);

    // 設定陀螺儀量程 ±500°/s
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x1B);          // GYRO_CONFIG
    Wire.write(0x08);          // ±500°/s (65.5 LSB/(°/s))
    Wire.endTransmission(true);

    // 設定加速度計量程 ±4g
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x1C);          // ACCEL_CONFIG
    Wire.write(0x08);          // ±4g (8192 LSB/g)
    Wire.endTransmission(true);

    Serial.println("MPU6050 初始化完成");
    delay(100);
}

void loop() {
    int16_t ax, ay, az, gx, gy, gz, temp;

    // Burst 讀取：從 0x3B 開始連續讀取 14 個位元組
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x3B);          // 起始暫存器
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDR, 14, true);

    ax = (Wire.read() << 8) | Wire.read();  // ACCEL_XOUT_H/L
    ay = (Wire.read() << 8) | Wire.read();
    az = (Wire.read() << 8) | Wire.read();
    temp = (Wire.read() << 8) | Wire.read();  // TEMP_OUT_H/L
    gx = (Wire.read() << 8) | Wire.read();    // GYRO_XOUT_H/L
    gy = (Wire.read() << 8) | Wire.read();
    gz = (Wire.read() << 8) | Wire.read();

    // 轉換為物理單位
    float a_scale = 8192.0;  // ±4g
    float g_scale = 65.5;    // ±500°/s
    float tempC = temp / 340.0 + 36.53;

    Serial.printf("Accel: %.2f %.2f %.2f g | Gyro: %.1f %.1f %.1f °/s | Temp: %.1f&C
",
        ax/a_scale, ay/a_scale, az/a_scale,
        gx/g_scale, gy/g_scale, gz/g_scale, tempC);

    delay(50);  // 20 Hz
}

DMP 姿態解算

MPU6050 內建的 DMP（Digital Motion Processor）是專用的硬體運動處理器，可將陀螺儀與加速度計資料融合，輸出四元數或尤拉角，大幅減輕主控晶片的運算負擔。

使用 MPU6050_tockn 函式庫

// ESP32 MPU6050 DMP 姿態解算（含 filter）
// 下載並安裝 MPU6050_tockn 函式庫
#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22);
    mpu6050.begin();
    mpu6050.calcGyroOffsets(true);  // 自動校正陀螺儀零偏
    Serial.println("DMP 初始化完成，請保持靜止 5 秒...");
}

void loop() {
    mpu6050.update();  // 更新感測器資料 + 互補濾波

    Serial.printf("Roll: %.2f° | Pitch: %.2f° | Yaw: %.2f° ",
        mpu6050.getAngleX(),  // Roll (繞 X 軸)
        mpu6050.getAngleY(),  // Pitch (繞 Y 軸)
        mpu6050.getAngleZ()); // Yaw (繞 Z 軸)

    Serial.printf("| Accel: %.2f %.2f %.2f g ",
        mpu6050.getAccX(), mpu6050.getAccY(), mpu6050.getAccZ());

    Serial.printf("| Gyro: %.2f %.2f %.2f °/s
",
        mpu6050.getGyroX(), mpu6050.getGyroY(), mpu6050.getGyroZ());

    delay(10);  // 100 Hz
}

使用 MPU6050 DMP 原生韌體庫（精確版）

// ESP32 MPU6050 DMP 原生 FIFO 讀取
// 安裝 MPU6050 by Electronic Cats 函式庫
#include <MPU6050_6Axis_MotionApps20.h>

MPU6050 mpu;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;         // 四元數
VectorInt16 aa;       // 加速度計 raw
VectorFloat gravity;  // 重力向量
float ypr[3];         // Yaw/Pitch/Roll (弧度)

void setup() {
    Serial.begin(115200);
    Wire.begin(21, 22);
    mpu.initialize();

    if (!mpu.testConnection()) {
        Serial.println("MPU6050 連接失敗！");
        while (1);
    }

    // 初始化 DMP
    uint8_t devStatus = mpu.dmpInitialize();
    if (devStatus != 0) {
        Serial.printf("DMP 初始化失敗：%d
", devStatus);
        while (1);
    }

    // 設定感測器偏移值（需事先校準）
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);

    // 啟用 DMP
    mpu.setDMPEnabled(true);
    mpu.resetFIFO();
    Serial.println("DMP 已啟用，請等待資料...");
}

void loop() {
    fifoCount = mpu.getFIFOCount();
    if (fifoCount < 42) return;  // DMP 封包大小

    mpu.getFIFOBytes(fifoBuffer, 42);

    // 從 FIFO 解出四元數
    mpu.dmpGetQuaternion(&q, fifoBuffer);

    // 轉換為尤拉角
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // 轉換為度數並輸出
    float yaw   = ypr[0] * 180.0 / M_PI;
    float pitch = ypr[1] * 180.0 / M_PI;
    float roll  = ypr[2] * 180.0 / M_PI;

    Serial.printf("Yaw: %7.2f° | Pitch: %7.2f° | Roll: %7.2f°
",
        yaw, pitch, roll);
}

互補濾波器 vs Kalman 濾波器 vs DMP

互補濾波器自實作

// 簡潔互補濾波器（搭配 MPU6050 原始資料）
typedef struct {
    float roll;   // °
    float pitch;  // °
} Attitude;

Attitude complementaryFilter(float ax, float ay, float az,
                              float gx, float gy, float gz,
                              Attitude prev, float dt, float alpha) {
    // alpha = 0.98, dt = 時間間隔 (秒)

    // 從加速度計計算傾角（參考重力向量）
    float accRoll  = atan2(ay, az) * 180.0 / M_PI;
    float accPitch = atan2(-ax, sqrt(ay*ay + az*az)) * 180.0 / M_PI;

    // 從陀螺儀積分角度
    float gyroRoll  = prev.roll  + gx * dt;
    float gyroPitch = prev.pitch + gy * dt;

    // 互補融合
    Attitude out;
    out.roll  = alpha * gyroRoll  + (1 - alpha) * accRoll;
    out.pitch = alpha * gyroPitch + (1 - alpha) * accPitch;

    return out;
}

FIFO 與中斷應用

// MPU6050 FIFO 中斷讀取（不阻塞主迴圈）
// INT 腳位接 ESP32 GPIO 4
volatile bool dataReady = false;

void IRAM_ATTR onDataReady() {
    dataReady = true;
}

void setup() {
    pinMode(4, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(4), onDataReady, RISING);

    // ... MPU6050 + DMP 初始化（同上）...

    // 啟用中斷輸出
    mpu.setIntEnabled(true);
    mpu.setIntDMPEnabled(true);
}

void loop() {
    if (!dataReady) {
        // 主程式可以執行其他工作（如 Wi-Fi、顯示）
        return;
    }
    dataReady = false;

    // 讀取 FIFO 並解算（同上）
    fifoCount = mpu.getFIFOCount();
    if (fifoCount >= 42) {
        mpu.getFIFOBytes(fifoBuffer, 42);
        // 解算並輸出姿態
    }
}

實務注意事項

• 電壓：MPU6050 僅支援 3.3V，接 5V 會燒毀！ESP32 的 3.3V 可直接供電
• 冷啟動：第一次上電時 MPU6050 處於睡眠模式（PWR_MGMT_1 = 0x40），需寫入 0x00 喚醒
• 陀螺儀零偏：開機後前幾秒的靜止資料平均可作為零偏補償，DMP 初始化時會自動校正
• Yaw 飄移：陀螺儀有累積誤差，Yaw 會隨時間緩慢飄移。MPU6050 沒有磁力計，無法絕對校準 Yaw。如需要精確 Yaw，請選用 MPU9250（含磁力計）
• 加速度計衝擊：加速度計對震動敏感，安裝時建議使用避震墊
• 取樣率 vs DLPF：內部取樣率 1 kHz（加速度）/ 8 kHz（陀螺儀），透過 DLPF 可設定低通截止頻率（5~260 Hz）
• FIFO 溢位：如果主控讀取太慢，FIFO 會溢位（overflow），需 reset FIFO 並重新開始

總結

MPU6050 是 CP 值最高的六軸 IMU 感測器，搭配 ESP32 的 I2C 介面和內建 DMP 硬體融合引擎，可在不佔用 CPU 資源的情況下即時輸出精確的 Roll/Pitch/Yaw 姿態資料。

選型參考：

• 基礎傾角偵測：MPU6050 + 互補濾波器（程式簡單、適合入門）
• 姿態即時追蹤：MPU6050 + DMP（硬體融合、不佔 CPU）
• 精確 Yaw 與方向：MPU9250（含 AK8963 磁力計）或 ICM-20948
• 機器人平衡車：MPU6050 + DMP + PID 控制（100 Hz 更新率已足夠）
• 無人機飛控：MPU6000（SPI 介面、更低延遲）或 ICM-20602