요 며칠새 블랜더도 손에 안잡히고
센서 다루는것도 손에 안잡혀서 계속 놀고 있었다.
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/
이 글이랑 영상 몇개보고
이해갔다.
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/
// 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
ypr 계산시 왜 gravity 백터를 사용하는지 궁금해서 구글링하다
국내에 mpu 사용한 글들 몇가지 찾아봄
https://blog.naver.com/ysahn2k/221410391235
https://m.blog.naver.com/yuninjae1234/220935189584
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 |