[STM32] MPU9250과 상보필터(2)

4월에 MPU9250 관련한 글을 쓰고 상보 필터에 관한 글을 빠르게 쓸 줄 알았지만 친구 졸업과제와 여러 가지 해야 할 일들이 있어서 쓰지 못했던 나머지 글을 쓰려한다.

 

 

HAL 코드

#define mpu9250_ADDRESS 0b1101000<<1 // AD0pin = 0
#define alpha 0.9996
#define dt 0.0025
 
 
typedef struct __MPU9250{
    I2C_HandleTypeDef* i2c;
 
    uint8_t gyro_address;
 
    int32_t getmpuaccx,getmpuaccy,getmpuaccz;
 
    float f_gyx, f_gyy,f_gyz;
 
    float pitch, roll;
 
}MPU9250;

기존 코드에서 변경점과 추가되는 값이 생겼다.

alpha의 값은 상보필터에서 사용된다.

dt는 상보필터에서 각도를 업데이트하는 시간을 나타낸다. 나는 2.5ms마다 드론의 각도를 변경하기 위해 0.0025의 값을 정의했다.

getmpuacc는 xyz축의 가속도의 raw데이터를 나타내고 f_gy는 실수 형태의 xyz각속도의 변수이다.

pitch, roll은 각도를 나타낸다.

 

void init_MPU9250(MPU9250* mpu9250, I2C_HandleTypeDef* hi2c){
    mpu9250->i2c=hi2c;
    mpu9250->gyro_address=mpu9250_ADDRESS;
    mpu9250->pitch=0.0;
    mpu9250->roll=0.0;
    HAL_Delay(1000);
    Gyro_Writebyte(mpu9250,PWR_MGMT_1,0x00);
 
    HAL_Delay(100);
    Gyro_Writebyte(mpu9250,PWR_MGMT_1,0x01);
 
    Gyro_Writebyte(mpu9250,SMPLRT_DIV,0x13);
 
    Gyro_Writebyte(mpu9250,GYRO_CONFIG,0x08);
 
    Gyro_Writebyte(mpu9250,ACCEL_CONFIG,0x08);
 
    Gyro_Writebyte(mpu9250,INT_PIN_CFG,0X02);
}
 

sqrtf와 asinf 등등을 사용하기 위해 math.h를 include 해야 한다.

상보 필터에 관한 자세한 이론은 생략하기로 하고 기본적인 코드에 대해 설명해보면

 

acctotvec는 xyz의 가속도의 크기이다. 이때 코드에 100을 각각 나눠주고 마지막에 10을 곱했는데, 해주지 않을 경우 오버플로우가 발생하여 값이 이상해지는 경우가 있었다. 따라서 오버플로우가 일어나지 않도록 처음 크기를 작게 만들고 나중에 10을 키워주는 방식으로 구했다.

 

accdeg의 경우 acctotvec에서 구한 전체 가속도 크기와 x, y의 가속도를 사용하여 x축, y축에 대한 각도를 구한 값이 들어간다. 57.29577951의 값은 asinf의 리턴 값이 degree가 아닌 radian의 값이어서 degree로 바꿔주기 위해 곱한 값이다.

 

가장 중요한 부분인 pitch, roll을 구하는 식이다.

(alpha)*(mpu9250->pitch-(mpu9250->f_gyy)*dt) 이 부분은 각속도를 이용하여 각도를 구하는 부분이다. 각속도를 사용하여 각도를 구한방식은 각속도를 적분하여 각도를 구한 것이다. 각속도만 사용하여 각도를 계산할 경우 드리프트가 발생하여 움직이지 않아도 각도가 변할 수 있다.

주의할 점은 pitch를 구할 때는 accdegx와 f_gyy, roll을 구할 때는 accdegy와 f_gyx를 사용해야 한다.

 

(1-alpha)*(accdegx) 부분이 가속도에 의해 계산된 각도이다. 이 값으로 각속도에 의해 발생하는 드리프트를 제어한다.

 

alpha값은 1과 0 사이 값이 들어가야 한다. 만약 alpha값이 작아지면 가속도로 구한 accdeg값에 영향을 더 많이 받게 되므로 각도가 많이 틀어지는 경우가 생긴다. alpha가 커질 경우 드리프트가 일어날 수 있다. 따라서 적당한 수치를 구해 사용하면 된다.

 

드론을 만들 때 사용한 코드에 필요 없는 부분은 지우고 약간 간략하게 만들긴 했다. 필요한 부분은 수정해서 사용하면 될 거 같다.

 

 

 

추가 -

https://ddtxrx.tistory.com/17?category=913763

[STM32] LL 드라이버 - I2C DMA로 작성한 MPU6050 상보필터

LL 드라이버로 작성한 I2C DMA 상보필터 코드도 작성했다.

 

HAL 드라이버와 다르게 레지스터에 직접 접근하여 제어하는 방식이라 조금 어려울 수 있지만 익숙해지면 HAL 드라이버보다 작동원리를 이해하기 쉽다.