MPU6050(GY-521) 이해하기 / Angle Z 값이 제대로 나오지 않을 때(MPU 92/65, GY-6500도 작동함)
Tutorial: How to use the GY-521 module (MPU-6050 breakout board) with the Arduino Uno
This tutorial teaches how to use the GY-521 Module (MPU-6050 MEMS). The tutorial covers wiring as well as programming of the GY-521 module.
mschoeffler.com
여기에 나오는 코드를 실행시켜 보면, 다른 값들은 잘 나오는데, 유독 aZ값만 제대로 나오지 않는다. 기판을 움직여보면 다른 값들은 -16000에서 16000로 왔다갔다하는데, 유독 aZ만 18426 식으로 18000에서 움직일 요량을 하지 않는다.
알아 보니, aZ값은 다른 값들에 기초해 계산을 해야 한단다.
https://stackoverflow.com/questions/19436706/filter-z-angle-rotation-mpu-6050-arduino
여기에 보면 답변이 나와 있다.
모든 축들의 회전을 계산한 다음, 그것에 기초헤 Z축 회전값을 계산해야 한단다. 그리고 칩을 가만히 두어도 좌표값이 변하는 것은, 칩이 움직이지 않을 때의 출력값의 평균을 계산해서, 그 값을, 칩이 움직일 때의 출력값에서 빼줘야 한다고 한다. 이것이 칼리브레이션이라고 한다.
아래의 설명이 아주 좋은데, 여기에 나오는 코드를 실행시켜보면, 우리가 아는 360도 식으로 각도를 잘 출력해준다. registers의 값을 읽어오는 각도 관련 raw데이터는, 각도와 속도를 뽑아내는 것의 기반이 될 뿐이지, 그것 자체가 각도가 아님을 명심할 필요가 있다.
https://medium.com/@kavindugimhanzoysa/lets-work-with-mpu6050-gy-521-part1-6db0d47a35e6
Lets work with MPU6050 (GY-521) — Part1
In this part I am mainly discussing the basic functionalities of MPU6050 and how to get the angular velocity and angular position using…
medium.com
MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2
https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
#include <Wire.h>
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;
long gyroXCalli = 0, gyroYCalli = 0, gyroZCalli = 0;
long gyroXPresent = 0, gyroYPresent = 0, gyroZPresent = 0;
long gyroXPast = 0, gyroYPast = 0, gyroZPast = 0;
float rotX, rotY, rotZ;
float angelX = 0, angelY = 0, angelZ = 0;
long timePast = 0;
long timePresent = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
setUpMPU();
callibrateGyroValues();
timePresent = millis();
}
void loop() {
//Serial.print(" Accel (g)");
readAndProcessAccelData();
readAndProcessGyroData();
printData();
delay(100);
}
void setUpMPU() {
// power management
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x6B); // Access the power management register
Wire.write(0b00000000); // Set sleep = 0
Wire.endTransmission(); // End the communication
// configure gyro
Wire.beginTransmission(0b1101000);
Wire.write(0x1B); // Access the gyro configuration register
Wire.write(0b00000000);
Wire.endTransmission();
// configure accelerometer
Wire.beginTransmission(0b1101000);
Wire.write(0x1C); // Access the accelerometer configuration register
Wire.write(0b00000000);
Wire.endTransmission();
}
void callibrateGyroValues() {
for (int i=0; i<5000; i++) {
getGyroValues();
gyroXCalli = gyroXCalli + gyroXPresent;
gyroYCalli = gyroYCalli + gyroYPresent;
gyroZCalli = gyroZCalli + gyroZPresent;
}
gyroXCalli = gyroXCalli/5000;
gyroYCalli = gyroYCalli/5000;
gyroZCalli = gyroZCalli/5000;
}
void readAndProcessAccelData() {
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000,6);
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read();
accelY = Wire.read()<<8|Wire.read();
accelZ = Wire.read()<<8|Wire.read();
processAccelData();
}
void processAccelData() {
gForceX = accelX/16384.0;
gForceY = accelY/16384.0;
gForceZ = accelZ/16384.0;
}
void readAndProcessGyroData() {
gyroXPast = gyroXPresent; // Assign Present gyro reaging to past gyro reading
gyroYPast = gyroYPresent; // Assign Present gyro reaging to past gyro reading
gyroZPast = gyroZPresent; // Assign Present gyro reaging to past gyro reading
timePast = timePresent; // Assign Present time to past time
timePresent = millis(); // get the current time in milli seconds, it is the present time
getGyroValues(); // get gyro readings
getAngularVelocity(); // get angular velocity
calculateAngle(); // calculate the angle
}
void getGyroValues() {
Wire.beginTransmission(0b1101000); // Start the communication by using address of MPU
Wire.write(0x43); // Access the starting register of gyro readings
Wire.endTransmission();
Wire.requestFrom(0b1101000,6); // Request for 6 bytes from gyro registers (43 - 48)
while(Wire.available() < 6); // Wait untill all 6 bytes are available
gyroXPresent = Wire.read()<<8|Wire.read(); // Store first two bytes into gyroXPresent
gyroYPresent = Wire.read()<<8|Wire.read(); // Store next two bytes into gyroYPresent
gyroZPresent = Wire.read()<<8|Wire.read(); //Store last two bytes into gyroZPresent
}
void getAngularVelocity() {
rotX = gyroXPresent / 131.0;
rotY = gyroYPresent / 131.0;
rotZ = gyroZPresent / 131.0;
}
void calculateAngle() {
// same equation can be written as
// angelZ = angelZ + ((timePresentZ - timePastZ)*(gyroZPresent + gyroZPast - 2*gyroZCalli)) / (2*1000*131);
// 1/(1000*2*131) = 0.00000382
// 1000 --> convert milli seconds into seconds
// 2 --> comes when calculation area of trapezium
// substacted the callibated result two times because there are two gyro readings
angelX = angelX + ((timePresent - timePast)*(gyroXPresent + gyroXPast - 2*gyroXCalli)) * 0.00000382;
angelY = angelY + ((timePresent - timePast)*(gyroYPresent + gyroYPast - 2*gyroYCalli)) * 0.00000382;
angelZ = angelZ + ((timePresent - timePast)*(gyroZPresent + gyroZPast - 2*gyroZCalli)) * 0.00000382;
}
void printData() {
Serial.print("Gyro (deg/sec)");
Serial.print(" X=");
Serial.print(rotX);
Serial.print(" Y=");
Serial.print(rotY);
Serial.print(" Angle (deg)");
Serial.print(" Z=");
Serial.print(rotZ);
//Serial.println("Angular displacement wrt started position (deg)");
Serial.print(" X=");
Serial.print(angelX);
Serial.print(" Y=");
Serial.print(angelY);
Serial.print(" Z=");
Serial.print(angelZ);
Serial.print(" Acceleration (g)");
Serial.print(" X=");
Serial.print(gForceX);
Serial.print(" Y=");
Serial.print(gForceY);
Serial.print(" Z=");
Serial.println(gForceZ);
}
MPU92/65도 저렴한 맛에 샀는데 헐~ 이것도 MPU6050 코드가 아무런 이상없이 잘 돌아가네요. register map 메뉴얼을 보니, MPU6050과 데이터 레지스터는 동일해 보이는데, 데이터 통신에는 별 차이가 없고, 핀 수가 더 많아서 거기서 비롯된 기능상 차이가 좀 있는가 보다. MPU6050이 2천원 정도, MPU92/65가 천원 남짓하는데, 이거 봉잡았네...
한줄에 결과가 다 출력되도록 원래 소스를 조금만 수정했는데, 결과는 MPU92/65와 MPU6050이 동일해 보인다. 칩을 장착한 브레드 보드를 움직이면 가운데의 X, Y, Z값이 변한다. roll, pitch, yaw 순서든가... 아무튼...
(흥분을 가라앉히고 두어 시간 동안 침착하게 확인해보니, 저렴이 MPU92/65는 GY6500 (MPU6500) 6축 센서이고, 위의 RAW 데이타와 가공 데이터 추출 코드는, 의미는 있으나 실용적이지는 않아서 상당히 부정확하다. 이런저런 라이브러리로 테스트해보고 있는데, 현재로선 MPU6050_tockn이 제법 쓸만하다. angleX, angleY는 확실히 잘 뽑아내고 흔들림이 없다. 다만 angleZ는 몇 번 회전하다보면 전혀 엉뚱한 값을 내놓는데, 라이브러리 제작자의 말을 들어보니, 6축 센서에서 angleZ를 정확하게 뽑아낼 방법은 없다고 한다. 보다 정확한 값을 원한다면 magnetic 기능이 있는 9축 센서를 사용하라고 한다. 가령 MPU9265... 가격은 만원이 넘어간다. 수평을 잡는 선에서 만족해야겠다. 수평 잡은 상태에서 방향까지 잡기를 바라는 것은 욕심인 듯... MPU6050에 나침반을 달면 방향을 알 수 있지 않을까? 그게 magnetic을 갖춘 9축 센서를 말하는 듯하다.
