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

mpu9250.h

0.01MB

드론 각도를 구하기 위해 사용한 센서는 MPU6050와 MPU9250두가지로 차이점은 MPU9250에는 MPU6050센서 내부에 지자기센서 AK8963를 추가했다는 점이다. 따라서 register가 비슷하다. 처음 드론을 만들때는 MPU6050 을 사용했지만 YAW를 효과적으로 잡기위해 MPU9250으로 변경했다.

지금은 지자기센서를 사용하지 않고 가속도와 각속도로 데이터를 상보필터에 적용하여 각도를 구하는 방법을 써보도록하겠다.

mpu9250 

 

 

GY-9250 schematic

I2C SDA, SCL 핀에 10k옴 풀업 저항이 달려있어 추가적으로 풀업저항을 달아줄 필요가 없다.

 

 

I2C address

mpu6050과 I2C address가 같다는 것을 알 수 있다.

 

 

HAL 코드

#define SLAVE_MPU_ADDRESS 0b1101000<<1 // AD0pin = 0
 
typedef struct __MPU9250{
    I2C_HandleTypeDef i2c;
    uint8_t gyro_address;
}MPU9250;
 
MPU9250 mpu9250={hi2c1,SLAVE_MPU_ADDRESS};

mpu9250의 주소와 HAL코드에 필요한 I2C 핸들구조체를 하나의 구조체에 묶어서 정의했다.

MPU9250 mpu9250={hi2c1,SLAVE_MPU_ADDRESS}; 와 같이 선언후 초기화를 해줘야 한다.

 

void Gyro_Writebyte(MPU9250 * I2C,uint8_t register_address,uint8_t data){
    uint8_t Trans[2]={register_address, data};
    HAL_I2C_Master_Transmit(&(I2C->i2c), I2C->gyro_address,Trans,2,10);
}
 
uint8_t Gyro_Readbyte(MPU9250 * I2C,uint8_t register_address){
    uint8_t Trans[1]={register_address};
    uint8_t Receive[1];
    HAL_I2C_Master_Transmit(&(I2C->i2c), I2C->gyro_address,Trans,1,10);
    HAL_I2C_Master_Receive(&(I2C->i2c),I2C->gyro_address,Receive,1,10);
 
    return Receive[0];
}
 
void init_MPU9250(MPU9250* mpu9250){
    Gyro_Writebyte(mpu9250,PWR_MGMT_1,0x00);
 
    HAL_Delay(100);
    Gyro_Writebyte(mpu9250,PWR_MGMT_1,0x01);
 
    Gyro_Writebyte(mpu9250,SMPLRT_DIV,0X07);
 
    Gyro_Writebyte(mpu9250,GYRO_CONFIG,0x08);
 
    Gyro_Writebyte(mpu9250,ACCEL_CONFIG,0x08);
}

mpu9250 register에서 데이터를 읽거나 데이터를 쓰는 함수를 정의하고 처음 전원을 켰을때 mpu9250을 초기화하는 코드를 만들었다.

ak8963지자기 센서에 접근하려면 Gyro_Writebyte(mpu9250,INT_PIN_CFG,0X02); 를 추가적으로 초기화 루틴에 넣어주면 접근할 수 있다.

PWR_MGMT_1, SMPLRT_DIV 등등 은 헤더파일에 정의되어있어 다운받아서 사용하면 된다.

 

float f_gyx, f_gyy,f_gyz;
int32_t getmpuaccx,getmpuaccy,getmpuaccz;
 
void MPU_read_acc_gyro(MPU9250* mpu9250){
    uint8_t databuf[14];
    int16_t tempmpuaccx,tempmpuaccy,tempmpuaccz;
    int16_t tempmpugyx,tempmpugyy,tempmpugyz;
 
    HAL_I2C_Mem_Read(&(mpu9250->i2c),mpu9250->gyro_address,0x3b,I2C_MEMADD_SIZE_8BIT,databuf,14,10);
    tempmpuaccx=((databuf[0]<<8)|databuf[1]);
    tempmpuaccy=((databuf[2]<<8)|databuf[3]);
    tempmpuaccz=((databuf[4]<<8)|databuf[5]);
    tempmpugyx=((databuf[8]<<8)|databuf[9]);
    tempmpugyy=((databuf[10]<<8)|databuf[11]);
    tempmpugyz=((databuf[12]<<8)|databuf[13]);
    getmpuaccx=tempmpuaccx;
    getmpuaccy=tempmpuaccy;
    getmpuaccz=tempmpuaccz;
 
    f_gyx=((float)(tempmpugyx))/65.5;
    f_gyy=((float)(tempmpugyy))/65.5;
    f_gyz=((float)(tempmpugyz))/65.5;
}

읽으려는 레지스터의 주소의 시작은 0x3B부터 순서대로 14개의 레지스터를 읽을것이다.

getmpuaccx, getmpuaccy, getmpuaccz는 가속도 값을 가진다. 0x1C 레지스터에 측정가능한 범위를 설정할 수 있다. 하지만 결국 하고자 하는것은 상보필터를 사용하여 각도를 구하기 위해서 이므로 raw data를 실제 xyz가속도로 변환하지는 않았다.

f_gyx, f_gyy, f_gyz는 자이로 센서를 읽어 각속도를 구한것 이다. 0x1B 레지스터의 설정값으로 측정범위를 설정할 수 있다. 자이로 데이터는 실제값을 상보필터에 사용하므로 65.5를 나눠 실제 각속도를 구했다. 만약 0x1B 레지스터를 변경하여 측정범위를 바꿀경우 65.5가 아닌 다른 값으로 나눠줘야한다.

databuf[6], databuf[7]의 값은 사용하지 않았다. 왜냐하면 databuf 6,7 번의 값은 0x41, 0x42의 레지스터의 값을 가지고 있는데 그 레지스터는 온도데이터를 출력하는 레지스터이다.

 

 

다음글-

ddtxrx.tistory.com/entry/STM32-MPU9250%EA%B3%BC-%EC%83%81%EB%B3%B4%ED%95%84%ED%84%B02?category=913763

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