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 dati (Roll, Pitch e Yaw) tramite Arduino con una semplice formula. Per la parte teorica vi rimando al magnifico articolo scritto da settorezero.com.

inclinazione_spazio

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

Angolo di beccheggio (Pitch)formula10

Angolo di rollio (Roll)formula111

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. Dopo aver acquisito il valore della velocità angolare di un asse, basterà dividere il valore per 1000 (1000ms = 1 secondo ) e sommare il risultato in una variabile ogni volta che si verifica un  piccolo spostamento del giroscopio. La formula finale sarà così:

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

Schema

GY-521_BreadBoard_Arduino

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

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

Ho scoperto da poco una libreria molto utile in grado di facilitare (e non di poco) l’utilizzo del 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 without lib

// Roll and Pitch with MPU6050
// http://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, 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 lo sketch qui in basso. 

Sketch with lib

// Roll, Pitch and Yaw with MPU6050
// http://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 21 è possibile settare la scala DPS e la sensibilità in G.

Per settare la scala 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 gyro 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! 🙂