Roll, Pitch e Yaw com MPU6050 – Arduino

Publicado em 24/05/2021 por cacpnrj

Tempo de leitura: 4 minutes

Um corpo pode girar no espaço ao longo de 3 eixos, referindo-se à dinâmica aplicada a aeronaves, falamos mais propriamente de roll (Roll), pitch (Pitch) e yaw (Yaw). Neste novo artigo sobre o acelerômetro/giroscópio MPU6050 irei falar sobre como é possível obter esses ângulos (Roll, Pitch e Yaw) usando o Arduino com uma fórmula simples.

Com um acelerômetro, os ângulos de rotação e inclinação podem ser determinados usando as seguintes fórmulas:

Ângulo de inclinação

Ângulo de rotação

Já para determinar o ângulo de guinada (Yaw) será necessário usar o giroscópio. O giroscópio é útil para medir a velocidade angular dos três eixos em graus/seg. Ao adquirir diferentes valores da velocidade angular do eixo Z do giroscópio para cada período de 1 segundo, será possível obter os graus de rotação em torno do eixo Z. O código utilizado para este fim é mostrado a seguir .

if (GyroAsseZ > 1 || GyroAsseZ < -1) {
  GyroAsseZ  /= 1000;
  yaw += GyroAsseZ;
}
delay(1);

Conteudo

Lista de materiais:

Arduino UNO (ou outro, veja a nota abaixo)
Módulo GY-521

ProtoBoard

MPU6050	ARDUINO
VCC	+3.3V
GND	GND
SDA	A4
SCL	A5

Para maiores esclarecimentos sobre as conexões a serem feitas, recomendo que você leia meu artigo sobre a IMU MPU6050.

Recentemente descobri uma biblioteca muito útil que pode facilitar (e não um pouco) o uso do MPU6050 para os menos experientes.

Em seguida, o sketch (sem o uso de bibliotecas externas) a ser carregado no Arduino, neste caso apenas os ângulos de Roll e Pitch serão calculados.

Desenhe sem usar a biblioteca

// Roll e Pitch com MPU6050

#include <SPI.h>
#include <Wire.h>
#define MPU 0x68  //Endereço I2C do MPU-6050

double AcX, AcY, AcZ;
int Pitch, Roll;

void setup() {
  Serial.begin(9600);
  init_MPU(); //Inicialização MPU6050
}

void loop() {
  FunctionsMPU(); // Eu adquiro eixos AcX, AcY, AcZ.

  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   //Cálculo do ângulo de rotação
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);  //Cálculo do ângulo de inclinação

  Serial.print("Pitch: "); Serial.print(Pitch);
  Serial.print("\t");
  Serial.print("Roll: "); Serial.print(Roll);
  Serial.print("\n");

}

void init_MPU() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // definido como zero (ativa o MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}

// Função para calcular os ângulos de inclinação e rotação
double FunctionsPitchRoll(double A, double B, double C) {
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B * B) + (C * C);
  DatoB = sqrt(DatoB);

  Value = atan2(DatoA, DatoB);
  Value = Value * 180 / 3.14;

  return (int)Value;
}

// Função para aquisição dos eixos X, Y, Z do MPU6050
void FunctionsMPU() {
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
}

Ao abrir o Arduino Serial Monitor é possível consultar os ângulos de Roll e Pitch.

Já os interessados em usar o MPU6050 por meio de uma biblioteca, terão que baixar a biblioteca aqui e inseri-la em /Documents/Arduino/libraries/ … e fazer o upload do seguinte sketch.

Sketch com o uso da biblioteca

// Roll, Pitch e Yaw com MPU6050

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// Valores de pitch, roll e yaw
int pitch = 0;
int roll = 0;
float yaw = 0;

void setup() {
  Serial.begin(115200);

  Serial.println("Initialize MPU6050");

  while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) {
    Serial.println("Não foi possível encontrar um sensor MPU6050 válido, verifique a ligação!");
    delay(500);
  }

  // Calibrar o giroscópio. A calibração deve estar em repouso.
  // Se você não quiser calibrar, comente esta linha.
  mpu.calibrateGyro();

  // Defina a sensibilidade do limite. Padrão 3.
  // Se você não quiser usar o limite, comente esta linha ou defina 0.
  mpu.setThreshold(1);

  // Verifique as configurações
  checkSettings();
}

void loop() {
  // Lê valores normalizados
  Vector normAccel = mpu.readNormalizeAccel();
  Vector normGyro = mpu.readNormalizeGyro();

  // Calcular Pitch & Roll
  pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis * normAccel.YAxis + normAccel.ZAxis * normAccel.ZAxis)) * 180.0) / M_PI;
  roll = (atan2(normAccel.YAxis, normAccel.ZAxis) * 180.0) / M_PI;

  // Ignore o giroscópio se nossa velocidade angular não atingir nosso limite
  if (normGyro.ZAxis > 1 || normGyro.ZAxis < -1) {
    normGyro.ZAxis /= 100;
    yaw += normGyro.ZAxis;
  }

  // Mantenha nosso ângulo entre 0-359 graus
  if (yaw < 0)
    yaw += 360;
  else if (yaw > 359)
    yaw -= 360;

  // Output
  Serial.print("Pitch = ");
  Serial.print(pitch);
  Serial.print("\tRoll = ");
  Serial.print(roll);
  Serial.print("\tYaw = ");
  Serial.print(yaw);

  Serial.println();

  delay(10);
}

void checkSettings() {
  Serial.println();

  Serial.print(" * Sleep Mode:        ");
  Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");

  Serial.print(" * Clock Source:      ");
  switch (mpu.getClockSource()) {
    case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Para o relógio e mantém o gerador de cronometragem reiniciado"); break;
    case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
    case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
    case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
    case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
  }

  Serial.print(" * Gyroscope:         ");
  switch (mpu.getScale()) {
    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
    case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
    case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
  }

  Serial.print(" * Gyroscope offsets: ");
  Serial.print(mpu.getGyroOffsetX());
  Serial.print(" / ");
  Serial.print(mpu.getGyroOffsetY());
  Serial.print(" / ");
  Serial.println(mpu.getGyroOffsetZ());

  Serial.println();
}

Como pode ser visto na linha 20, é possível definir a escala total do DPS e a sensibilidade em G.

Para definir a escala total no DPS, basta substituir MPU6050_SCALE_2000DPS por: