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 최초 작성
'Development > Embedded' 카테고리의 다른 글
STM32F103 - USART Interrupt 소스 (4) | 2020.01.20 |
---|---|
ATmega128 MPU6050 (18) | 2014.03.18 |
Getting Start STM32F103 Dev (0) | 2014.02.28 |
STM32F103 - QuadCopter 참고 소스 (0) | 2014.02.19 |
STM32F103 - PWM 소스 (0) | 2014.02.19 |
FB155BC 설정 with USB2UART Downloader (0) | 2014.02.19 |
안녕하세요 주인장님 제가 atmega128로 mpu6050값을 받는걸 공부하고있는데요
실례되는 말이지만 위에 쓰신 소스 전문을 받으수있을까요??
제가 학생때 공부했던 내용이라 자료 관리에 소홀하여
현재 전체 소스를 가지고 있지 않습니다.
블로그 내용에 MPU6050 읽기 소스 등을 참고하여 소스를 재작성 해야 될겁니다
이코드 현재 분석중인데
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;
}
에서 1.8 은 무엇을 의미 하는 거에요 ??
MPU 6050 자이로 모듈 동작 전압입니다
당시 제가 찾던 자료들에서 확인한 공식에 의해 작성되었습니다
기초가 부족했던 때라 레퍼런스 남기는걸 잊었습니다
getGyroDegree함수에서 gx, gy와 달리 gz는 왜 하지않는건가요?
Yaw 값이 필요 없었습니다
코딩 하던 당시에
답변 감사합니다.
그럼 roll값과 pitch값이 가속도와 각속도가 합쳐진 값인거죠??
네 가속도 값을 베이스로 하고 각속도로 보정하는 방식입니다
자료 감사히 잘봤구요, 답변 감사합니다. 도움 많이 됬습니다.^^
비밀댓글입니다
타이머 0,1동시 사용은 저도 정확히 모르겠네요
Dt는 100헤르츠면 0.01맞습니다
비밀댓글입니다
비밀댓글입니다
인터럽트를 사용해서 일정 주기마다 실행을 해야 샘플링이 되는거라
그냥 반복문에 무작정 돌리면 문제가 될 수 있어요
이 코드를 보시는건 CodeVision 사용하는 중이신가요?
가능하면 AtmelStudio로 넘어가시는걸 추천드립니다
그리고 타이머0 인터럽트는 정상적으로 발생하는지요?
비밀댓글입니다
이런 질문 드려도 될런지....
혹시 식에서 0.01이라고 적힌 것 위에 댓글 보니깐 dt 값인거 같은데
그 값은 어떻게 정하는 건가요...? 저도 사용하려는데 dt 값을 어떻게 해야 할지 몰라서 며칠 째 공부 중인데 답이 안나오네요...
dt 값은 임의로 정하는 것입니다
초당 100회 이상 샘플링을 하는 것이 일반적인것 같습니다
덕분에 좋은 정보 알아갑니다.