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:
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:
- Arduino UNO (o altro, leggi nota più in basso)
- Modulo GY-521
Breadboard
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’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! 🙂