BaseFlight IMU - FabLabSeoul/WingProject GitHub Wiki

IMU (Inertia Measurement Unit)

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

Multiwii Baseflight를 Stm32 Value line discovery 보드에 맞게 코드를 재작성하였다.

Stm32 Value line discovery 보드는 24MHz 속도가 최대이기 때문에, 72MHz 속도의 Naze32보드 환경에서 프로그래밍된 Baseflight 소스가 문제가 발생할 여지가 있다. 특히 시스템 클럭을 이용하는 micros() 함수와 millis()함수가 문제인데, 현재 시점에서 micros() 함수가 잘 작동하지 않는다는 걸 확인 했다. 조만간 문제를 해결하고, 글을 쓸 생각이다.

Baseflight 에서 imu.c 라이브러리는 Sensor 라이브러리를 이용해, 멀티콥터의 Roll,Pitch,Yaw각을 계산한다. imu 라이브러리의 주요함수는 다음과 같다.

• imuInit()
  • IMU 초기화
• computeIMU()
  • 가속도,자이로,지자기 센서 값을 읽어오고, getEstimatedAttitude() 함수를 호출한다.
• getEstimatedAttitude()
  • 가속도, 자이로스코프, 지자기 센서 값을 이용해, 멀티콥터의 Roll,Pitch,Yaw각을 계산한다.

imuInit() 함수

void imuInit(void)
{
    smallAngle = lrintf(acc_1G * cosf(RAD * cfg.small_angle));
    accVelScale = 9.80665f / acc_1G / 10000.0f;
    throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);

    fc_acc = 0.5f / (M_PI * cfg.accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf

    Mag_init();
}

lrintf()함수는 float 값을 소수점이하 값을 잘라서 int형으로 변환 한다.

computeIMU() 함수

Baseflight imu::computeIMU() 함수는 다음과 같다.

void computeIMU(void)
{
    Gyro_getADC();
    ACC_getADC();
    getEstimatedAttitude();

    gyroData[YAW] = gyroADC[YAW];
    gyroData[ROLL] = gyroADC[ROLL];
    gyroData[PITCH] = gyroADC[PITCH];
}

가속도, 자이로스코프 센서 값을 읽어온 후, getEstimatedAttitude()함수를 호출한다.

getEstimatedAttitude() 함수

getEstimatedAttitude() 함수에서는 accADC[], gyroADC[], magADC[] 변수를 토대로 멀티콥터의 Roll,Pitch,Yaw 각을 계산한다.

전체 코드는 다음과 같다. 하나하나씩 분석해보자.

static void getEstimatedAttitude(void)
{
    int32_t axis;
    int32_t accMag = 0;
    static t_fp_vector EstM;
    static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
    static float accLPF[3];
    static uint32_t previousT;
    uint32_t currentT = micros();
    uint32_t deltaT;
    float scale, deltaGyroAngle[3];
    deltaT = currentT - previousT;
    scale = deltaT * gyro.scale;
	
	// TO: dont understand
	// 시간이 초기화 되는 버그 때문에 추가함.
	// 이유는 아직 모르겠음.
	// 현재시간과 전 시간 차이가 너무 크면 연산하지 않는다.
	if (currentT - previousT > 10000) // 10ms over
	{
	    previousT = currentT;
		return;
	}

    previousT = currentT;	
	
    // Initialization
	for (axis = 0; axis < 3; axis++)
	{
        deltaGyroAngle[axis] = gyroADC[axis] * scale;
        if (cfg.acc_lpf_factor > 0) 
		{
            accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
            accSmooth[axis] = accLPF[axis];
        } 
		else 
		{
            accSmooth[axis] = accADC[axis];
        }
				
        accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
    }
    accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);

    rotateV(&EstG.V, deltaGyroAngle);

    // Apply complimentary filter (Gyro drift correction)
    // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
    // To do that, we just skip filter, as EstV already rotated by Gyro
    if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
        for (axis = 0; axis < 3; axis++)
            EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
    }

    f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);

    // Attitude of the estimated vector
    anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
    anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
    angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
    angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));

	if (0)
	{
        rotateV(&EstM.V, deltaGyroAngle);
       for (axis = 0; axis < 3; axis++)
            EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
       heading = calculateHeading(&EstM);
    } 
	else 
	{
        rotateV(&EstN.V, deltaGyroAngle);
        normalizeV(&EstN.V, &EstN.V);
        heading = calculateHeading(&EstN);
	}

    acc_calc(deltaT); // rotate acc vector into earth frame

    if (cfg.throttle_correction_value) 
	{
        float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);

        if (cosZ <= 0.015f) 
		{ // we are inverted, vertical or with a small angle < 0.86 deg
            throttleAngleCorrection = 0;
        } 
		else 
		{
            int deg = lrintf(acosf(cosZ) * throttleAngleScale);
            if (deg > 900)
                deg = 900;
            throttleAngleCorrection = lrintf(cfg.throttle_correction_value * sinf(deg / (900.0f * M_PI / 2.0f)));
        }
    }		
}

###1. deltaT 계산 Stme32 Value line discovery 보드의 속도 때문에, micros()함수가 중간에 초기화 되는 문제가 있다. 그래서 전 시간과 너무 많이 차이나면 무시하게 하는 코드가 추가 되었다. 어디까지나 임시방편 코드이기 때문에, 수정해야 할 코드다.

uint32_t currentT = micros();
uint32_t deltaT;
deltaT = currentT - previousT;

if (currentT - previousT > 10000) // 10ms over
{
    previousT = currentT;
    return;
}

###2. 가속도, 자이로스코프 센서 값을 가져온다. gyroADC[axis] * scale; 코드를 다시 쓰면 gyroADC[axis] * (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f * deltaT; 와 같다. 각속도 값이 자이로스코프 센서 값을 micro seconds 단위의 라디안 속도 값으로 변환해주는 코드다. 이제 자이로스코프 센서 값은 deltaGyroAngle[axis]에 저장된다.

cfg.acc_lpf_factor 전역변수는 4로 초기화 되어 있는 상태이기 때문에, if (cfg.acc_lpf_factor > 0) 조건은 성립한다. 블럭 안에 있는 코드는 Low Pass Filter를 뜻하며, 가속도 값의 평균을 구하되, 1/cfg.acc_lpf_factor크기 만큼의 성분을 가장 최근 값에 비중을 두고, 나머지 1 - (1/cfg.acc_lpf_factor) 값만큼 그 전 평균값에 비중을 둔다는 뜻이다.

Low Pass Filtering 된 값은 accLPF[] 스태틱 변수와 accSmooth[]변수에 저장된다. 로우 패스 필터를 쓰든 쓰지않든, 최종 가속도 센서 값은 accSmooth[]변수에 저장된다.

accMag 변수는 (가속도 벡터의 크기 X 100) 을 저장한다. float값을 int16_t로 변환할 때, 소수점 두 번째 자리까지 표현하기 위해서 X100 이 되었다.

정리하면

• deltaGyroAngle[] = 초당 (x,y,z) 각속도 -> 마이크로 초당 (x,y,z) 라디안 속도를 저장한다.
• accSmooth[] = 로우 패스 필터가 적용된 x,y,z 가속도를 저장한다.
• accMag = 가속도 벡터 크기 X 100 값을 저장한다.

int32_t accMag = 0;
static float accLPF[3];
float scale, deltaGyroAngle[3];
deltaT = currentT - previousT;
scale = deltaT * gyro.scale;

// Initialization
for (axis = 0; axis < 3; axis++)
{
    deltaGyroAngle[axis] = gyroADC[axis] * scale;
    if (cfg.acc_lpf_factor > 0) 
    {
        accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
        accSmooth[axis] = accLPF[axis];
    } 
	else 
	{
        accSmooth[axis] = accADC[axis];
    }
				
    accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
}
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);

###3. x,y,z축 마이크로초당 라디안 속도 값으로 벡터를 회전시킨다.

rotateV()함수는 인자로 넘어온 벡터를 x,y,z축 각속도 값을 적용해 회전시킨다.

rotateV()함수는 정확히 x,y,z축 오일러 회전값으로 회전행렬을 만든 후, 벡터를 회전시키는 코드다.

결국 EstG 벡터는 멀티콥터가 바라보는 방향을 나타낸다.

t_fp_vector EstG; // 전역변수

rotateV(&EstG.V, deltaGyroAngle);


// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
    struct fp_vector v_tmp = *v;

    // This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
    float mat[3][3];
    float cosx, sinx, cosy, siny, cosz, sinz;
    float coszcosx, sinzcosx, coszsinx, sinzsinx;

    cosx = cosf(delta[ROLL]);
    sinx = sinf(delta[ROLL]);
    cosy = cosf(delta[PITCH]);
    siny = sinf(delta[PITCH]);
    cosz = cosf(delta[YAW]);
    sinz = sinf(delta[YAW]);

    coszcosx = cosz * cosx;
    sinzcosx = sinz * cosx;
    coszsinx = sinx * cosz;
    sinzsinx = sinx * sinz;

    mat[0][0] = cosz * cosy;
    mat[0][1] = -cosy * sinz;
    mat[0][2] = siny;
    mat[1][0] = sinzcosx + (coszsinx * siny);
    mat[1][1] = coszcosx - (sinzsinx * siny);
    mat[1][2] = -sinx * cosy;
    mat[2][0] = (sinzsinx) - (coszcosx * siny);
    mat[2][1] = (coszsinx) + (sinzcosx * siny);
    mat[2][2] = cosy * cosx;

    v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
    v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
    v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}

###4. 상보필터 적용

accMag은 가속도벡터크기 X 100 이 된 상태이므로, 100이 1G가 된다. 코드 상에서 72 < accMag < 133 이므로, 가속도벡터 크기가 0.72G 보다 크면서, 1.33G보다 작다면, 상보필터를 적용한다. 즉, 적당히 움직일 때만, 상보필터를 적용하고, 그렇지 않은 경우에는 자이로스코프 센서값을 그대로 따르겠다는 뜻이다.

상보필터 값 mcfg.gyro_cmpf_factor는 600이 설정되어있다. 코드가 약간 복잡하게 되어있지만, 상보필터 공식을 그대로 따른다. EstG.A[axis] = EstG.A[axis] * (600/601) + accSmooth[axis]/601 로 표현 할 수 있다.

더 간단히 하자면,

EstG.A[axis] = EstG.A[axis] * 0.99833f + accSmooth[axis]*0.00166f 로 표현된다.

mcfg.gyro_cmpf_factor값은 micro second 단위 인지, milli second 단위 인지, 함수가 호출되는 주기에 따라 결정된다.

// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
    for (axis = 0; axis < 3; axis++)
        EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}

###5. 멀티콥터의 자세 Roll, Pitch, Yaw 각을 계산한다. Z축 회전각이 smallAngle값을 넘어가면 f.SMALL_ANGLE 플래그가 활성화 된다.

최종 계산된 EstG벡터로 멀티콥터의 Roll, Pitch 회전각을 계산한다. 이 수식은 x,y,z 벡터를 이용해 Roll, Pitch 각을 구하는 공식 그대로 따른다.

anglerad[]변수는 라디안으로 회전각을 저장하고, angle[]은 각도 값으로 저장한다.

angle[]을 구할 때, (1800.0f / M_PI) 곱하는 이유는, 실수 값을 정수 값으로 변환 될 때, 소수점자리가 없어지기 때문에, 적어도 소수점 첫 번째 자리까지 계산하기 위해 X10 을 한다. 그래서 실제로 angle[]변수는 45도 일 때는 450값을 가지게된다.

f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);

// Attitude of the estimated vector
anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI));
angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI));

###6. 멀티콥터가 바라보는 방향을 계산한다. 지자기 센서가 있을 때와 그렇지 않을 때, 따로 분기 되었다.

지자기 센서가 있을 경우, 상보필터를 이용해 EstM벡터를 구한 후, calculateHeading() 함수로 최종 Yaw 회전각을 계산한다. 지자기 센서 상보필터 계수 mcfg.gyro_cmpfm_factor`는 250으로 설정된다.

지자기 센서가 없을 경우, 현재 x,y,z축 각속도 값을 나타내는 deltaGyroAngle[] 변수로 EstN벡터를 구하고, calculateHeading() 함수로 최종 Yaw 회전각을 계산한다.

//    if (sensors(SENSOR_MAG)) 
if (0)
{
    rotateV(&EstM.V, deltaGyroAngle);
    for (axis = 0; axis < 3; axis++)
        EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
   heading = calculateHeading(&EstM);
} 
else 
{
    rotateV(&EstN.V, deltaGyroAngle);
    normalizeV(&EstN.V, &EstN.V);
    heading = calculateHeading(&EstN);
}


// baseflight calculation by Luggi09 originates from arducopter
static int16_t calculateHeading(t_fp_vector *vec)
{
    int16_t head;

    float cosineRoll = cosf(anglerad[ROLL]);
    float sineRoll = sinf(anglerad[ROLL]);
    float cosinePitch = cosf(anglerad[PITCH]);
    float sinePitch = sinf(anglerad[PITCH]);
    float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
    float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
    float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
    head = lrintf(hd);
    if (head < 0)
        head += 360;

    return head;
}