요 며칠새 블랜더도 손에 안잡히고
센서 다루는것도 손에 안잡혀서 계속 놀고 있었다.
mpu6050 3개에서 ypr 가져오도록 하긴했는데
평평한 곳에 같은 방향으로 놓아도 yaw값이 다 다르게 나와서
mpu6050에 가속조 자이로는 없어도 지자기가 없다길래
mpu9250이라도 새로 사서 해야하나 좌절하고 있었다.
오랜만에 imu 가지고 트래킹하는걸 찾다가
내가 생각한거랑 비슷한걸 한 사람을 찾았다.
이 사람의 경우 블랜더에서 하는걸 보여주는데
계속 센서 위치를 어떻게 놓는게 좋을까 고민하던중에 보게되었다.
https://www.youtube.com/watch?v=F1IdRtIDdIs
더불어 소스코드도 같이 올려줬는데
이 친구는 ypr이 아닌 쿼터니언을 뽑아오도록 하더라
근데 찝찝한게 내 mpu6050은 같은 방향을 보도록 해도 yaw가 다르던데
쿼터니언으로 안한게 문제였을까?
mpu6050_dmp 코드를 보면
오일러, 쿼터니언, ypr, 가속도 등 출력해내도록 있던데
ypr이나 오일러각이랑 비슷한거 아닌가?
싶어서 구글링 좀하면서 해매다가
이글도 처음에 대충 지나가면서 이해가 안갔는데
https://taeyoung96.github.io/mathematics/Euler/
Euler Angle 과 Roll Pitch Yaw
Euler Angle(오일러 각) 과 Roll Pitch Yaw에 대해서 알아보자.
taeyoung96.github.io
이 글이랑 영상 몇개보고
이해갔다.
roll pitch yaw는 고정된 초기 축에 따라서 회전학 각도고
오일러각은 회전하면서 회전기준축도 이동한다.
위 국내 글에 그림을 잘보면 이 내용이 있는데 제대로 안봐서 못보고 지나갓엇다.
아무튼 내 mpu6050도 쿼터니언으로 뽑으면 좀 나으려나?
안그래도 이전에 만들면서도 오프셋 고쳐야할탠대 싶었는데
위 영상 만든사람도 오프셋을 고쳐서 하라고한다.
그런데 보다보니까
내가 갖고잇는 dmp 코드랑 뭔가 조금 다르더라.
// supply your own gyro offsets here, scaled for min sensitivity
mpu68.setXGyroOffset(0);
mpu68.setYGyroOffset(0);
mpu68.setZGyroOffset(0);
mpu68.setXAccelOffset(0);
mpu68.setYAccelOffset(0);
mpu68.setZAccelOffset(0);
mpu69.setXGyroOffset(0);
mpu69.setYGyroOffset(0);
mpu69.setZGyroOffset(0);
mpu69.setXAccelOffset(0);
mpu69.setYAccelOffset(0);
mpu69.setZAccelOffset(0);
// Calibration Time: generate offsets and calibrate our MPU6050
mpu68.CalibrateAccel(6);
mpu68.CalibrateGyro(6);
mpu69.CalibrateAccel(6);
mpu69.CalibrateGyro(6);
이게 2019년도에 업데이트된 최신 코드같은데
내가 사용한 mpu6050 라이브러리는
2013년에 만들어진 아주 오래된 코드를 기반으로 하고있었다 --..
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
최신 코드
체인지로그
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2019-07-10 - Uses the new version of the DMP Firmware V6.12
// - Note: I believe the Teapot demo is broken with this versin as
// - the fifo buffer structure has changed
// 2016-04-18 - Eliminated a potential infinite loop
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
3/6 작성
---------------------------------
3/12 작성
계속 개으름 피우다가 다시 시작
이 mpu6050 ypr 추출 코드 작성한 사람 깃헙을 보면
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
모션앱뒤에 20, 41, 612가 붙어있는데
이게 뭔가 보니
DMP 펌웨어 버전을 말하는듯하다.
https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/
MPU-6050 | TDK InvenSense
NOT RECOMMENDED FOR NEW DESIGNS MPU-6050 Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices The MPU-6050™ parts are the world’s first MotionTracking devices designed for the low power, low cost, and high-performance requirements of smart
invensense.tdk.com
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2019-07-10 - Uses the new version of the DMP Firmware V6.12
// - Note: I believe the Teapot demo is broken with this versin as
// - the fifo buffer structure has changed
일단 이 예제코드로 동작시켜봄
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps612.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(51);
mpu.setYGyroOffset(8);
mpu.setZGyroOffset(21);
mpu.setXAccelOffset(1150);
mpu.setYAccelOffset(-50);
mpu.setZAccelOffset(1060);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
Serial.println();
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180 / M_PI);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[2] * 180 / M_PI);
/*
mpu.dmpGetAccel(&aa, fifoBuffer);
Serial.print("\tRaw Accl XYZ\t");
Serial.print(aa.x);
Serial.print("\t");
Serial.print(aa.y);
Serial.print("\t");
Serial.print(aa.z);
mpu.dmpGetGyro(&gy, fifoBuffer);
Serial.print("\tRaw Gyro XYZ\t");
Serial.print(gy.x);
Serial.print("\t");
Serial.print(gy.y);
Serial.print("\t");
Serial.print(gy.z);
*/
Serial.println();
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense Teapot demo format:
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
잘 나오기는 한데 euler에서는 중력을 안쓰고 ypr 계산시에 중력을 사용하는 이유가 뭘까
실제 코드 dmpgetypr 들어가면 gravity 사용해서 계산하긴한다.
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180 / M_PI);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
if (packet == 0) packet = dmpPacketBuffer;
data[0] = ((packet[0] << 8) | packet[1]);
data[1] = ((packet[4] << 8) | packet[5]);
data[2] = ((packet[8] << 8) | packet[9]);
data[3] = ((packet[12] << 8) | packet[13]);
return 0;
}
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
int16_t qI[4];
uint8_t status = dmpGetQuaternion(qI, packet);
if (status == 0) {
q -> w = (float)qI[0] / 16384.0f;
q -> x = (float)qI[1] / 16384.0f;
q -> y = (float)qI[2] / 16384.0f;
q -> z = (float)qI[3] / 16384.0f;
return 0;
}
return status; // int16 return value, indicates error if this line is reached
}
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi
data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
return 0;
}
#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
#else
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if (gravity -> z < 0) {
if(data[1] > 0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}
#endif
위 계산에 대한 해설은 다음 링크에 있다.
https://oduerr.github.io/gesture/ypr_calculations.html
YawPitchRollCalculation
This document describes how the orientation of the IMU in terms of yaw,pitch, and roll is calculated from the information provided by the IMU (we use the MPU-9250). Calculations as done on the Arduino The result of the calculations in the Digital Motion Pr
oduerr.github.io
ypr 계산시 왜 gravity 백터를 사용하는지 궁금해서 구글링하다
국내에 mpu 사용한 글들 몇가지 찾아봄
https://blog.naver.com/ysahn2k/221410391235
[MEMS] MPU6050 + DMP6 기본동작
이번 블로그에서는 지난 상보필터에 대한 블로그에서 잠깐 언급한 Jeff Rowberg님의 DMP6 프로그램에 ...
blog.naver.com
https://m.blog.naver.com/yuninjae1234/220935189584
자이로센서 roll, pitch, yaw 데이터 처리 구조 (AHRS)
드론을 만들기위해 자이로 센서를 공부하면서 많은 예제들을 보고 공부하였지만 드문드문 있는 자료들을 머...
blog.naver.com
ypr 대신 그냥 쿼터니언과 오일러만 출력시켜보니
초기 시점 기준으로 회전한 만큼 각이 바뀐다.
중력벡터를 사용하면서 실제 중력 기준으로 각도를 변환해주는 거구나.
중력벡터를 안쓰고 하면 초기 위치 기준으로 값들이 출력된다.
왜 맨 위에 블렌더에서 실험한 사람은 yaw pitch roll을 안쓰고 바로 쿼터니언을 사용한것인지 이제 이해가 간다.
아래는 필요없는 부분 제거해서 좀더 간략하게 줄인 코드
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps612.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
//#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_EULER
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(51);
mpu.setYGyroOffset(8);
mpu.setZGyroOffset(21);
mpu.setXAccelOffset(1150);
mpu.setYAccelOffset(-50);
mpu.setZAccelOffset(1060);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
Serial.println();
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180 / M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180 / M_PI);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
Serial.print(ypr[2] * 180 / M_PI);
Serial.println();
#endif
}
}
'컴퓨터과학 > 언리얼' 카테고리의 다른 글
| 관성모션 - 15. ESP32 MPU6050 여러 개 언리얼 액터 제어 (0) | 2024.03.02 |
|---|---|
| 관성모션 - 14. ESP32 MPU6050 여러 개 YPR 블루투스로 가져오기 (0) | 2024.02.29 |
| 관성모션 - 13. ESP32 블루투스 + MPU6050 YPR + 보조 베터리 (0) | 2024.02.28 |
| 관성모션 - 12. ESP32 블루투스 사용해보기 (0) | 2024.02.28 |
| 관성모션 - 11. MPU6050 여러 개 써보기(MUX 없이) + 인터럽트 상태 살펴보기 (0) | 2024.02.28 |
