Sensor inersia MPU9250 adalah salah satu komponen elektronik yang populer digunakan dalam berbagai proyek yang melibatkan pengukuran gerakan dan orientasi. Dalam artikel ini, kita akan membahas cara ukur sudut menggunakan sensor MPU9250 dengan bantuan Arduino. Kami akan memandu Anda melalui kode program dasar dan penjelasan singkat tentang setiap langkahnya.
Sensor MPU9250 adalah sensor inersia 9-DOF (Degrees of Freedom) yang mengintegrasikan akselerometer, giroskop, dan magnetometer. Dalam proyek ini, kita akan menggunakan data dari giroskop dan akselerometer untuk mengukur sudut roll dan pitch dari perangkat. Roll mengukur kemiringan sepanjang sumbu X, sementara pitch mengukur kemiringan sepanjang sumbu Y.
Persiapan
Sebelum kita mulai, pastikan Anda memiliki komponen berikut:
- Arduino board (contoh: Arduino Uno)
- Sensor MPU9250
- Kabel jumper
- Komputer dengan Arduino IDE terinstal
Instalasi Library
Pertama, pastikan Anda telah menginstal library yang diperlukan untuk komunikasi dengan sensor MPU9250. Anda dapat mengunduh library “MPU9250” dari Library Manager di Arduino IDE.
Sambungkan Perangkat
Sambungkan sensor MPU9250 ke Arduino menggunakan kabel jumper. Hubungkan kabel VCC ke pin 3.3V, kabel GND ke pin GND, kabel SDA ke pin A4, dan kabel SCL ke pin A5 pada Arduino.
Kode Program Tipe Pertama
Buka Arduino IDE dan buat sketch baru. Salin kode program di bawah ini ke dalam sketch:
#include "quaternionFilters.h"
#include "MPU9250.h"
#define SerialDebug true
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1
MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
float AngleRoll, AnglePitch;
float RateRoll, RatePitch;
float RateCalibrationRoll, RateCalibrationPitch;
float KalmanAngleRoll = 0, KalmanUncertaintyAngleRoll = 2 * 2;
float KalmanAnglePitch = 0, KalmanUncertaintyAnglePitch = 2 * 2;
float Kalman1DOutput[] = {0, 0};
uint32_t LoopTimer;
void setup() {
Wire.begin();
Serial.begin(115200);
while (!Serial) {};
byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
if (c == 0x71)
{
Serial.println(F("MPU9250 is online..."));
myIMU.MPU9250SelfTest(myIMU.selfTest);
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
myIMU.initMPU9250();
myIMU.getAres();
myIMU.getGres();
} else {
Serial.print("Could not connect to MPU9250: 0x");
Serial.println(c, HEX);
Serial.println(F("Communication failed, abort!"));
Serial.flush();
abort();
}
}
void loop() {
gyro_signals();
RateRoll -= RateCalibrationRoll;
RatePitch -= RateCalibrationPitch;
kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
KalmanAngleRoll = Kalman1DOutput[0];
KalmanUncertaintyAngleRoll = Kalman1DOutput[1];
kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
KalmanAnglePitch = Kalman1DOutput[0];
KalmanUncertaintyAnglePitch = Kalman1DOutput[1];
Serial.print("Roll Angle [°] ");
Serial.print(KalmanAngleRoll);
Serial.print(" Pitch Angle [°] ");
Serial.println(KalmanAnglePitch);
while (micros() - LoopTimer < 4000);
LoopTimer = micros();
}
void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
KalmanState = KalmanState + 0.004 * KalmanInput;
KalmanUncertainty = KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
float KalmanGain = KalmanUncertainty * 1 / (1 * KalmanUncertainty + 3 * 3);
KalmanState = KalmanState + KalmanGain * (KalmanMeasurement - KalmanState);
KalmanUncertainty = (1 - KalmanGain) * KalmanUncertainty;
Kalman1DOutput[0] = KalmanState;
Kalman1DOutput[1] = KalmanUncertainty;
}
void gyro_signals() {
myIMU.readAccelData(myIMU.accelCount); // Read the x/y/z adc values
myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes; // - myIMU.accelBias[0];
myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes; // - myIMU.accelBias[1];
myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes; // - myIMU.accelBias[2];
myIMU.readGyroData(myIMU.gyroCount); // Read the x/y/z adc values
myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
myIMU.updateTime();
MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az, myIMU.gx * DEG_TO_RAD,
myIMU.gy * DEG_TO_RAD, myIMU.gz * DEG_TO_RAD, myIMU.my,
myIMU.mx, myIMU.mz, myIMU.deltat);
RateRoll = myIMU.gx / 65;
RatePitch = myIMU.gy / 65;
AngleRoll = atan(myIMU.ay / sqrt(myIMU.ax * myIMU.ax + myIMU.az * myIMU.az)) * 1 / (3.142 / 180);
AnglePitch = -atan(myIMU.ax / sqrt(myIMU.ay * myIMU.ay + myIMU.az * myIMU.az)) * 1 / (3.142 / 180);
Serial.println();
}Kode Program Tipe Kedua
Kode program kedua ini lebih responsife dan cepat.
#include "quaternionFilters.h"
#include "MPU9250.h"
#define SerialDebug true
#define I2Cclock 400000
#define I2Cport Wire
#define MPU9250_ADDRESS MPU9250_ADDRESS_AD0
//#define MPU9250_ADDRESS MPU9250_ADDRESS_AD1
MPU9250 myIMU(MPU9250_ADDRESS, I2Cport, I2Cclock);
float KalmanStateRoll = 0.0;
float KalmanGainRoll = 0.05;
float KalmanStatePitch = 0.0;
float KalmanGainPitch = 0.05;
float gyroRateRoll = 0.0;
float gyroRatePitch = 0.0;
float accelAngleRoll = 0.0;
float accelAnglePitch = 0.0;
float dt = 0.01; // Time interval (adjust as needed)
void setup() {
Wire.begin();
Serial.begin(115200);
while (!Serial) {};
byte c = myIMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
if (c == 0x71) {
Serial.println(F("MPU9250 is online..."));
myIMU.MPU9250SelfTest(myIMU.selfTest);
myIMU.calibrateMPU9250(myIMU.gyroBias, myIMU.accelBias);
myIMU.initMPU9250();
myIMU.getAres();
myIMU.getGres();
} else {
Serial.print("Could not connect to MPU9250: 0x");
Serial.println(c, HEX);
Serial.println(F("Communication failed, abort!"));
Serial.flush();
abort();
}
KalmanStateRoll = 0.0;
KalmanStatePitch = 0.0;
}
void loop() {
// Read sensor data
readAccel(); // Read accelerometer data
readGyro(); // Read gyro rate data
// Calculate predicted state using rate of change (gyroscope)
KalmanStateRoll += gyroRateRoll * dt;
KalmanStatePitch += gyroRatePitch * dt;
// Update Kalman Gain based on rotation rate
KalmanGainRoll = KalmanGainRoll / (KalmanGainRoll + gyroRateRoll * dt);
KalmanGainPitch = KalmanGainPitch / (KalmanGainPitch + gyroRatePitch * dt);
// Calculate Kalman-filtered angles
float KalmanAngleRoll = KalmanStateRoll + KalmanGainRoll * (accelAngleRoll - KalmanStateRoll);
float KalmanAnglePitch = KalmanStatePitch + KalmanGainPitch * (accelAnglePitch - KalmanStatePitch);
// Print the filtered angles
Serial.print("Roll Angle: ");
Serial.print(KalmanAngleRoll);
Serial.print("°, Pitch Angle: ");
Serial.print(KalmanAnglePitch);
Serial.println("°");
delay(dt * 1000); // Delay for the time interval
}
void readAccel() {
int16_t accelCount[3];
myIMU.readAccelData(accelCount);
accelAngleRoll = atan((float)accelCount[1] / sqrt(pow((float)accelCount[0], 2) + pow((float)accelCount[2], 2))) * 180.0 / PI;
accelAnglePitch = atan(-1 * (float)accelCount[0] / sqrt(pow((float)accelCount[1], 2) + pow((float)accelCount[2], 2))) * 180.0 / PI;
}
void readGyro() {
int16_t gyroCount[3];
myIMU.readGyroData(gyroCount);
gyroRateRoll = (float)gyroCount[0] * myIMU.gRes;
gyroRatePitch = (float)gyroCount[1] * myIMU.gRes;
}Setelah memasukkan kode program di atas ke dalam sketch Arduino IDE, pastikan Anda telah memilih papan Arduino yang sesuai dan port COM yang benar. Kemudian, unggah (upload) kode program ke papan Arduino Anda.
Setelah mengunggah kode program, buka Serial Monitor di Arduino IDE. Anda akan melihat data sudut roll dan pitch yang diukur dari sensor MPU9250. Pastikan baud rate pada Serial Monitor diatur ke 115200.
Sumber dasar perhitungan dari Carbon Aeronautics.
