BaseFlight Sensor - FabLabSeoul/WingProject GitHub Wiki

이 글은 오픈소스 Multiwii Baseflight 를 분석한 글이다.

Baseflight는 가속센서, 자이로스코프, 지자기센서를 Sensor라는 라이브러리로 관리한다. 총 9 DOF를 처리하는 라이브러리다.

예제 소스는 STM32\stm32vldiscovery_package\Project\Examples\Sensor_Baseflight 경로에 있다. Baseflight Sensor라이브러리를 이용해 MPU-6050과 HMC5883L 센서를 테스트하는 예제다.

Sensor라이브러리를 이해하기 위해서 Baseflight의 기본 라이브러리를 이해하고 있어야 한다. 기본 라이브러리는 다음과 같다.

• drv_system
• drv_gpio
• drv_usart
• drv_serial
• serial
• drv_hmc5883l
• drv_mpu5060
• drv_i2c
• printf

Sensor라이브러리는 위에 열거한 라이브러리를 모두 쓰고 있다. 그리고 추가된 라이브러리는 다음과 같다.

• imu
• util
• sensor

이 글에서는 주로 util, imu, sensor 라이브러리를 설명하고, 전체 흐름을 설명하는데 중점을 두겠다.

초기화

Sensor 라이브러리를 사용하기위해 초기화 하는 과정을 보여준다.

void setup()
{
	gpio_config_t gpio;
	bool sensorsOK = false;

	mcfg.emf_avoidance = 0;
	mcfg.gyro_cmpf_factor = 600;    // default MWC	
	mcfg.gyro_cmpfm_factor = 250;   // default MWC
	mcfg.gyro_lpf = 42;             // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
	mcfg.gyro_align = ALIGN_DEFAULT;
	mcfg.acc_align = ALIGN_DEFAULT;
	mcfg.mag_align = ALIGN_DEFAULT;
	mcfg.vbatscale = 110;
	mcfg.currentscale = 400; // for Allegro ACS758LCB-100U (40mV/A)
	mcfg.moron_threshold = 32;

	cfg.mag_declination = 0;    // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
	cfg.small_angle = 25;
	cfg.throttle_correction_value = 0;      // could 10 with althold or 40 for fpv
	cfg.throttle_correction_angle = 800;    // could be 80.0 deg with atlhold or 45.0 for fpv
	cfg.accz_lpf_cutoff = 5.0f;
	cfg.acc_lpf_factor = 4;
	cfg.accz_deadband = 40;
	cfg.accxy_deadband = 40;
	
    // RCC 초기화.
    SystemInit();

	// 클럭, 타이머 초기화
	systemInit();

	// sleep for 100ms
	delay(100);

	// Stm32 보드에 있는 LED3,4를 활성화 한다.
	gpio.pin = Pin_All;
	gpio.speed = Speed_2MHz;
	gpio.mode = Mode_Out_PP;
	gpioInit(GPIOC, &gpio);

	// MPU-6050, HMC5883L과 통신할 i2c를 초기화 한다.
	i2cInit(I2CDEV_1, 0x00); // Init Master i2c 

    // MPU-6050, HMC6553L 센서 체크
    // 센서 초기화.
	sensorsOK = sensorsAutodetect();
	
	// if gyro was not detected due to whatever reason, we give up now.
	if (!sensorsOK)
			failureMode(3);

	// IMU 초기화
    // 지자기 센서 HMC6553L 초기화
	imuInit();

	// 시리얼 통신 초기화.
    // PA10,9 (Rx,Tx)
	serialInit(115200);
}

먼저 전역적으로 필요한 정보들을 master_t구조체와 config_t구조체의 각각 전역변수 mcfg, cfg 를 초기화 한다. 현재 Sensor 라이브러리에 필요한 값들만 초기화 했다.

delay() 함수를 사용하기위해 Baseflight함수 systeminit() 를 먼저 호출해야 한다. 그리고 Stm32 value line discovery 보드의 LED3,4 를 사용하기위해 GPIOC를 활성화 한다.

센서와 통신을 위해 I2C라이브러리를 i2cInit(I2CDEV_1, 0x00); 함수를 이용해 초기화 한다. 첫 번째 I2C 장치를 이용하고, 마스터가 되기 때문에 주소값은 필요없으므로 0x00으로 설정한다.

여기까지 진행되었다면, 본격적으로 센서와 통신을 시작한다. sensorsAutodetect() 함수를 통해 MPU-6050, HMC6553L과 통신을 시도해 연결여부를 확인하고, 기본 설정값으로 센서를 초기화 한다.

imuInit() 함수에서 지자기센서에 필요한 변수들을 초기화 한다.

마지막으로 디버깅을 위해 serialInit(115200) 함수로 PA10,9(Rx,Tx) 포트로 시리얼통신을 할 수 있게, 포트를 연다.

Sensor 구조체

가속도 센서, 자이로스코프, 지자기 센서는 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;

sensorsAutodetect() 함수

MPU-6050 과 HMC5883L 센서가 사용 가능한지 검사한다.

mpu6050Detect() 함수에 관해서는 Baseflight MPU-6050문서를 참조하자.

hmc5883lDetect() 함수에 관해서는 Baseflight HMC5883L문서를 참조하자.

sensor_t acc;                       // acc access functions
sensor_t gyro;                      // gyro access functions
sensor_t mag;                       // mag access functions
uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
uint8_t magHardware = MAG_DEFAULT;

bool sensorsAutodetect(void)
{
    int16_t deg, min;

	// Autodetect gyro hardware. We have MPU3050 or MPU6050 or MPU6500 on SPI
    if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale)) 
    {
    } 
	else
    {
            // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
           return false;
    }

	mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &core.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
	accHardware = ACC_MPU6050;

		
    // Now time to init things, acc first
    acc.init(mcfg.acc_align);
    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
    gyro.init(mcfg.gyro_align);

		
	if (hmc5883lDetect(&mag)) 
	{
			magHardware = MAG_HMC5883L;
	}

	// calculate magnetic declination
    deg = cfg.mag_declination / 100;
    min = cfg.mag_declination % 100;
    magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units

    return true;
}

센서 정보 가져오기

• BaseFlight Sensor2 문서에 설명되어 있다.

IMU 라이브러리를 이용한 센서 값 계산

• Baseflight IMU 문서에 설명되어있다.