(esp32/MPU6050_DMP6 라이브러리) 쉽게 사용하기

 

지금까지 테스트한 대여섯 종 MPU6050 라이브러리 중에서 정확도 면과 실용성 면에서 단연 앞서는 라이브러리는 MPU6050_DMP6 라이브러리다. Jeff Rowberg라는 독일 양반에게 경배!

MPU6050 by Jeff Rowberg 버전 설치는 아래 기사의 하단을 참고하기 바란다. eps32 및 아두이노 우노/나노 공통 사항.

 

https://madrabbit7.tistory.com/202

 

아두이노 우노에서 MPU6050 라이브러리 사용 시 freeze 현상 일어날 때

 

로봇이 균형을 유지하자면 roll, pitch 값을 참고하면 되고, 로봇카처럼 수평 상태에서  방향을 잡자면 yaw값을 참고할 수 있다. yaw값을 구하자면 자기센서가 필요한데, 이것 없이도 코딩의 힘만으로 yaw값을 최대한 구한 양반이 있으니 그가 이 독일 양반! 경배!

 

예제 소스인 MPU6050_DMP6를 들여다보면 제법 복잡해서, 이걸 어떻게 나 자신의 소스에 통합할지 고민스러워진다. 이걸 별도 파일로 빼고 함수로 뭉뚱거려서 간단하게 만들어보았다.

 

아래 파일을 MPU6050_setup.h 라는 이름으로 저장해서,  이 라이브러리의 헤드파일 있는 곳에 놔두고 include해서 사용하면 간편하다. 혹은 아래 첨부 파일을 내려받아서 그곳에 옮겨놓아도 된다.

 

MPU6050_setup.h

0.00MB

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

#define INTERRUPT_PIN   23

// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus;  // holds atctual 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]    quanternion container
VectorFloat gravity; // [x, y, z]       gravity vector
float equler[3];     // [psi, theta, phi]   Euler angle container
float ypr[3];        // [yaw, pitch, roll]  yaw/pitch/roll container and gravity vector

// INTERRUPT DETECTION ROUTINE
volatile bool mpuInterrupt = false;  // indicates wheter MPU interrupt pin has gone high
volatile unsigned int  counter_ = 0;

void dmpDataReady() {
  mpuInterrupt = true;
}

// INITIAL SETUP
void MPU6050_setup() {
  Wire.begin();
  Wire.setClock(400000);

  Serial.begin(115200);

  Serial.println("Initializing I2C devices...");
  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);

  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  Serial.println("Initializing DMP...");
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);  // 1688 factory default for my test chip

  // 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);
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    Serial.println("Enabling DMP...");
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.print("Enabling interrupt detection (Arduino external interrupt ");
    //Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
    Serial.println(")...");
    attachInterrupt(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("DMP ready! Waiting for first interrupt...");
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = inital memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print("DMP Initailization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }
}

void mpuDmpGet() {
  // if programming failed, don't try to do anything
  if (!dmpReady) return;
  // read a packet from FIFO
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Lastest Packet // uint8_t fifoBuffer[64];  // FIFO storage buffer
    // display Euler angles in degress
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  }
}

 

주의해야 할 점은, 어지간한 헤더파일 include를 여기서 다 하고 있고, setup() 에서 보통 하게 되는 함수들을 여기서 다 호출하고 있으므로, 중복 호출하지 않도록 주의해야 한다. 이렇게 해놓으면, 다음과 같은 간단한 사용이 가능하다.

#include "MPU6050_setup.h"

void setup() {
  MPU6050_setup();
}

void loop() {
    mpuDmpGet();

    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();
}

 덧붙이자면, 원 소스에서 인터럽트 핀은 아두이노 보드용으로 2번을 설정했는데 ESP32에서는 거의 모든 핀에서 인터럽트 설정을 지원하기 때문에, 필자는 편의상 23번을 설정했으니, 본인이 원하는 대로 설정하고 배선하면 된다. ESP32에서 2번 핀은 내장 LED on/off에 할당되어 있어서 인터럽트 설정에 적합하지 않다. 그리고 원 소스에서 내장 LED를 켜고 끄는 부분을 빼버렸고, 자이로 기능 및 기타 기능을 다 제외했으니 참고하기 바란다.