MPU6050 bị trôi góc như thế nào?

để test MPU6050 với ESP32, tập trung vào việc đo Gyro + Roll/Pitch + Yaw và độ trôi Yaw. Code sẽ tự lấy mẫu lúc khởi động để bù offset gyro.

Đấu dây

MPU6050 GY-521	ESP32 38 chân
VCC	5V
GND	GND
SDA	GPIO 21
SCL	GPIO 22
INT	Không cần
AD0	GND / để mặc định

module GY-521 MPU6050 thông dụng thì có thể cấp VCC = 5V, và SDA/SCL thường có thể nối trực tiếp với ESP32. Nối nguồn 3.3V dễ bị lỗi, treo MPU6050.

Địa chỉ mặc định: 0x68.

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

#define MPU_ADDR 0x68
#define SDA_PIN 21
#define SCL_PIN 22

// ==============================
// RAW DATA
// ==============================
int16_t rawAx, rawAy, rawAz;
int16_t rawGx, rawGy, rawGz;

// ==============================
// SENSOR VALUES
// ==============================
float ax, ay, az;
float gx, gy, gz;

// Offset gyro
float offsetGx = 0;
float offsetGy = 0;
float offsetGz = 0;

// Góc
float roll = 0;
float pitch = 0;
float yaw = 0;

// Timing
unsigned long lastMicros = 0;
unsigned long startMillis = 0;
unsigned long lastPrint = 0;


// ======================================================
// WRITE MPU6050 REGISTER
// ======================================================
void writeMPU(byte reg, byte data)
{
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}


// ======================================================
// READ MPU6050
// ======================================================
bool readMPU()
{
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);

  if (Wire.endTransmission(false) != 0)
    return false;

  Wire.requestFrom(MPU_ADDR, 14, true);

  if (Wire.available() < 14)
    return false;

  rawAx = (Wire.read() << 8) | Wire.read();
  rawAy = (Wire.read() << 8) | Wire.read();
  rawAz = (Wire.read() << 8) | Wire.read();

  // Bỏ qua nhiệt độ
  Wire.read();
  Wire.read();

  rawGx = (Wire.read() << 8) | Wire.read();
  rawGy = (Wire.read() << 8) | Wire.read();
  rawGz = (Wire.read() << 8) | Wire.read();

  // Accelerometer ±2g
  ax = rawAx / 16384.0f;
  ay = rawAy / 16384.0f;
  az = rawAz / 16384.0f;

  // Gyroscope ±250 deg/s
  gx = rawGx / 131.0f;
  gy = rawGy / 131.0f;
  gz = rawGz / 131.0f;

  return true;
}


// ======================================================
// CALIBRATE GYROSCOPE
// ======================================================
void calibrateGyro()
{
  Serial.println();
  Serial.println("======================================");
  Serial.println("CALIBRATE MPU6050");
  Serial.println("DAT MPU6050 NAM YEN HOAN TOAN!");
  Serial.println("KHONG CHAM / KHONG RUNG");
  Serial.println("======================================");

  delay(2000);

  const int samples = 2000;

  double sumX = 0;
  double sumY = 0;
  double sumZ = 0;

  int count = 0;

  for (int i = 0; i < samples; i++)
  {
    if (readMPU())
    {
      sumX += gx;
      sumY += gy;
      sumZ += gz;
      count++;
    }

    delay(2);
  }

  if (count > 0)
  {
    offsetGx = sumX / count;
    offsetGy = sumY / count;
    offsetGz = sumZ / count;
  }

  Serial.println();
  Serial.println("===== GYRO OFFSET =====");

  Serial.print("GX offset = ");
  Serial.println(offsetGx, 6);

  Serial.print("GY offset = ");
  Serial.println(offsetGy, 6);

  Serial.print("GZ offset = ");
  Serial.println(offsetGz, 6);

  Serial.println("=======================");
  Serial.println();

  delay(1000);
}


// ======================================================
// SETUP
// ======================================================
void setup()
{
  Serial.begin(115200);
  delay(1000);

  Serial.println();
  Serial.println("================================");
  Serial.println("ESP32 + MPU6050 TEST");
  Serial.println("================================");

  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(400000);

  // Kiểm tra MPU6050
  Wire.beginTransmission(MPU_ADDR);
  byte error = Wire.endTransmission();

  if (error != 0)
  {
    Serial.println("KHONG TIM THAY MPU6050!");
    Serial.println("Kiem tra SDA/SCL/VCC/GND.");

    while (1)
    {
      delay(1000);
    }
  }

  Serial.println("Tim thay MPU6050 tai 0x68");

  // Wake up MPU6050
  writeMPU(0x6B, 0x00);

  delay(100);

  // Gyro ±250 deg/s
  writeMPU(0x1B, 0x00);

  // Accelerometer ±2g
  writeMPU(0x1C, 0x00);

  // Digital Low Pass Filter
  // DLPF_CFG = 3
  writeMPU(0x1A, 0x03);

  delay(100);

  // Calibration
  calibrateGyro();

  // Reset góc
  yaw = 0;

  startMillis = millis();
  lastMicros = micros();
  lastPrint = millis();

  Serial.println("BAT DAU TEST...");
  Serial.println("KHONG DI CHUYEN MPU6050 DE TEST DRIFT");
  Serial.println();
}


// ======================================================
// LOOP
// ======================================================
void loop()
{
  if (!readMPU())
    return;

  unsigned long nowMicros = micros();

  float dt = (nowMicros - lastMicros) / 1000000.0f;

  lastMicros = nowMicros;

  // Chống lỗi dt bất thường
  if (dt <= 0 || dt > 0.1)
    return;

  // ==============================
  // BÙ OFFSET GYRO
  // ==============================

  float gyroX = gx - offsetGx;
  float gyroY = gy - offsetGy;
  float gyroZ = gz - offsetGz;


  // ==============================
  // ROLL / PITCH
  // từ accelerometer
  // ==============================

  roll = atan2(ay, az) * 180.0f / PI;

  pitch = atan2(
            -ax,
            sqrt(ay * ay + az * az)
          ) * 180.0f / PI;


  // ==============================
  // YAW
  // tích phân Gyro Z
  // ==============================

  yaw += gyroZ * dt;


  // ==============================
  // PRINT MỖI 500ms
  // ==============================

  if (millis() - lastPrint >= 500)
  {
    lastPrint = millis();

    float timeSec =
      (millis() - startMillis) / 1000.0f;

    Serial.print("Time: ");
    Serial.print(timeSec, 1);
    Serial.print(" s");

    Serial.print(" | Roll: ");
    Serial.print(roll, 2);

    Serial.print(" | Pitch: ");
    Serial.print(pitch, 2);

    Serial.print(" | Yaw: ");
    Serial.print(yaw, 3);

    Serial.print(" | GyroZ: ");
    Serial.print(gyroZ, 4);

    Serial.println(" deg/s");
  }
}

Khi bật nguồn, để MPU6050 nằm yên hoàn toàn khoảng 7–8 giây. Giai đoạn đầu nó sẽ lấy 2.000 mẫu để xác định offset. Sau đó Serial Monitor sẽ có dạng:

Kết quả này khá tốt. MPU6050 của anh đang có độ trôi Yaw thấp sau khi calibration.

Từ log:

39.0 s: Yaw = +0.018°
50.0 s: Yaw = −0.196°
Trong 11 giây thay đổi khoảng −0.214°, tương đương xu hướng khoảng −0.0195°/s trong đoạn log này.
Nếu xu hướng đó giữ nguyên, 30 giây tương đương khoảng 0.58°, 60 giây khoảng 1.17°. Tuy nhiên không nên ngoại suy quá xa vì bias gyro thay đổi theo nhiệt độ.

Điểm đáng chú ý là GyroZ khi cảm biến đứng yên dao động khoảng -0.08 → +0.05 °/s. Đây là bình thường; do tích phân liên tục nên các sai số nhỏ này mới làm Yaw từ từ trôi.

Roll ≈ 4.2–4.5° không có nghĩa cảm biến sai 4°. Nhiều khả năng bo MPU6050 của anh đang đặt nghiêng khoảng 4°. Pitch khoảng 0.4–0.7°.

11:29:01.183 -> Time: 453.0 s | Roll: 5.03 | Pitch: 0.78 | Yaw: -10.475 | GyroZ: 0.0478 deg/s

11:29:01.638 -> Time: 453.5 s | Roll: 4.67 | Pitch: 0.73 | Yaw: -10.491 | GyroZ: -0.0132 deg/s

11:29:02.178 -> Time: 454.0 s | Roll: 5.16 | Pitch: 0.78 | Yaw: -10.505 | GyroZ: -0.0361 deg/s

Kết quả sau hơn 7,5 phút cho thấy rõ hạn chế của MPU6050 khi dùng để đo Yaw.

Tại 450.5 s, Yaw = −10.407° và tại 458.5 s, Yaw = −10.630°. Như vậy tốc độ drift trung bình toàn khoảng đo xấp xỉ:

\[ 10.63 / 458.5 \approx 0.0232^\circ/s \]

Tương đương gần đúng:

Thời gian	Drift ước tính
10 giây	0.23°
30 giây	0.70°
1 phút	1.39°
5 phút	6.96°
7.6 phút	10.63° thực tế
10 phút	~13.9°

Điều thú vị là kết quả này khá tuyến tính. Ví dụ đoạn 450.5 → 458.5 s, Yaw từ −10.407° → −10.630°, tức thêm khoảng −0.223° trong 8 giây = −0.0279°/s, khá gần mức trung bình toàn bài test.

Với robot của anh thì sao?

Nếu một đoạn robot chạy thẳng chỉ khoảng 10–30 giây, MPU6050 vẫn có khả năng dùng tốt.

Ví dụ 30 giây drift khoảng:

~0.7°

Nếu robot chạy 10 m mà thực sự sai hướng 0.7° liên tục thì độ lệch ngang lý thuyết khoảng:

\[ 10m\times\tan(0.7^\circ)\approx 0.122m \]

tức khoảng 12 cm. Tuy nhiên đây không đồng nghĩa robot chắc chắn lệch 12 cm, vì bộ điều khiển liên tục cân chỉnh bánh xe và các sai số cơ khí khác cũng tham gia.

Vấn đề lớn hơn là không nên lấy Yaw = 0 lúc bật máy rồi dùng mốc đó suốt 10–30 phút.

Tôi sẽ thiết kế logic robot kiểu:

Bắt đầu đoạn thẳng
       ↓
ghi currentYaw làm targetYaw
       ↓
chạy
       ↓
MPU6050 đo lệch góc
       ↓
PID điều chỉnh 2 bánh
       ↓
đến cuối đoạn
       ↓
quay 90°
       ↓
bắt đầu đoạn mới
       ↓
đặt targetYaw mới

Như vậy MPU6050 chỉ cần chính xác trong mỗi đoạn chạy, thay vì phải chính xác hàng chục phút.

Có một vấn đề nữa trong log rất đáng chú ý: Roll ban đầu khoảng 4.3°, đến 458 s khoảng 4.9–5.0°. Nếu cảm biến hoàn toàn không bị di chuyển, mức thay đổi này cho thấy nhiệt độ/rung/nhiễu hoặc cách tính Roll trực tiếp từ accelerometer cũng đang dao động.

Kết luận với con MPU6050 này: giữ hướng tương đối trong 10–30 giây: khá khả thi; giữ Yaw trong nhiều phút: không nên. BNO085/BNO086 vẫn phù hợp hơn nếu cần góc quay lâu dài ổn định.

Nếu muốn, bước tiếp theo tôi có thể sửa code để tự bù drift ~−0.023°/s. Với chính cảm biến này, Yaw sau 7 phút có thể giảm sai số rất đáng kể, nhưng nên bù theo bias được đo sau khi cảm biến đã ổn định nhiệt thay vì hard-code một con số.

-----------------

Giải pháp

Code để tự bù drift ~−0.023°/s. Với chính cảm biến này, Yaw sau 7 phút có thể giảm sai số rất đáng kể, nhưng nên bù theo bias được đo sau khi cảm biến đã ổn định nhiệt thay vì hard-code một con số.

Với kết quả vừa đo, tôi khuyên không hard-code 0.023°/s ngay, mà cho ESP32 tự đo drift khi khởi động rồi tự bù. Điều kiện là MPU6050 phải đứng yên tuyệt đối trong lúc calibration.

Bản dưới đây calibration khoảng 10 giây, xác định gyroZOffset, sau đó Yaw dùng giá trị đã bù. Tôi cũng thêm deadband rất nhỏ để nhiễu gyro khi đứng yên không bị tích phân liên tục.

Kết quả này tốt hơn rất nhiều so với lần test trước.

Từ 42.5 s → 50.5 s, Yaw chỉ từ −0.032° → −0.049°, tức thay đổi khoảng:

\[ 0.017^\circ / 8s \approx 0.0021^\circ/s \]

Quan trọng hơn, phần lớn thời gian anh thấy:

GZ corrected: 0.0000 deg/s

nghĩa là deadband đang loại được nhiễu gyro khi cảm biến đứng yên. GZ raw ≈ -0.7°/s không đáng lo vì offset đo lúc calibration cũng đang khoảng mức đó; chương trình đã trừ offset trước khi xét deadband.

Có vài mẫu như:

GZ corrected: -0.0824
GZ corrected: +0.1084

vượt ngưỡng ±0.08°/s, nên vẫn được tích phân vào Yaw. Vì vậy Yaw vẫn thay đổi rất nhỏ.

Chưa nên tăng deadband quá cao, vì khi gắn lên robot, robot có thể lệch hướng rất chậm. Nếu đặt chẳng hạn 0.5°/s, MPU6050 có thể coi chuyển động lệch thật là nhiễu.

Hiện tại ±0.08°/s là mức hợp lý để tiếp tục thử.

Một điểm rất đáng chú ý: Yaw ở 50 giây chỉ −0.042°, trong khi code trước ở 50 giây khoảng −0.196°. Tức kết quả hiện tại tốt hơn khoảng 4–5 lần ở mốc này.

chưa hề xoay hay di chuyển MPU6050 mà Yaw từ khoảng -0.077° ở 187,5 giây lên +0.968° ở 250 giây thì có vấn đề đáng chú ý: trong khoảng 62,5 giây cảm biến đã tự trôi khoảng +1,045°.

Điều này cho thấy deadband 0.08°/s chỉ chặn được phần lớn nhiễu, nhưng có những thời điểm GZ corrected vượt ±0.08 nên vẫn được cộng vào Yaw. Sau đó khi GZ corrected = 0, Yaw chỉ đứng lại ở giá trị sai, chứ deadband không kéo Yaw về 0.

Vì vậy, không nên kết luận MPU6050 đã hết drift từ việc thấy GZ corrected = 0.

Tôi đề xuất sửa thuật toán theo hướng tốt hơn cho robot: khi cảm biến được xác định là đứng yên, ESP32 sẽ liên tục học lại offsetGz (adaptive bias), thay vì chỉ dùng deadband. Khi robot thực sự quay, việc tự học offset phải dừng lại để không ăn mất chuyển động thật.

Bây giờ kết quả đã khá rõ. Nếu trong suốt thời gian này anh không hề xoay MPU6050, thì sau khoảng 591 giây ≈ 9 phút 51 giây, Yaw đã từ 0° lên +2.251°.

So với code ban đầu, đây vẫn là cải thiện lớn: trước đó khoảng 458 giây đã −10.63°; hiện tại gần 10 phút mới khoảng +2.25°. Tuy nhiên, deadband không loại được drift hoàn toàn.

Đặc biệt từ 586 → 591 s, dù các dòng được in đều cho GZ corrected = 0, Yaw vẫn từ 2.236 → 2.251°. Điều này xảy ra vì Serial chỉ in mỗi 500 ms, còn MPU được đọc rất nhiều lần giữa hai lần in. Một số mẫu trung gian vượt ±0.08°/s và vẫn được tích phân.

Tốc độ drift trung bình toàn bài test khoảng:

\[ 2.251/591 = 0.00381^\circ/s \]

tức khoảng 0.23°/phút. Nhưng drift không tuyến tính hoàn toàn, nên không nên chỉ lấy 0.00381°/s rồi trừ cố định.

Với robot, tôi sẽ không tăng deadband lên quá lớn. Ví dụ tăng lên 0.2–0.3°/s có thể làm robot quay rất chậm mà MPU6050 tưởng là không quay.

Giải pháp tốt hơn là adaptive bias. Khi ESP32 xác định robot thực sự đứng yên, nó từ từ cập nhật:

Sau mỗi lần robot STOP, chỉ cần đứng yên khoảng 0.5–2 giây, MPU6050 có thể cập nhật lại bias trước đoạn chạy tiếp theo. Đây phù hợp hơn rất nhiều với kiểu robot chạy thẳng → quay → chạy thẳng → quay của anh.

Nếu mục tiêu trước mắt vẫn là test riêng MPU6050 chưa gắn robot, tôi có thể gửi luôn bản code V2 có tự nhận biết đứng yên + adaptive bias, rồi chúng ta để nó chạy 10–20 phút xem Yaw còn trôi bao nhiêu.

Được. Bản này tôi đổi sang V2 Adaptive Bias. Điểm khác quan trọng là ESP32 sẽ tự nhận biết MPU6050 đang đứng yên và từ từ cập nhật lại offsetGz. Khi phát hiện đang xoay/chuyển động, nó ngừng học offset và dùng gyro để tính Yaw.

Để test đúng, bật nguồn và không chạm MPU6050 khoảng 10 giây đầu.

Khi test, chú ý Status

Sau khoảng 0,5 giây đứng yên, anh phải thấy:

Status:STILL - LEARNING

và OffsetZ sẽ thay đổi rất chậm theo nhiệt độ của MPU6050:

OffsetZ:-0.73542

Khi đó chương trình ngừng học offset, vì nếu tiếp tục học trong lúc xoay thì nó sẽ nhầm chuyển động thật thành sai số gyro.

Khi đặt xuống:

MOVING
MOVING
...
STILL - LEARNING
STILL - LEARNING

nó bắt đầu tự hiệu chỉnh bias trở lại.

Bài test tôi muốn anh làm

Đầu tiên chưa xoay gì cả, để khoảng 10 phút và xem:

0 s       Yaw ≈ 0°
60 s      ?
300 s     ?
600 s     ?

Sau đó mới làm bài:

0° → xoay +90° → giữ → xoay về 0° → giữ.

Có một điểm quan trọng: bản V2 này phù hợp để thử thuật toán, nhưng khi đưa lên robot tôi sẽ sửa nhận biết STILL dựa thêm vào trạng thái motor/Hall. Như vậy khi robot đang chạy thẳng rất đều với tốc độ góc gần 0, chương trình sẽ không nhầm robot đang chạy là đang đứng yên và tự thay đổi offset.

tôi sẽ gửi luôn code V3 tổng thể, trong đó nhận biết đứng yên tốt hơn bằng cách theo dõi độ biến thiên của gyro/accelerometer, thay vì chỉ kiểm tra một mẫu có vượt 0.30 hay không. Cách đó hợp với MPU6050 hơn.

Kết quả này khá rõ và MPU6050 của anh đang đo góc quay tương đối khá tốt.

Theo các mốc anh đánh dấu:

Vị trí thực tế	Yaw MPU6050	Sai lệch xấp xỉ
0° ban đầu	-1.31°	lấy làm mốc
90°	-85.35°	khoảng 4–5°
180°	-175.85°	khoảng 4°
quay về 0°	-2.81°	lệch ~1.50° so với ban đầu

Điểm tôi quan tâm nhất là quay về đúng vị trí ban đầu:

\[ -2.81 - (-1.31) \approx -1.50^\circ \]

Tức sau cả chu trình:

0° → 90° → 180° → 0°

sai số vòng kín chỉ khoảng 1.5°. Với MPU6050 không có từ kế, kết quả này khá khả quan cho robot của anh.

Có một điều cần lưu ý: ở mốc “90°”, Yaw chỉ khoảng -85.35°. Chưa thể kết luận MPU thiếu chính xác 4.65°, vì anh quay bằng tay nên góc thực tế có thể không đúng chính xác 90°. Mốc quay trở lại đúng vị trí đánh dấu ban đầu đáng tin hơn để đánh giá.

Tôi đánh giá bài test này đủ tốt để chuyển sang bước tiếp theo: ghép MPU6050 vào thuật toán robot giữ thẳng và quay 90°. Khi ghép, tôi sẽ bỏ việc MPU tự quyết định STILL và cho trạng thái motor + Hall quyết định lúc nào được phép adaptive bias, an toàn hơn V3.1 hiện tại.

#include <Arduino.h>

#include <Wire.h>

#include <math.h>

// ======================================================

// ESP32 + MPU6050 V3.1

// - Initial calibration

// - Roll / Pitch / Yaw

// - Low-pass gyro

// - Detect STILL

// - Adaptive gyro bias

// - Gyro deadband

// - Debug đầy đủ

// ESP32:

// SDA = GPIO21

// SCL = GPIO22

// ======================================================

// MPU6050

// ======================================================

#define MPU_ADDR 0x68

#define SDA_PIN 21

#define SCL_PIN 22

// ======================================================

// CALIBRATION

// ======================================================

// Số mẫu lấy offset ban đầu

#define CALIBRATION_SAMPLES 3000

// Chờ MPU6050 ổn định nhiệt ban đầu

#define WARMUP_TIME 5000

// ======================================================

// SERIAL

// ======================================================

#define PRINT_INTERVAL 500

// ======================================================

// STILL DETECTION

// ======================================================

// Gyro đã lọc phải nhỏ hơn ngưỡng này

#define STILL_GYRO_THRESHOLD 0.60f

// MPU của anh đang đo khoảng 0.88G khi đứng yên

// nên mở vùng nhận biết rộng hơn

#define STILL_ACCEL_MIN 0.80f

#define STILL_ACCEL_MAX 1.20f

// Phải đứng yên liên tục 1 giây

#define STILL_TIME 1000

// ======================================================

// LOW PASS FILTER

// ======================================================

// 0.10 = lọc khá mạnh

#define FILTER_ALPHA 0.10f

// ======================================================

// ADAPTIVE BIAS

// ======================================================

// Tốc độ tự học lại offset gyro

// nhỏ -> học chậm nhưng an toàn

#define BIAS_ALPHA 0.0005f

// ======================================================

// GYRO DEAD BAND

// ======================================================

// Nhiễu gyro Z nhỏ hơn mức này

// không tích phân vào Yaw

#define GYRO_Z_DEADBAND 0.08f

// ======================================================

// RAW DATA

// ======================================================

int16_t rawAx = 0;

int16_t rawAy = 0;

int16_t rawAz = 0;

int16_t rawGx = 0;

int16_t rawGy = 0;

int16_t rawGz = 0;

// ======================================================

// SENSOR DATA

// ======================================================

float ax = 0;

float ay = 0;

float az = 0;

float gx = 0;

float gy = 0;

float gz = 0;

// ======================================================

// OFFSET

// ======================================================

float offsetGx = 0;

float offsetGy = 0;

float offsetGz = 0;

// ======================================================

// FILTERED GYRO

// ======================================================

float filteredGx = 0;

float filteredGy = 0;

float filteredGz = 0;

// ======================================================

// ANGLES

// ======================================================

float roll = 0;

float pitch = 0;

float yaw = 0;

// ======================================================

// ACCEL

// ======================================================

float accelMagnitude = 0;

// ======================================================

// STILL STATUS

// ======================================================

bool sensorStill = false;

unsigned long stillStartTime = 0;

// ======================================================

// TIME

// ======================================================

unsigned long lastMicros = 0;

unsigned long startMillis = 0;

unsigned long lastPrint = 0;

// ======================================================

// WRITE REGISTER

// ======================================================

void writeMPU(uint8_t reg, uint8_t data)

{

Wire.beginTransmission(MPU_ADDR);

Wire.write(reg);

Wire.write(data);

Wire.endTransmission();

}

// ======================================================

// CHECK MPU6050

// ======================================================

bool checkMPU()

{

Wire.beginTransmission(MPU_ADDR);

byte error = Wire.endTransmission();

return (error == 0);

}

// ======================================================

// READ MPU6050

// ======================================================

bool readMPU()

{

Wire.beginTransmission(MPU_ADDR);

Wire.write(0x3B);

if (Wire.endTransmission(false) != 0)

{

return false;

}

Wire.requestFrom(

(uint8_t)MPU_ADDR,

(uint8_t)14,

(uint8_t)true

);

if (Wire.available() < 14)

{

return false;

}

// ==================================================

// ACCEL

// ==================================================

rawAx =

((int16_t)Wire.read() << 8)

| Wire.read();

rawAy =

((int16_t)Wire.read() << 8)

| Wire.read();

rawAz =

((int16_t)Wire.read() << 8)

| Wire.read();

// ==================================================

// TEMPERATURE

// Bỏ qua 2 byte

// ==================================================

Wire.read();

// ==================================================

// GYRO

// ==================================================

rawGx =

((int16_t)Wire.read() << 8)

| Wire.read();

rawGy =

((int16_t)Wire.read() << 8)

| Wire.read();

rawGz =

((int16_t)Wire.read() << 8)

| Wire.read();

// ==================================================

// CONVERT ACCEL

// ±2G

// 16384 LSB/G

// ==================================================

ax = rawAx / 16384.0f;

ay = rawAy / 16384.0f;

az = rawAz / 16384.0f;

// ==================================================

// CONVERT GYRO

// ±250 deg/s

// 131 LSB/(deg/s)

// ==================================================

gx = rawGx / 131.0f;

gy = rawGy / 131.0f;

gz = rawGz / 131.0f;

return true;

}

// ======================================================

// INITIALIZE MPU6050

// ======================================================

void initMPU()

{

// Wake up

writeMPU(

0x6B,

0x00

);

delay(100);

// Gyroscope ±250 deg/s

writeMPU(

0x1B,

0x00

);

// Accelerometer ±2G

writeMPU(

0x1C,

0x00

);

// Digital Low Pass Filter

// DLPF_CFG = 3

writeMPU(

0x1A,

0x03

);

delay(100);

}

// ======================================================

// INITIAL CALIBRATION

// ======================================================

void calibrateGyro()

{

Serial.println();

Serial.println(

"========================================"

);

Serial.println(

"INITIAL GYRO CALIBRATION"

);

Serial.println(

"========================================"

);

Serial.println();

Serial.println(

"GIU MPU6050 YEN TUYET DOI!"

);

Serial.println();

Serial.println(

"Warm-up 5 seconds..."

);

delay(WARMUP_TIME);

Serial.println(

"Measuring gyro offset..."

);

double sumX = 0;

double sumY = 0;

double sumZ = 0;

uint32_t count = 0;

for (

int i = 0;

i < CALIBRATION_SAMPLES;

i++

)

{

if (readMPU())

{

sumX += gx;

sumY += gy;

sumZ += gz;

count++;

}

delay(2);

}

if (count > 0)

{

offsetGx =

sumX / count;

offsetGy =

sumY / count;

offsetGz =

sumZ / count;

}

Serial.println();

Serial.println(

"========== INITIAL OFFSET =========="

);

Serial.print("GX = ");

Serial.println(

offsetGx,

);

Serial.print("GY = ");

Serial.println(

offsetGy,

);

Serial.print("GZ = ");

Serial.println(

offsetGz,

);

Serial.println(

"===================================="

);

Serial.println();

}

// ======================================================

// LOW PASS FILTER GYRO

// ======================================================

void updateGyroFilter(

float gyroX,

float gyroY,

float gyroZ

)

{

filteredGx =

filteredGx

(1.0f - FILTER_ALPHA)

gyroX

FILTER_ALPHA;

filteredGy =

filteredGy

(1.0f - FILTER_ALPHA)

gyroY

FILTER_ALPHA;

filteredGz =

filteredGz

(1.0f - FILTER_ALPHA)

gyroZ

FILTER_ALPHA;

}

// ======================================================

// DETECT STILL

// ======================================================

void detectStill()

{

// ==================================================

// ACCEL MAGNITUDE

// ==================================================

accelMagnitude =

sqrt(

ax * ax

ay * ay

az * az

);

// ==================================================

// CHECK GYRO

// ==================================================

bool gyroStill =

fabs(filteredGx)

< STILL_GYRO_THRESHOLD

fabs(filteredGy)

< STILL_GYRO_THRESHOLD

fabs(filteredGz)

< STILL_GYRO_THRESHOLD;

// ==================================================

// CHECK ACCEL

// ==================================================

bool accelStill =

accelMagnitude

> STILL_ACCEL_MIN

accelMagnitude

< STILL_ACCEL_MAX;

// ==================================================

// STILL TIMER

// ==================================================

if (

gyroStill

accelStill

)

{

if (stillStartTime == 0)

{

stillStartTime =

millis();

}

if (

millis()

stillStartTime

>= STILL_TIME

)

{

sensorStill =

true;

}

else

{

sensorStill =

false;

stillStartTime =

}

// ======================================================

// ADAPTIVE BIAS

// ======================================================

void updateAdaptiveBias()

{

if (!sensorStill)

{

return;

}

// ==================================================

// AUTO LEARN GYRO OFFSET

// ==================================================

offsetGx =

offsetGx

(1.0f - BIAS_ALPHA)

BIAS_ALPHA;

offsetGy =

offsetGy

(1.0f - BIAS_ALPHA)

BIAS_ALPHA;

offsetGz =

offsetGz

(1.0f - BIAS_ALPHA)

BIAS_ALPHA;

}

// ======================================================

// TRY CONNECT MPU6050

// ======================================================

bool connectMPU()

{

Serial.println(

"Dang tim MPU6050..."

);

for (

int attempt = 1;

attempt <= 10;

attempt++

)

{

Wire.beginTransmission(

MPU_ADDR

);

byte error =

Wire.endTransmission();

if (error == 0)

{

Serial.print(

"MPU6050 FOUND - attempt "

);

Serial.println(

attempt

);

return true;

}

Serial.print(

"Attempt "

);

Serial.print(

attempt

);

Serial.print(

" | I2C error = "

);

Serial.println(

error

);

delay(500);

}

return false;

}

// ======================================================

// SETUP

// ======================================================

void setup()

{

Serial.begin(115200);

delay(1000);

Serial.println();

Serial.println(

"========================================"

);

Serial.println(

"ESP32 + MPU6050 V3.1"

);

Serial.println(

"ADAPTIVE YAW DRIFT COMPENSATION"

);

Serial.println(

"========================================"

);

// ==================================================

// I2C

// ==================================================

Wire.begin(

SDA_PIN,

SCL_PIN

);

// Dùng 100kHz để ưu tiên ổn định

Wire.setClock(

100000

);

delay(500);

// ==================================================

// FIND MPU

// ==================================================

if (!connectMPU())

{

Serial.println();

Serial.println(

"MPU6050 NOT FOUND!"

);

Serial.println(

"Thu khoi dong lai I2C..."

);

Wire.end();

delay(500);

Wire.begin(

SDA_PIN,

SCL_PIN

);

Wire.setClock(

100000

);

delay(1000);

if (!connectMPU())

{

Serial.println();

Serial.println(

"MPU6050 VAN KHONG PHAN HOI!"

);

// ==========================================

// Không khóa ESP32 vĩnh viễn.

// Tiếp tục thử lại.

// ==========================================

while (true)

{

delay(1000);

Wire.beginTransmission(

MPU_ADDR

);

byte error =

Wire.endTransmission();

Serial.print(

"Retry MPU | error = "

);

Serial.println(

error

);

if (error == 0)

{

Serial.println(

"MPU6050 DA KET NOI LAI!"

);

delay(500);

ESP.restart();

}

// ==================================================

// INIT MPU

// ==================================================

initMPU();

// ==================================================

// CALIBRATION

// ==================================================

calibrateGyro();

// ==================================================

// RESET FILTER

// ==================================================

filteredGx = 0;

filteredGy = 0;

filteredGz = 0;

// ==================================================

// RESET ANGLES

// ==================================================

roll = 0;

pitch = 0;

yaw = 0;

// ==================================================

// RESET STILL

// ==================================================

sensorStill = false;

stillStartTime = 0;

// ==================================================

// TIMER

// ==================================================

startMillis =

millis();

lastMicros =

micros();

lastPrint =

millis();

Serial.println();

Serial.println(

"========================================"

);

Serial.println(

"START TEST"

);

Serial.println(

"Yaw = 0 degree"

);

Serial.println(

"Adaptive Bias = ENABLED"

);

Serial.println(

"========================================"

);

Serial.println();

}

// ======================================================

// LOOP

// ======================================================

void loop()

{

// ==================================================

// READ SENSOR

// ==================================================

if (!readMPU())

{

Serial.println(

"MPU READ ERROR"

);

delay(10);

return;

}

// ==================================================

// DELTA TIME

// ==================================================

unsigned long nowMicros =

micros();

float dt =

(nowMicros - lastMicros)

1000000.0f;

lastMicros =

nowMicros;

if (

dt <= 0

dt > 0.1

)

{

return;

}

// ==================================================

// REMOVE OFFSET

// ==================================================

float gyroX =

gx - offsetGx;

float gyroY =

gy - offsetGy;

float gyroZ =

gz - offsetGz;

// ==================================================

// FILTER

// ==================================================

updateGyroFilter(

gyroX,

gyroY,

gyroZ

);

// ==================================================

// DETECT STILL

// ==================================================

detectStill();

// ==================================================

// ADAPTIVE BIAS

// ==================================================

updateAdaptiveBias();

// ==================================================

// CALCULATE AGAIN

// ==================================================

gyroX =

gx - offsetGx;

gyroY =

gy - offsetGy;

gyroZ =

gz - offsetGz;

// ==================================================

// DEAD BAND Z

// ==================================================

if (

fabs(gyroZ)

GYRO_Z_DEADBAND

)

{

gyroZ = 0;

}

// ==================================================

// ROLL

// ==================================================

roll =

atan2(

ay,

)

180.0f

PI;

// ==================================================

// PITCH

// ==================================================

pitch =

atan2(

-ax,

sqrt(

ay * ay

az * az

)

180.0f

PI;

// ==================================================

// YAW

// ==================================================

yaw +=

gyroZ

dt;

// ==================================================

// NORMALIZE YAW

// -180 ... +180

// ==================================================

if (yaw > 180.0f)

{

yaw -= 360.0f;

}

if (yaw < -180.0f)

{

yaw += 360.0f;

}

// ==================================================

// SERIAL

// ==================================================

if (

millis()

lastPrint

PRINT_INTERVAL

)

{

lastPrint =

millis();

float timeSec =

(millis() - startMillis)

1000.0f;

Serial.print(

"Time:"

);

Serial.print(

timeSec,

);

Serial.print(

" | Yaw:"

);

Serial.print(

yaw,

);

Serial.print(

" | GX:"

);

Serial.print(

gyroX,

);

Serial.print(

" | GY:"

);

Serial.print(

gyroY,

);

Serial.print(

" | GZ:"

);

Serial.print(

gyroZ,

);

Serial.print(

" | FGZ:"

);

Serial.print(

filteredGz,

);

Serial.print(

" | Acc:"

);

Serial.print(

accelMagnitude,

);

Serial.print(

" | OffsetZ:"

);

Serial.print(

offsetGz,

);

Serial.print(

" | R:"

);

Serial.print(

roll,

);

Serial.print(

" | P:"

);

Serial.print(

pitch,

);

Serial.print(

" | "

);

if (sensorStill)

{

Serial.println(

"STILL"

);

}

else

{

Serial.println(

"MOVING"

);

}

Code test I2C cho ESP32 Dùng đoạn code Arduino IDE này để quét I2C trên ESP32. Với BNO086 của bạn: SDA = GPIO21, SCL = GPIO22. #include <Wire.h> #define SDA_PIN 21 #define SCL_PIN 22 void setup() { Serial.begin(115200); delay(1000); Serial.println(); Serial.println("=== E...

[Đọc tiếp...]

Nguyên lý hoạt động của LiDAR LD14 LD14 là LiDAR quét 360°. Nó không tự biết đâu là “phía trước robot”; ESP32 phải dựa vào góc đo + khoảng cách để xác định vùng vật cản. Mỗi vòng quay, LD14 liên tục trả về các điểm dạng: Góc θ + Khoảng cách D + cường độ tín hiệu Ví dụ nếu khi lắp cảm biến ta quy ...

[Đọc tiếp...]

MPU6050 bị trôi góc như thế nào?