BaseFlight MPU 6050 - FabLabSeoul/WingProject GitHub Wiki

이 글은 오픈소스 MultiWii BaseFlight 프로젝트를 분석한 글이다.

Baseflight 의 MPU-6050 라이브러리를 이용한 가속도, 자이로 센서 소스는 STM32\stm32vldiscovery_package\Project\Examples\MPU-6050_Baseflight경로에 있다.

MPU-6050 센서의 정보를 읽기위해, 처음 거쳐야 과정은 drv_mpu6050.c 라이브러리의 mpu6050Detect() 함수에 나와있다.

센서 구조체

가속도 센서, 자이로스코프는 sensor_t 구조체로 표현된다. 멤버함수포인터 init, read를 통해 센서를 초기화하고, 정보를 읽어온다. 다양한 센서에 맞춰 확장시킬 수 있도록 디자인 되었다. C++의 추상 클래스와 닮았다.

typedef struct sensor_t {
    sensorInitFuncPtr init;               // initialize function
    sensorReadFuncPtr read;               // read 3 axis data function
    sensorReadFuncPtr temperature;        // read temperature if available
    float scale;           // scalefactor (currently used for gyro only, todo for accel)
} sensor_t;

typedef void (*sensorInitFuncPtr)(sensor_align_e align);   // sensor init prototype
typedef void (*sensorReadFuncPtr)(int16_t *data);          // sensor read and align prototype

typedef enum {
    ALIGN_DEFAULT = 0,                                      // driver-provided alignment
    CW0_DEG = 1,
    CW90_DEG = 2,
    CW180_DEG = 3,
    CW270_DEG = 4,
    CW0_DEG_FLIP = 5,
    CW90_DEG_FLIP = 6,
    CW180_DEG_FLIP = 7,
    CW270_DEG_FLIP = 8
} sensor_align_e;

센서인식, mpu6050Detect() 함수

mpu6050Detect()함수는 넘어온 인자 acc, gyro, scale 값을 채워넣어서 리턴하는 함수다. 실패하면 false를 리턴한다. lpf값은 low pass filter 값으로, 기본적으로 42값을 넣는다.

이 함수를 차근차근 따라가보자.

• 우선 MPU-6050과 연결되었는지 확인하는 과정을 거친다. MPU-6050의 MPU_RA_WHO_AM_I레지스터에 접근해 값을 확인한다. 이 때 값은 0x68 이어야 한다.

• 다음 과정은 MPU-6050의 revision, product id를 가져오는 부분이다. 이 부분은 MPU-6050의 register map에도 문서화 되지 않은 내용이라 확실히 모르겠다. 일단 넘어가자.

• 가속도 센서를 이용하기 위한 변수들을 초기화하는 함수, 가속도 센서 값을 가져오는 함수, 자이로스코프 정보를 초기화하고, 값을 가져오는 함수 포인터를 설정한다. 다양한 센서들을 적용하기 쉽게하기위한 코드라고 보면된다.

• 자이로스코프 값을 간단하게 읽어오기 위한 스케일값 초기화 (MPU-6050_Baseflight에서는 이 값을 사용하지 않는다.)

// 16.4 dps/lsb scalefactor
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

• +-2000도 초당 각속도를 읽어오게 설정되어 있으므로, 센서 출력으로 나온 값을 16.4(scale factor)로 나누어야 한다. 각속도이므로, 라디안 단위로 바꾸기 위해 (M_PI / 180.0f) 를 곱한다. Stm32에서 micro seconds 단위로 시간을 계산하고 있으므로, 초당각속도에 0.000001f를 곱한다.

• x4 의 의미는 아직 모르겠다.

• 실제 gyro->scale 값은 다음처럼 사용된다. 최종 결과 값은 센서의 자세를 라디안으로 계산한 값이다. 이 코드는 imu.c 라이브러리에 있다.

actual angle[ axis] = gyro->angle[ axis] * gyro->scale * deltaT;

• low pass filter 값에 42를 넣고, 센서 초기화 코드는 끝난다.

extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;

bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
{
    bool ack;
    uint8_t sig, rev;
    uint8_t tmp[6];

    delay(35);                  // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe

    ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
    if (!ack)
        return false;

    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0뭩 7-bit I2C address.
    // The least significant bit of the MPU-60X0뭩 I2C address is determined by the value of the AD0 pin. (we know that already).
    // But here's the best part: The value of the AD0 pin is not reflected in this register.
	// 정상적으로 작동한다면 sig 값은 1101000(b) = 0x68 값을 가진다. 
    if (sig != (MPU6050_ADDRESS & 0x7e))
        return false;

    // determine product ID and accel revision
    i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp);
    rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01);
    if (rev) {
        /* Congrats, these parts are better. */
        if (rev == 1) {
            mpuAccelHalf = 1;
        } else if (rev == 2) {
            mpuAccelHalf = 0;
        } else {
            failureMode(5);
        }
    } else {
        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &sig);
        rev = sig & 0x0F;
        if (!rev) {
            failureMode(5);
        } else if (rev == 4) {
            mpuAccelHalf = 1;
        } else {
            mpuAccelHalf = 0;
        }
    }

    acc->init = mpu6050AccInit;
    acc->read = mpu6050AccRead;
    gyro->init = mpu6050GyroInit;
    gyro->read = mpu6050GyroRead;

    // 16.4 dps/lsb scalefactor
    gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

    // give halfacc (old revision) back to system
    if (scale)
        *scale = mpuAccelHalf;

    // default lpf is 42Hz
    if (lpf >= 188)
        mpuLowPassFilter = INV_FILTER_188HZ;
    else if (lpf >= 98)
        mpuLowPassFilter = INV_FILTER_98HZ;
    else if (lpf >= 42)
        mpuLowPassFilter = INV_FILTER_42HZ;
    else if (lpf >= 20)
        mpuLowPassFilter = INV_FILTER_20HZ;
    else if (lpf >= 10)
        mpuLowPassFilter = INV_FILTER_10HZ;
    else
        mpuLowPassFilter = INV_FILTER_5HZ;

    return true;
}

센서 초기화, acc.init(), gyro.init()

가속도 센서를 초기화하는 코드는 복잡하지 않다. 그리고 MPU-6050_Baseflight에서는 mpu6050AccInit() 함수에서 초기화된 변수는 쓰이지 않고 있다. 가속도 센서 환경설정 코드는 현재 mpu6050GyroInit() 함수에 있다.

이 코드는 가속도 센서 인식 범위를 +-8g 가 되게 한다. 그래서 Scale Factor는 4,096이 된다.

// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

자이로센서를 초기화하는 코드는 조금 복잡하다. 설정하는 값은 다음과 같다.

• MPU_RA_SMPLRT_DIV = 0
• MPU_RA_PWR_MGMT_1 = 3
  • Z Axis 센서 클럭으로 매인 클럭을 맞춘다는 의미다. 27 KHz다. 좀더 안정적인 센싱을위해 제조사에서 이 방식을 추천하고 있다.
• MPU_RA_INT_PIN_CFG = 2
• MPU_RA_CONFIG = 42
• MPU_RA_GYRO_CONFIG = 0x18
  • 자이로스코프 센싱 범위를 +-2000도 로 하게 한다. 그래서 Scale Factor 값은 16.4 가 된다.

static void mpu6050AccInit(sensor_align_e align)
{
    if (mpuAccelHalf)
        acc_1G = 255 * 8;
    else
        acc_1G = 512 * 8;

    if (align > 0)
        accAlign = align;
}

static void mpu6050GyroInit(sensor_align_e align)
{
    gpio_config_t gpio;

    gpio.pin = Pin_13;
    gpio.speed = Speed_2MHz;
    gpio.mode = Mode_IN_FLOATING;
    gpioInit(GPIOC, &gpio);

    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
    delay(100);
    i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00);      //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03);      //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
    i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter);  //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);

    // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
    // Accel scale 8g (4096 LSB/g)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

    if (align > 0)
        gyroAlign = align;
}

Main() 함수

가속도, 자이로스코프 센서 값을 가져오는 함수는 acc.read(); gyro.read();

int main()
{
	if (!setup())
		return 0;

	calibrate_sensors();
	set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);

	while(1)
	{
		int16_t accelgyro[6];	
		acc.read(accelgyro);
		gyro.read(accelgyro+3);
		
		SendSerialAccelGryro(accelgyro);
		
		delay(10);
	}
}