Roll, Pitch e Yaw con MPU6050 – Arduino

Un corpo può ruotare nello spazio lungo 3 assi, facendo riferimento alla dinamica applicata ai velivoli si parla più propriamente di rollio (Roll), beccheggio (Pitch) e imbardata (Yaw). In questo nuovo articolo sull’accelerometro/giroscopio MPU6050 parlerò proprio di come sia possibile ricavare questi angoli (Roll, Pitch e Yaw) tramite Arduino con una semplice formula. Per la parte teorica vi rimando al magnifico articolo scritto da settorezero.com.

Con un accelerometro è possibile determinare gli angoli di rollio e beccheggio utilizzando le seguenti formule:

Angolo di beccheggio (Pitch)
Angolo di rollio (Roll)

Mentre per determinare l’angolo di imbardata (Yaw) sarà necessario utilizzare il giroscopio. Il giroscopio è utile per misurare la velocità angolare dei tre assi in gradi/sec. Acquisendo diversi valori della velocità angolare dell’asse Z del giroscopio per ogni periodo di 1 secondo sarà possibile ricavare i gradi di rotazione attorno all’asse Z. Di seguito viene mostrato il pezzetto di codice utilizzato per questo scopo.

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

Lista del materiale:

Breadboard

MPU6050ARDUINO
VCC+3.3V
GNDGND
SDAA4
SCLA5

Per ulteriori chiarimenti sui collegamenti da fare, vi consiglio di prendere visione del mio articolo sull’IMU MPU6050.

Ho scoperto da poco una libreria molto utile in grado di facilitare (e non di poco) l’utilizzo dell’MPU6050 ai meno esperti.

In seguito lo sketch (senza l’uso di librerie esterne) da caricare su Arduino, in questo caso verranno calcolati solo gli angoli Roll e Pitch.

Sketch senza l’uso della libreria

// Roll and Pitch with MPU6050
// https://www.giuseppecaccavale.it
// Giuseppe Caccavale

#include <SPI.h>
#include <Wire.h>
#define MPU 0x68  // I2C address of the MPU-6050

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

void setup() {
  Serial.begin(9600);
  init_MPU(); // Inizializzazione MPU6050
}

void loop() {
  FunctionsMPU(); // Acquisisco assi AcX, AcY, AcZ.

  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   //Calcolo angolo Roll
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);  //Calcolo angolo Pitch

  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);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}

//Funzione per il calcolo degli angoli Pitch e Roll
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;
}

//Funzione per l'acquisizione degli assi X,Y,Z del 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)
}

Aprendo il Monitor Seriale di Arduino è possibile consultare gli angoli di Rollio e Beccheggio.

Mentre chi è interessato ad utilizzare l’MPU6050 tramite una libreria, dovrà scaricare la libreria da qui ed inserirla in /Documents/Arduino/libraries/… e caricare il seguente sketch.

Sketch con l’uso della libreria

// Roll, Pitch and Yaw with MPU6050
// https://www.giuseppecaccavale.it
// Giuseppe Caccavale

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

MPU6050 mpu;

// Pitch, Roll and Yaw values
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("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }

  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
  mpu.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  mpu.setThreshold(1);

  // Check settings
  checkSettings();
}

void loop() {
  // Read normalized values
  Vector normAccel = mpu.readNormalizeAccel();
  Vector normGyro = mpu.readNormalizeGyro();

  // Calculate 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 the gyro if our angular velocity does not meet our threshold
  if (normGyro.ZAxis > 1 || normGyro.ZAxis < -1) {
    normGyro.ZAxis /= 100;
    yaw += normGyro.ZAxis;
  }

  //Keep our angle between 0-359 degrees
  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("Stops the clock and keeps the timing generator in reset"); 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();
}

Come si può notare dalla riga 20 è possibile impostare il fondo scala DPS e la sensibilità in G.

Per impostare il fondo scala in DPS basta sostituire MPU6050_SCALE_2000DPS con:

  • MPU6050_SCALE_2000DPS
  • MPU6050_SCALE_1000DPS
  • MPU6050_SCALE_500DPS
  • MPU6050_SCALE_250DPS

Mentre per quanto riguarda il range di sensibilità bisogna sostituire MPU6050_RANGE_2G con:

  • MPU6050_RANGE_2G
  • MPU6050_RANGE_4G
  • MPU6050_RANGE_8G
  • MPU6050_RANGE_16G

NB: Ho diviso il valore dell’asse z del giroscopio per 100 (e non per 1000) poichè ho aggiunto un ritardo di 10ms, dunque 1000/10 = 100.

Aprendo il monitor seriale si dovrebbero visualizzare i tre angoli. Per ora è tutto.

Per qualsiasi dubbio non esitate a lasciare un commento. Ciao a tutti a presto! 🙂