본문 바로가기
컴퓨터/Linux

라즈베리파이와 MPU6050

JSFamily 2014. 12. 11. 17:32



한 3개월간 해봤던 프로젝트에서 사용한 방법이다.


라즈베리파이 GPIO의 I2C통신을 통해서(SCL, SDA)  받아오는데


아쉽게도 DMP까진 구현하질 못했다.


센서값을 통해서 상보필터를 씌워서 구현하는 정도로 만족해서 사용했다..


뭔가 더 하고 싶었는데..


다른걸 구현하느라 시간도 없다보니 그냥 넘어갔다.





http://ozzmaker.com/2013/04/29/guide-to-interfacing-a-gyro-and-accelerometer-with-a-raspberry-pi/



위의 링크에서 많은 도움을 얻었다.


다만 위의 링크는 다른 센서를 이용했기에


저 소스를 기반으로 MPU6050에 맞게 컨버팅 해봤다.




그리고 밑의 소스를 바탕으로 구현하려면 여기에 올려 놓은 i2c-dev,h 파일이 필요하다.

#include "i2c-dev.h"


i2c-dev.h



i2c-dev.h 파일을 들여다 보고 잘 연구해 보면 소스를 더 좋게 만들 수 있을 것 같은데


뭐 시간 때문에.... ㅠㅠ




또한 당연히 GPIO를 활용하기 때문에 wiringPi 라이브러리는 필수로 설치해야 한다.







==================== Device 장치 Open ===================



1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#define MPU6050_ADDR                    0x68
#define MPU6050_PWR_MGMT_1        0x6B
#define MPU6050_GYRO_CONFIG        0x1B
#define MPU6050_ACCEL_CONFIG    0x1C
#define MPU6050_CONFIG            0x1A
#define MPU6050_SMPRT_DIV        0x19
 
int fd_IMU;
 
void enableIMU(){
        __u16 block[I2C_SMBUS_BLOCK_MAX];
        int res, bus, size;
        char filename[20];
        sprintf(filename, "/dev/i2c-%d", 1);
        fd_IMU = open(filename, O_RDWR);
        if(fd_IMU < 0){
                printf("Unable to open I2C bus!\n");
                exit(1);
        }
        writeReg (MPU6050_PWR_MGMT_1, 0b00000000);
                resetReg (MPU6050_CONFIG, 0b11000000);
        writeReg2(MPU6050_CONFIG, 0b00000010);
        resetReg (MPU6050_SMPRT_DIV, 0b00000000);
        writeReg2(MPU6050_SMPRT_DIV, 0b00000011);    // SMPRT_DIV = 3
 
    /************************************************
        Sample Rate = Gyroscope Output Rate / ( 1 + MPU6050_SMPRT_DIV )
                if DLPF EN : Gyroscope Output Rate = 1Khz
                else        : Gyroscope Output Rate = 8Khz
                
                LOOP_CYCLE = 3.0ms   =>   333hz
                333hz = 1000hz / ( 1 + x )
                SMPRT_DIV : 2  => 3 >= max LOOP_CYCLE
        *******************************************/
}    
cs






================== Register Read 함수와 Write, Write2함수 ============


Write함수는 그냥 덮어쓰는 함수

Write2함수는 읽어온 후 원하는 비트만 셋 해서 다시 쓰는 함수

또 안에 보면 Block단위로 읽어오는 함수도 있음.


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
void resetReg(uint8_t reg, uint8_t reset_reg){
        uint8_t read_reg;
        readBlock(reg, sizeof(read_reg), &read_reg);
        printf("RESET Read Reg : %x\n", read_reg);
        reset_reg &= read_reg;
         writeReg(reg, reset_reg);
        readBlock(reg, sizeof(read_reg), &read_reg);
        printf("RESET Changed Reg : %x\n", read_reg);
}
void writeReg2(uint8_t reg, uint8_t write_reg){
        uint8_t read_reg;
        readBlock(reg, sizeof(read_reg), &read_reg);
        printf("WRITE Read Reg : %x\n", read_reg);
        write_reg |= read_reg;
        writeReg(reg, write_reg);
        readBlock(reg, sizeof(read_reg), &read_reg);
        printf("WRITE Changed Reg : %x\n", read_reg);
}
void writeReg(uint8_t reg, uint8_t value){
        if(ioctl(fd_IMU, I2C_SLAVE, MPU6050_ADDR) < 0 ){
                fprintf(stderr, "Error : Could not select device %s\n", strerror(errno));
                exit(1);
        }
        int result = i2c_smbus_write_byte_data(fd_IMU, reg, value);
        if(result == -1){
                printf("Failed to write byte to I2C Acc\n");
                exit(1);
        }
}
void readBlock(uint8_t command, uint8_t size, uint8_t *data){
        int result = i2c_smbus_read_i2c_block_data(fd_IMU, command, size, data);
        if(result != size){
                printf("Failed to read block from I2C\n");
                exit(1);
        }
}
cs







======================== 센서값 읽어오기 ============


1
2
3
4
5
6
7
8
9
10
11
12
13
14
void readACC(int *acc){
        uint8_t block[6];
        readBlock(MPU6050_ACCEL_XOUT_H, sizeof(block), block);
        *acc        = (int16_t) (block[0] << 8 | block[1]);
        *(acc+1)    = (int16_t) (block[2] << 8 | block[3]);
        *(acc+2)     = (int16_t) (block[4] << 8 | block[5]);
}
void readGYR(int *gyr){
        uint8_t block[6];
        readBlock( MPU6050_GYRO_XOUT_H, sizeof(block), block);
        *gyr        = (int16_t) (block[0] << 8 | block[1]) >> 4;
        *(gyr+1)     = (int16_t) (block[2] << 8 | block[3]) >> 4;
        *(gyr+2)     = (int16_t) (block[4] << 8 | block[5]) >> 4;
}
cs






====================== 센서값 계산부 ========================================


센서값 읽어들인 후 LOOP_CYCLE만큼 Delay 하여

그 Delay한 시간을 Gyroscope 값을 계산할 dt로 활용한다.


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
#define G_GAIN        0.07
#define RAD_TO_DEG     57.29578
#define M_PI2        3.141592
 
int *Pacc_raw, *Pgyr_raw;
int acc_raw[3], gyr_raw[3];
unsigned int startTime, finishTime;float CfXangle=0.0, CfYangle=0.0;
float rate_gyr_x=0.0, rate_gyr_y=0.0, rate_gyr_z=0.0;
float GyroXangle=0.0, GyroYangle=0.0, GyroZangle=0.0;
float AccXangle=0.0, AccYangle=0.0;
float term = 0, Pterm;
 
if(wiringPiSetupGpio() == -1){
    printf("GPIO Setup Failed\n");
    return;
}
 
while(1){
    startTime = micros();
    readACC(Pacc_raw);
    readGYR(Pgyr_raw);
    rate_gyr_x = (float) *gyr_raw    * G_GAIN;
    rate_gyr_y = (float) *(gyr_raw+1)    * G_GAIN;
    rate_gyr_z = (float) *(gyr_raw+2)     * G_GAIN;
 
    GyroXangle += rate_gyr_x * (Pterm/1000);
    GyroYangle += rate_gyr_y * (Pterm/1000);
    GyroZangle += rate_gyr_z * (Pterm/1000);
 
    AccXangle = (float) (atan2(*(acc_raw+1), *(acc_raw+2)) + M_PI2) * RAD_TO_DEG;
    AccYangle = (float) (atan2(*(acc_raw+2), *acc_raw) + M_PI2) * RAD_TO_DEG;
    AccXangle -= (float) 180 + XANGLE_OFFSET;
    AccYangle -= (float) 270 + YANGLE_OFFSET;
 
    CfXangle = AA * (CfXangle + rate_gyr_x * (Pterm/1000)) + (1 - AA) * AccXangle;
    CfYangle = AA * (CfYangle + rate_gyr_y * (Pterm/1000)) + (1 - AA) * AccYangle;
 
    while( term < LOOP_CYCLE ){
        term = (float) (micros() - startTime) / 1000;
        delayMicroseconds(100);
        if( term >= LOOP_CYCLE){
            Pterm = term;
            term = 0;
            break;
        }
    }
}
cs










'컴퓨터/Linux' Related Articles

티스토리툴바