ESP32 đọc MPU-6050 tính góc nghiêng bằng bộ lọc bù: nền tảng để flycam tự cân bằng

Nối MPU-6050 với ESP32, đọc thanh ghi không cần thư viện, tính góc roll, pitch từ gia tốc kế, ghép với con quay bằng bộ lọc bù và kiểm tra dấu trước khi dùng cho flycam.

Muốn flycam tự giữ thăng bằng, mạch điều khiển bay phải biết thân máy đang nghiêng bao nhiêu độ và đang quay nhanh cỡ nào. Hai đại lượng này đến từ cảm biến quán tính IMU: con quay hồi chuyển (gyro) đo tốc độ góc, gia tốc kế đo hướng của trọng lực. MPU-6050 có cả hai trong một chip, giao tiếp I2C, nên là lựa chọn quen thuộc khi tự làm mạch bay bằng ESP32.

Bài viết hướng dẫn nối MPU-6050 với ESP32, đọc thẳng các thanh ghi không cần thư viện, tính góc roll, pitch từ gia tốc kế, rồi ghép với con quay bằng bộ lọc bù (complementary filter). Đây cũng là phần đọc góc của chương trình ESP32 làm mạch điều khiển bay ở bài cuối của loạt.

1. Quy ước trục và dấu

Cả loạt bài dùng chung quy ước: roll dương là nghiêng sang phải, pitch dương là ngóc đầu lên, yaw dương là xoay đầu sang phải. Module gắn với mũi tên X in trên mạch chỉ về đầu flycam, Y chỉ sang trái, Z hướng lên trên.

• Con quay đo theo quy tắc bàn tay phải: quay quanh trục +X (hướng về đầu) theo chiều dương làm bên trái nhấc lên, tức là nghiêng phải, nên tốc độ roll bằng gx.
• Quay dương quanh +Y (hướng sang trái) làm đầu chúi xuống, nên tốc độ pitch theo quy ước "ngóc lên là dương" bằng −gy. Tương tự, tốc độ yaw bằng −gz.
• Gắn module theo hướng khác (xoay 90°, lật ngược) thì đổi trục hoặc đổi dấu tương ứng ở ba dòng tính tốc độ góc và hai dòng tính góc từ gia tốc kế.

2. Linh kiện cần chuẩn bị

• ESP32 DevKit V1 30 chân.
• Module MPU-6050 loại GY-521 (có sẵn ổn áp và điện trở kéo lên cho SDA, SCL).
• Dây cắm test board loại ngắn.
• Một tấm phẳng (mica, gỗ mỏng) để gắn ESP32 và module, nghiêng thử bằng tay như thân flycam.

3. Sơ đồ nối dây

• VCC của GY-521 nối 3V3 của ESP32. Module có ổn áp riêng nên cấp 5 V cũng được, nhưng cấp 3,3 V thì mức logic SDA, SCL chắc chắn hợp với ESP32.
• Chân AD0 để trống (có điện trở kéo xuống trên module) thì địa chỉ I2C là 0x68. Nối AD0 lên 3,3 V thì địa chỉ đổi thành 0x69, khi đó sửa DIA_CHI_MPU trong chương trình.
• Trên flycam, gắn module gần tâm khung, chặt và có lớp đệm mỏng chống rung. Rung của động cơ truyền thẳng vào cảm biến là nguyên nhân chính làm góc tính ra bị nhiễu.

4. Tính góc từ gia tốc kế

Khi đứng yên, gia tốc kế chỉ đo trọng lực: một véc tơ dài 1 g luôn hướng thẳng lên. Thân máy nghiêng thì véc tơ này chia lên các trục của module theo sin và cos của góc nghiêng, nên có thể tính ngược ra góc:

• roll = atan2(ay, az)
• pitch = atan2(ax, √(ay² + az²))

Công thức này cho góc tuyệt đối, không trôi theo thời gian. Nhược điểm là gia tốc kế không phân biệt được trọng lực với gia tốc do chuyển động và rung. Khi động cơ quay, góc tính từ gia tốc kế dao động vài độ quanh giá trị đúng; khi flycam tăng tốc hay đổi hướng, giá trị còn lệch hẳn đi một lúc. Gia tốc kế cũng không cho biết góc yaw, vì xoay quanh trục thẳng đứng không làm thay đổi hướng của trọng lực.

5. Con quay và bộ lọc bù

Con quay đo tốc độ góc (°/s). Cộng dồn tốc độ × thời gian sẽ ra góc, rất mượt và phản ứng ngay khi thân máy quay. Nhưng con quay luôn có một phần lệch nhỏ, kể cả khi đã hiệu chuẩn lúc khởi động; cộng dồn mãi thì phần lệch đó thành góc trôi dần, mỗi giây thêm một chút.

Bộ lọc bù ghép hai nguồn theo điểm mạnh của từng loại:

góc = α × (góc + ω × dt) + (1 − α) × góc_gia_tốc

• Với α = 0,98 và chu kỳ 4 ms, hằng số thời gian là α·dt/(1 − α) ≈ 0,2 giây. Thay đổi nhanh hơn khoảng này đi theo con quay; thay đổi chậm hơn, như độ trôi, bị gia tốc kế kéo dần về góc đúng.
• Tăng α (ví dụ 0,99) thì góc mượt hơn, ít bị rung ảnh hưởng nhưng kéo độ trôi về chậm hơn. Giảm α (0,95) thì ngược lại.
• Thay đổi chu kỳ tính thì hằng số thời gian cũng thay đổi theo; muốn giữ nguyên đặc tính thì tính lại α.

Trong mô phỏng, giả sử con quay còn lệch 1 °/s sau hiệu chuẩn: chỉ cộng dồn con quay thì sau 7 giây góc đã sai 7°. Bộ lọc bù giữ sai số còn khoảng 1 °/s × 0,2 s ≈ 0,2°, cộng phần nhiễu nhỏ còn lại từ gia tốc kế.

6. Chương trình

Chương trình đọc thẳng thanh ghi của MPU-6050 bằng thư viện Wire có sẵn trong gói ESP32, không cần cài thêm thư viện. Mở Serial Monitor ở 115200 baud, kiểu xuống dòng "New Line" để gõ lệnh:

/*
  PYWORLD.VN - ESP32 đọc MPU-6050, tính góc roll, pitch bằng bộ lọc bù (nền tảng cân bằng flycam)
  MPU-6050 (GY-521): VCC = 3V3, GND = GND, SCL = GPIO22, SDA = GPIO21; mũi tên X chỉ về đầu, Y sang trái
  Lệnh Serial: c hiệu chuẩn lại con quay, p bật/tắt kiểu Serial Plotter, a<số> đặt hệ số lọc (ví dụ a0.98)
*/

#include <Wire.h>
#include <math.h>

#define CHAN_SDA 21
#define CHAN_SCL 22
#define CHU_KY_US 4000            // Vòng tính 250 lần/giây
#define SO_MAU_HIEU_CHUAN 500     // Khoảng 2 giây giữ yên mạch

const uint8_t DIA_CHI_MPU = 0x68;
const float DO_NHAY_GYRO = 65.5;   // Số đếm mỗi °/s ở thang ±500 °/s
const float RAD_SANG_DO = 57.29578;

float heSoLoc = 0.98;              // Tỉ trọng của con quay trong bộ lọc bù
float lechGx = 0, lechGy = 0, lechGz = 0;
float gocRoll = 0, gocPitch = 0;   // Kết quả bộ lọc bù (độ)
float rollGiaToc = 0, pitchGiaToc = 0;
float tocDoYaw = 0;
bool kieuPlotter = true;
unsigned long lanTinhTruoc = 0;
unsigned long lanInTruoc = 0;

bool ghiThanhGhi(uint8_t thanhGhi, uint8_t giaTri) {
  Wire.beginTransmission(DIA_CHI_MPU);
  Wire.write(thanhGhi);
  Wire.write(giaTri);
  return Wire.endTransmission() == 0;
}

uint8_t docMotByte(uint8_t thanhGhi) {
  Wire.beginTransmission(DIA_CHI_MPU);
  Wire.write(thanhGhi);
  Wire.endTransmission(false);
  Wire.requestFrom(DIA_CHI_MPU, (uint8_t)1);
  return Wire.available() ? Wire.read() : 0xFF;
}

// Đọc 14 byte từ 0x3B: gia tốc X Y Z, nhiệt độ, con quay X Y Z
bool docCamBien(float &ax, float &ay, float &az, float &gx, float &gy, float &gz) {
  Wire.beginTransmission(DIA_CHI_MPU);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0 || Wire.requestFrom(DIA_CHI_MPU, (uint8_t)14) != 14) {
    return false;
  }
  int16_t v[7];
  for (int i = 0; i < 7; i++) {
    uint8_t cao = Wire.read();
    uint8_t thap = Wire.read();
    v[i] = (int16_t)((cao << 8) | thap);
  }
  ax = v[0] / 16384.0;   // đơn vị g, thang ±2 g
  ay = v[1] / 16384.0;
  az = v[2] / 16384.0;
  gx = v[4] / DO_NHAY_GYRO;   // đơn vị °/s
  gy = v[5] / DO_NHAY_GYRO;
  gz = v[6] / DO_NHAY_GYRO;
  return true;
}

void hieuChuanConQuay() {
  Serial.println("Dang hieu chuan con quay, giu yen mach khoang 2 giay...");
  double tx = 0, ty = 0, tz = 0;
  int n = 0;
  while (n < SO_MAU_HIEU_CHUAN) {
    float ax, ay, az, gx, gy, gz;
    if (docCamBien(ax, ay, az, gx, gy, gz)) {
      tx += gx;
      ty += gy;
      tz += gz;
      n++;
    }
    delay(4);
  }
  lechGx = tx / n;
  lechGy = ty / n;
  lechGz = tz / n;
  Serial.printf("Lech con quay (do/s): X %.2f  Y %.2f  Z %.2f\n", lechGx, lechGy, lechGz);
}

void xuLyLenh() {
  if (!Serial.available()) {
    return;
  }
  String lenh = Serial.readStringUntil('\n');
  lenh.trim();
  if (lenh.length() == 0) {
    return;
  }
  char ma = lenh.charAt(0);
  if (ma == 'c') {
    hieuChuanConQuay();
  } else if (ma == 'p') {
    kieuPlotter = !kieuPlotter;
  } else if (ma == 'a') {
    float a = lenh.substring(1).toFloat();
    if (a >= 0.80 && a <= 0.999) {
      heSoLoc = a;
      Serial.printf("He so loc = %.3f\n", heSoLoc);
    }
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(CHAN_SDA, CHAN_SCL, 400000);
  delay(100);
  Serial.println();
  Serial.printf("MPU-6050 WHO_AM_I = 0x%02X\n", docMotByte(0x75));
  // Đánh thức chip, lọc thông thấp khoảng 44 Hz, con quay ±500 °/s, gia tốc ±2 g
  if (!ghiThanhGhi(0x6B, 0x00) || !ghiThanhGhi(0x1A, 0x03) || !ghiThanhGhi(0x1B, 0x08) || !ghiThanhGhi(0x1C, 0x00)) {
    Serial.println("Khong thay MPU-6050 o dia chi 0x68: kiem tra day SDA, SCL va nguon");
  }
  delay(100);
  hieuChuanConQuay();
  float ax, ay, az, gx, gy, gz;
  if (docCamBien(ax, ay, az, gx, gy, gz)) {
    gocRoll = atan2(ay, az) * RAD_SANG_DO;   // Lấy góc ban đầu từ gia tốc kế
    gocPitch = atan2(ax, sqrt(ay * ay + az * az)) * RAD_SANG_DO;
  }
  lanTinhTruoc = micros();
}

void loop() {
  xuLyLenh();
  unsigned long bayGio = micros();
  if (bayGio - lanTinhTruoc < CHU_KY_US) {
    return;
  }
  float dt = (bayGio - lanTinhTruoc) / 1000000.0;
  lanTinhTruoc = bayGio;

  float ax, ay, az, gx, gy, gz;
  if (!docCamBien(ax, ay, az, gx, gy, gz)) {
    return;
  }
  // Quy ước: roll dương là nghiêng phải, pitch dương là ngóc đầu lên, yaw dương là xoay đầu sang phải
  float tocRoll = gx - lechGx;
  float tocPitch = -(gy - lechGy);
  tocDoYaw = -(gz - lechGz);
  rollGiaToc = atan2(ay, az) * RAD_SANG_DO;
  pitchGiaToc = atan2(ax, sqrt(ay * ay + az * az)) * RAD_SANG_DO;
  // Bộ lọc bù: con quay cho phần thay đổi nhanh, gia tốc kế kéo dần về góc đúng
  gocRoll = heSoLoc * (gocRoll + tocRoll * dt) + (1 - heSoLoc) * rollGiaToc;
  gocPitch = heSoLoc * (gocPitch + tocPitch * dt) + (1 - heSoLoc) * pitchGiaToc;

  if (millis() - lanInTruoc >= 40) {   // In 25 lần/giây
    lanInTruoc = millis();
    if (kieuPlotter) {
      Serial.printf("roll_loc:%.2f,pitch_loc:%.2f,roll_gia_toc:%.2f,pitch_gia_toc:%.2f\n",
                    gocRoll, gocPitch, rollGiaToc, pitchGiaToc);
    } else {
      Serial.printf("Roll %6.1f | Pitch %6.1f | gia toc: %6.1f %6.1f | yaw %6.1f do/s\n",
                    gocRoll, gocPitch, rollGiaToc, pitchGiaToc, tocDoYaw);
    }
  }
}

7. Giải thích các phần chính

• Cấu hình chip: ghi 0x00 vào thanh ghi 0x6B để đánh thức chip (mặc định chip ở chế độ ngủ). Thanh ghi 0x1A = 0x03 bật bộ lọc thông thấp bên trong khoảng 44 Hz để giảm rung. 0x1B = 0x08 chọn thang con quay ±500 °/s (65,5 số đếm mỗi °/s), 0x1C = 0x00 chọn thang gia tốc ±2 g (16384 số đếm mỗi g).
• Đọc dữ liệu: hàm docCamBien() đọc liền 14 byte bắt đầu từ thanh ghi 0x3B: ba trục gia tốc, nhiệt độ, ba trục con quay, mỗi giá trị 16 bit có dấu, byte cao trước. Ở tốc độ I2C 400 kHz, một lần đọc mất chưa tới 0,5 ms.
• Hiệu chuẩn con quay: lúc khởi động, chương trình lấy trung bình 500 mẫu khi mạch đứng yên và trừ đi ở mọi lần đọc sau. Vì vậy phải để flycam nằm yên khi cấp điện.
• Chu kỳ cố định: vòng tính chạy mỗi 4 ms (250 lần/giây) và đo dt thật bằng micros(), nên góc vẫn đúng nếu một vòng bị chậm đôi chút.
• Thang ±500 °/s đủ cho thử trên bàn và cho flycam mini bay nhẹ nhàng. Flycam FPV nhào lộn quay nhanh hơn nhiều, khi đó chọn ±2000 °/s (0x1B = 0x18, 16,4 số đếm mỗi °/s), nếu không con quay sẽ bị bão hòa.
• Công thức pitch dùng được trong khoảng ±90°. Mạch bay ở chế độ tự cân bằng chỉ cho nghiêng vài chục độ nên không gặp giới hạn này.

8. Kết quả và cách kiểm tra dấu

Ở kiểu mặc định, mở Tools → Serial Plotter để xem bốn đường: roll, pitch sau bộ lọc và roll, pitch tính riêng từ gia tốc kế. Đường từ gia tốc kế sẽ rung rõ khi gõ nhẹ vào tấm gắn mạch, còn đường sau bộ lọc gần như không đổi. Kiểm tra dấu trước khi dùng cho flycam:

1. Nghiêng tấm sang phải: Roll phải dương.
2. Nâng đầu (phía mũi tên X) lên: Pitch phải dương.
3. Xoay tấm trên mặt bàn theo chiều kim đồng hồ khi nhìn từ trên xuống: yaw phải dương trong lúc đang xoay.

Dấu nào ngược thì module đang gắn khác quy ước ở mục 1: xoay lại module hoặc sửa dấu trong chương trình. Sai dấu ở bước này sẽ làm mạch điều khiển bay đẩy thân máy nghiêng thêm thay vì kéo về.

9. Xử lý lỗi thường gặp

10. Xem thêm

• Cảm biến gia tốc: cấu tạo MEMS và đo góc nghiêng bằng ESP32
• Thực hành hiệu chuẩn tĩnh gia tốc kế MEMS bằng trọng trường (giảm sai lệch của góc từ gia tốc kế)
• I2C là gì: địa chỉ, ACK và cách đọc ghi thanh ghi trên ESP32
• Giao tiếp I2C trên ESP32: quét địa chỉ, đổi chân SDA/SCL
• Giữ tốc độ động cơ DC ổn định bằng PID trên ESP32

Cùng loạt: mạch điều khiển bay của flycam (IMU, PID, Betaflight); ESP32 điều khiển động cơ không chổi than qua ESC; ESP32 làm mạch điều khiển bay cho flycam mini.