[벨런싱 로봇] Raspberry Pi 3에서 MPU6050 DMP 모드 이용하여 YPR 각도 측정
Small Devices/Raspberry Pi 2018. 6. 30. 15:21MPU6050 은 자이로센서와 가속도 센서가 결합된 센서이다. 이 센서에는 DMP(Digital Motion Processing) 모드가 있어서 상보필터를 이용해서 계산하지 않아도 Yaw/Pitch/Roll 각도를 구할 수 있다. Raspberry Pi에서 MPU6050의 DMP 모드는 C++ 코드로 이용 가능하다.
다음은 MPU6050 센서에서 값을 가져오는 코드가 있는 github 이다.
https://github.com/richardghirst/PiBits
이 코드는 I2Cdev 라이브러리가 포함되어 있는데, Raspberry Pi 3에서 이용하기 위해서는 I2cDev.cpp에서 /dev/i2c-0 를 /dev/i2c-1 로 바꿔주어야 한다.
git clone https://github.com/richardghirst/PiBits.git
여기에서 I2Cdev.cpp 에 /dev/i2c-0 를 찾아서 /dev/i2c-1로 바꿔주고 make 하게 되면 빌드가 된다. 빌드할떼 demo_3d 는 lilbgtkmm-3.0-dev 가 설치되어 있어야 하는데, 본 포스팅에서는 불필요해서 생략한다.
빌드되고 나면 demo_raw, demo_dmp가 생성되는데 이중 demo_dmp 가 dmp 모드로 센서에서 값을 읽는 것이다.
demo_dmp.cpp 에서 #define OUTPUT_READABLE_YAWPITCHROLL 을 주석 해제하고 나머지 OUTPUT_ 로 되어 있는 부분은 주석처리 한다.
컴파일 하고 실행하면 yaw, pitch roll 각도가 출력된다.
다음은 demo_dmp.cpp 에서 필요한 yaw,pitch,roll 각도만 출력되도록 정리한 코드이다.
#include <stdio.h> #include <stdlib.h> #include <unistd.h> #include <stdint.h> #include <string.h> #include <math.h> #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; uint16_t setupDMP() { uint16_t packetSize=42; // expected DMP packet size (default is 42 bytes) uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) printf("Initializing I2C devices...\n"); mpu.initialize(); // verify connection printf("Testing device connections...\n"); printf(mpu.testConnection() ? "MPU6050 connection successful\n" : "MPU6050 connection failed\n"); // load and configure the DMP printf("Initializing DMP...\n"); devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready printf("Enabling DMP...\n"); mpu.setDMPEnabled(true); //mpuIntStatus = mpu.getIntStatus(); //set our DMP Ready flag so the main loop() function knows it's okay to use it printf("DMP ready!\n"); // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); return packetSize; } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) printf("DMP Initialization failed (code %d)\n", devStatus); return -1; } } int main() { float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector float x_angle; float y_angle; float z_angle; VectorFloat gravity; // [x, y, z] gravity vector uint8_t fifoBuffer[64]; // FIFO storage buffer uint16_t fifoCount; // count of all bytes currently in FIFO Quaternion q; // [w, x, y, z] quaternion container uint16_t packetSize=setupDMP(); usleep(100000); printf("PacketSize:%d\n",packetSize); int loopFlag=1; while(loopFlag==1){ fifoCount = mpu.getFIFOCount(); if (fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); printf("FIFO overflow!\n"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (fifoCount >= 42) { // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // display Euler angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); x_angle=ypr[2]*180/M_PI; y_angle=ypr[1]*180/M_PI; z_angle=ypr[0]*180/M_PI; printf("x:%7.2f y:%7.2f z:%7.2f \n", x_angle,y_angle,z_angle); } }; return 0; } |
