BaseFlight Sensor2 - FabLabSeoul/WingProject GitHub Wiki

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

Stm32 value line discovery 보드에 맞게 코드를 재작성 했다.

이 글은 Baseflight의 Sensor 라이브러리를 분석한 글이다. Sensor 초기화에 관련된 글은 Sensor 첫 번째 문서 BaseFlight Sensor 문서를 참조하자.

Sensor에 관한 글이 길어져 두 문서로 나누었다. 여기서는 센서 값을 읽어오는 코드를 주로 분석한다.

Sensor 라이브러리(sensors.c) 에서 센서 값을 가져오는 함수는 다음과 같다.

• ACC_getADC()
  • 가속도 센서로부터 값을 읽어온 후, ACC_Common()함수를 호출한다.
• ACC_Common()
  • 가속도 센서 값을 필터링해 x,y,z축 가속도(g) 최종 값을 accADC[]에 저장한다.
• Gyro_getADC()
  • 자이로스코프 값을 읽어온 후, GYRO_Common()함수를 호출한다.
• GYRO_Common()
  • 자이로스코프 값을 필터링해 x,y,z축 각속도(degree/seconds) 최종 값을 gyroADC[]에 저장한다.
• Mag_getADC()
  • 지자기 센서 값을 필터링해 x,y,z축 자기장(gauss) 최종 값을 magADC[]에 저장한다.

ACC_Common() 함수

Baseflight Sensor::ACC_Common()함수는 다음과 같다.

#define CALIBRATING_ACC_CYCLES              400

int16_t accADC[3];
uint16_t calibratingA = CALIBRATING_ACC_CYCLES;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
uint16_t acc_1G = 256;          // this is the 1G measured acceleration.

static void ACC_Common(void)
{
    static int32_t a[3];
    int axis;

    if (calibratingA > 0) {
        for (axis = 0; axis < 3; axis++) {
            // Reset a[axis] at start of calibration
            if (calibratingA == CALIBRATING_ACC_CYCLES)
                a[axis] = 0;
            // Sum up CALIBRATING_ACC_CYCLES readings
            a[axis] += accADC[axis];
            // Clear global variables for next reading
            accADC[axis] = 0;
            mcfg.accZero[axis] = 0;
        }
        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
        if (calibratingA == 1) {
            mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
            mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
            mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
            cfg.angleTrim[ROLL] = 0;
            cfg.angleTrim[PITCH] = 0;
        }
        calibratingA--;
    }

    accADC[ROLL] -= mcfg.accZero[ROLL];
    accADC[PITCH] -= mcfg.accZero[PITCH];
    accADC[YAW] -= mcfg.accZero[YAW];
}

프로그램이 시작되면, 400 사이클동안 가속도 값을 측정해 평균을 내어 mcfg.accZero[] 에 저장한다. 그리고 평균 값을 반올림하기 위해서 평균 값에 1/2를 더한 값을 저장한다. Z축에 - acc_1G를 하는 이유는 기본 중력 가속도가 이미 반영된 상태이므로 그 값을 제거하기 위한 계산이다.

Calibration, 즉 교정단계가 끝나면, 본격적으로 가속도 센서 값을 읽어 온다. 최종 값은 accADC[] 전역 변수에 저장된다.

accADC[ROLL] -= mcfg.accZero[ROLL];
accADC[PITCH] -= mcfg.accZero[PITCH];
accADC[YAW] -= mcfg.accZero[YAW];

처음 가속도 센서 교정시 측정되었던 값이 mcfg.accZero[]에 저장되어 있기 때문에, 이 값을 실제 센서값에서 빼서 저장한다. 이렇게 하는 이유는, 초기 센서의 자세를 원점으로, 초기 자세에서 변화된 가속도 값을 얻기 위해서다. 프로그램이 시작될 때, 센서가 기울어져 있는 경우가 있고, 처음부터 기울어진 상태로 설계되었더라도, 원점에서 변화된 값만 읽어오기 때문에, 문제없이 가속도 값을 얻어올 수 있게 된다.

GYRO_Common() 함수

Baseflight Sensor::GYRO_Common() 함수는 다음과 같다.

#define CALIBRATING_GYRO_CYCLES             1000
uint16_t calibratingG = CALIBRATING_GYRO_CYCLES;
int16_t gyroADC[3], accADC[3], accSmooth[3];
int16_t gyroZero[3] = { 0, 0, 0 };

mcfg.moron_threshold = 32;

static void GYRO_Common(void)
{
    int axis;
    static int32_t g[3];
    static stdev_t var[3];

    if (calibratingG > 0) {
        for (axis = 0; axis < 3; axis++) {
            // Reset g[axis] at start of calibration
            if (calibratingG == CALIBRATING_GYRO_CYCLES) {
                g[axis] = 0;
                devClear(&var[axis]);
            }
            // Sum up 1000 readings
            g[axis] += gyroADC[axis];
            devPush(&var[axis], gyroADC[axis]);
            // Clear global variables for next reading
            gyroADC[axis] = 0;
            gyroZero[axis] = 0;
            if (calibratingG == 1) {
                float dev = devStandardDeviation(&var[axis]);
                // check deviation and startover if idiot was moving the model
                if (mcfg.moron_threshold && dev > mcfg.moron_threshold) {
                    calibratingG = CALIBRATING_GYRO_CYCLES;
                    devClear(&var[0]);
                    devClear(&var[1]);
                    devClear(&var[2]);
                    g[0] = g[1] = g[2] = 0;
                    continue;
                }
                gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
                blinkLED(10, 15, 1);
            }
        }
        calibratingG--;
    }
    for (axis = 0; axis < 3; axis++)
        gyroADC[axis] -= gyroZero[axis];
}

자이로스코프 값의 표준 편차를 구하기 위한 함수들

• stdev_t
  • 표준 편차를 구하기위한 변수들 저장
• devClear()
• devPush()
  • 평균, 편차 저장
• devVariance()
  • 분산 계산
• devStandardDeviation()
  • 표준 편차 계산

프로그램이 시작되면 1000 사이클 동안 자이로스코프 교정(Calibration)단계를 진행한다. 1000 사이클동안 센서의 표준편차가 mcfg.moron_threshold 보다 크다면, 교정단계를 다시 거친다. mcfg.moron_threshold 값은 32로 설정되어 있다. 이 의미는, 초기 센서 자세가 움직이지않고, 고정되어 있어야 한다는 뜻이다. 교정 단계가 문제 없이 끝났다면, 1000 사이클 동안의 자이로센서 값의 평균을 gyroZero[] 변수에 저장한다. gyroZero[]는 자이로스코프의 원점이 된다.

    for (axis = 0; axis < 3; axis++)
        gyroADC[axis] -= gyroZero[axis];

자이로스코프 Calibration과정이 모두 끝났다면, 읽어온 자이로 센서 값에 gyroZero[] 값을 빼서 최종 값을 gyroADC[]에 저장한다. 이 의미는 프로그램이 시작될 때, 센서의 자세를 기본 자세로해서 그 변화 값을 최종 각도로 결정하겠다는 뜻이다.

Mag_getADC() 함수

Baseflight Sensor::Mag_getADC() 함수는 다음과 같다.

int16_t magADC[3];

int Mag_getADC(void)
{
    static uint32_t t, tCal = 0;
    static int16_t magZeroTempMin[3];
    static int16_t magZeroTempMax[3];
    uint32_t axis;

    if ((int32_t)(currentTime - t) < 0)
        return 0;                 //each read is spaced by 100ms
    t = currentTime + 100000;

    // Read mag sensor
    mag.read(magADC);

    if (f.CALIBRATE_MAG) {
        tCal = t;
       for (axis = 0; axis < 3; axis++) {
            mcfg.magZero[axis] = 0;
            magZeroTempMin[axis] = magADC[axis];
            magZeroTempMax[axis] = magADC[axis];
        }
        f.CALIBRATE_MAG = 0;
    }

    if (magInit) {              // we apply offset only once mag calibration is done
        magADC[X] -= mcfg.magZero[X];
        magADC[Y] -= mcfg.magZero[Y];
        magADC[Z] -= mcfg.magZero[Z];
    }

    if (tCal != 0) {
        if ((t - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
            LED0_TOGGLE;
            for (axis = 0; axis < 3; axis++) {
                if (magADC[axis] < magZeroTempMin[axis])
                    magZeroTempMin[axis] = magADC[axis];
                if (magADC[axis] > magZeroTempMax[axis])
                    magZeroTempMax[axis] = magADC[axis];
            }
        } else {
            tCal = 0;
            for (axis = 0; axis < 3; axis++)
                mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
            //writeEEPROM(1, true);
        }
    }

    return 1;
}

Mag_getADC()함수는 1초에 10번만 계산하게 되어 있다. 지자기 센서가 정확하지 않기 때문에 그런게 아닌가 생각한다. 지자기 센서 값은 크게 가공하지 않고 그대로 최종 값이 된다.

지자기 센서의 교정(calibration)단계는 기본적으로 거치지 않는다. 교정 작업은 f.CALIBRATE_MAG이 활성화 되었을 때 부터 시작하고, 교정이 끝난 후, 최종 원점은 (tCal != 0) && ((t - tCal) > 30000000) 조건이 성립했을 때, 계산된다.

현재 코드에서 tCal이 업데이트 되는 경우는 f.CALIBRATE_MAG 이 1이상일 때, 설정된다. 이 플래그는 외부 입력으로 활성화 된다. 지자기 센서가 Calibration을 시작하게 되면, 30초 동안 3축의 센서 값의 최대, 최소값을 구한 후, 중간 값을 원점 값으로 결정짓는다. 지자기 센서 원점 값은 mcfg.magZero[]에 저장된다. 교정 과정을 거치지 않으면, 센서의 현재 자세가 기본 값이 된다. 이 의미는, 지자기 센서의 N 방향이 앞을 나타내므로, 쿼드콥터의 정면이 북쪽을 향해있다고 가정한다는 뜻이다.

지자기 센서의 최종 필터링 된 값은 magADC[]에 저장된다.