World of Komastar

ATmega128 에서 MPU6050 의 가속도 자이로 값을 읽어오는 방법 100Hz로 샘플링을 하기 위해
타이머 오버플로우로 0.01초마다 인터럽트가 걸리게 설정함
TIMSK = 0x01;
TCCR0 = 0x07;
TCNT0 = 99;
SREG = 0x80;

그리고 인터럽트 소스를 작성

interrupt [TIM0_OVF] void timer_int0(void)
{       
    getRawData();
    getAcclDegree();
    getGyroDegree();
    compFilter();
    TCNT0 = 99;
}

가속도 센서와 자이로 센서 값을 읽어오는건 이전에 올린 소스가 있음
MPU6050_read() 함수는 지난 포스팅 참조 -Click-

void getRawData()
{
    buffer[0] = MPU6050_read(0x3B);
    buffer[1] = MPU6050_read(0x3C);
    buffer[2] = MPU6050_read(0x3D);
    buffer[3] = MPU6050_read(0x3E);
    buffer[4] = MPU6050_read(0x3F);
    buffer[5] = MPU6050_read(0x40);
     
    buffer[6] = MPU6050_read(0x43);
    buffer[7] = MPU6050_read(0x44);
    buffer[8] = MPU6050_read(0x45);
    buffer[9] = MPU6050_read(0x46);
    buffer[10] = MPU6050_read(0x47);
    buffer[11] = MPU6050_read(0x48);
     
    ax = (int)buffer[0] << 8 | (int)buffer[1];
    ay = (int)buffer[2] << 8 | (int)buffer[3];
    az = (int)buffer[4] << 8 | (int)buffer[5];
    gx = (int)buffer[6] << 8 | (int)buffer[7];
    gy = (int)buffer[8] << 8 | (int)buffer[9];
    gz = (int)buffer[10] << 8 | (int)buffer[11];
}

읽어온 값을 라디안 값으로 변환시켜줌

void getAcclDegree(void)
{
    x_aTmp1 = ((long)ay * (long)ay) + ((long)az * (long)az);
    y_aTmp1 = ((long)ax * (long)ax) + ((long)az * (long)az);
    z_aTmp1 = ((long)ay * (long)ay) + ((long)az * (long)az);
    
    x_aTmp2 = sqrt((float)x_aTmp1);
    y_aTmp2 = sqrt((float)y_aTmp1);
    z_aTmp2 = sqrt((float)z_aTmp1);
    
    x_aResult = atan((float)ax / x_aTmp2);
    y_aResult = atan((float)ay / y_aTmp2);
    z_aResult = atan(z_aTmp2 / (float)az);
}
void getGyroDegree(void)
{
    x_gTmp1 = (float)gx / 65536;
    y_gTmp1 = (float)gy / 65536;
    
    x_gTmp1 = x_gTmp1 * 1.8;
    y_gTmp1 = y_gTmp1 * 1.8;
    
    x_gResult = x_gTmp1;
    y_gResult = y_gTmp1;
}

volatile int gx = 0, gy = 0, gz = 0, ax = 0, ay = 0, az = 0;
volatile long x_aTmp1, y_aTmp1, z_aTmp1;
volatile float x_aTmp2, y_aTmp2, z_aTmp2, x_aResult, y_aResult, z_aResult;
volatile float x_gTmp1, y_gTmp1, x_gResult, y_gResult;
기본적으로 위의 모든 변수들은 float 혹은 double형으로 선언해두고
가속도 부분의 x_aTmp1, y_aTmp1, z_aTmp1 이 세개만 long형으로 선언
gx gy gz ax ay az는 raw data를 최초로 저장하는 변수임
참고로 gy-521의 MPU-6050모듈은 16bit ADC이고 레지스터 당 8비트니까
High bit / Low bit를 나눠서 읽어온 다음 쉬프트 연산과 OR 연산을 적절히 사용해 합쳐주는 방법을 사용했음.
마지막으로 필터부분

void compFilter(void)
{
    xTmp1 = (-y_aResult) + (float)xFilterAngle;
    xIntTmp1 = (float)xIntTmp1 + (xTmp1 * 0.01);
    xTmp2 = (-kp * xTmp1) + (-ki * (float)xIntTmp1) + x_gResult;
    xFilterAngle = xFilterAngle + (xTmp2 * 0.01);
    pitch = (int)(xFilterAngle * 180 / PI);
    
    yTmp1 = (-x_aResult) + (float)yFilterAngle;
    yIntTmp1 = (float)yIntTmp1 + (yTmp1 * 0.01);
    yTmp2 = (-kp * yTmp1) + (-ki * (float)yIntTmp1) + y_gResult;
    yFilterAngle = yFilterAngle + (yTmp2 * 0.01);
    roll = (int)(yFilterAngle * 180 / PI);
}

변수 선언은
float kp = 12.0f, ki = 1.0f;
volatile float xTmp1, yTmp1, xTmp2, yTmp2, xIntTmp1, yIntTmp1;
volatile float xFilterAngle = 0.0f, yFilterAngle = 0.0f;
volatile int pitch = 0, roll = 0;
위와 같이 하였다
지금까지 선언한 변수들은 쓰기 편하게 전역변수로 선언하고 그냥 가져다 쓰는 방식

2014 / 03 / 18 - 20:04 최초 작성