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

MPU6050 六軸慣性感測器

當你的專案需要知道自己「現在歪了幾度」的時候，第一個會遇到的零件大概就是 MPU6050。它是 InvenSense 推出的 MEMS 六軸慣性感測器，把 3 軸陀螺儀與 3 軸加速度計整合在單一晶片裡，還內建了 DMP（Digital Motion Processor）這顆硬體姿態融合引擎。

它之所以普及，原因很單純：便宜、好接（兩條 I2C 線）、而且姿態解算可以直接交給晶片自己做。從無人機飛控、手機螢幕旋轉、機器人平衡到 VR/AR 頭盔，需要即時姿態追蹤的地方幾乎都有它。

規格一覽

硬體接線

接線只有四條（加上選用的中斷腳），但有一件事必須先講清楚：

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

最常出現的災難是把它接到 5V —— 這顆晶片最高只到 3.6V，接錯就是直接燒掉。另外 AD0 接地或接 3.3V 會決定 I2C 位址是 0x68 還是 0x69，同一條匯流排上要掛兩顆的時候就靠它。

I2C 暫存器讀取

MPU6050 的操作方式很傳統：就是對著暫存器讀寫。以下是一次完整的「連續讀回加速度資料」流程：

注意最後那一段：讀取時位址會自動遞增，所以一次 Burst 就能把加速度、溫度、陀螺儀全部拿回來，不必分好幾次交易。

關鍵暫存器速查

這張表是實務上最常動到的幾個暫存器，建議先掃一遍再開始寫程式：

Arduino 程式設計

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

先從最原始的方式開始 —— 直接讀暫存器、自己組回 16-bit 數值：

// 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 裡面，ESP32 只負責把結果搬出來。這也是為什麼在資源吃緊的場合，DMP 幾乎是唯一合理的選擇。

使用 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 原生韌體庫（精確版）

要精確控制，就得走 DMP 原生路線。它的初始化順序不能跳：

初始化完成後，資料是透過 FIFO 搬出來的，而每個封包的長度是固定的：

// 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);
}

上圖是姿態輸出的示意。注意綠色那條 Yaw：它會隨時間持續往一個方向跑。這不是程式寫錯，而是 MPU6050 沒有磁力計，陀螺儀的累積誤差無從校正 —— 這就是下面那張表要討論的取捨。

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

姿態解算的方法有好幾種，差別在「你要花多少 CPU 換多少精度」：

互補濾波器自實作

如果你不想依賴任何函式庫，互補濾波器是 CP 值最高的入門做法 —— 幾行數學就能有可用的角度：

// 簡潔互補濾波器（搭配 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 主動通知你：

// 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

我自己的判斷順序是：先問「Yaw 需不需要絕對方向」。需要，就別在 MPU6050 上浪費時間，直接換有磁力計的型號；不需要，那 MPU6050 加 DMP 幾乎就是最佳解，便宜又省 CPU。