BaseFlight HMC5883L - FabLabSeoul/WingProject GitHub Wiki

오픈소스 MultiWii Baseflight 프로젝트에는 3축 지자기 센서인 HMC5883L를 인식하는 hmc5883l.c,h 라이브러리가 있다. 이 소스를 가져오기 위해서는 i2c, gpio, system, board 파일이 필요하다. i2c, gpio, system은 여러군데에서 쓰이는 기본 라이브러리이기 때문에 같이 가져오면 되고, board는 Naze32보드를 위한 별도의 파일이기 때문에 가져와서 쓸 수 없다. 이 부분은 Stm32 Value line discovery 보드에 맞게 수정되어서 가져와야 한다. 예를들면 LED0, LED1은 Naze32와 Stm32 보드의 핀번호가 다르다.

때문에 Stm32보드 전용 정의를 구현하는 global.h 파일을 만들어, 전역 변수나 열거형들을 정의한다. board.h를 대신하는 소스라고 생각하면 된다. i2c, gpio, system 라이브러리 파일들은 크게 고치지 않고, 쉽게 소스를 가져다 쓸 수 있다. hmc5883l 소스도 마찬가지다.

구현된 소스는 I2C_Baseflight 프로젝트에 있다.

초기화

systemInit() 함수는 drv_system.h 라이브러리를 초기화하는 함수다. 클럭을 초기화하고, delay, tick 기능이 수행될 수 있게 초기화한다. SerialSetup()함수는 PA10,9(Rx,Tx)포트를 시리얼포트로 초기화 한다. init_printf()함수는 printf()함수를 사용할 수 있게 초기화한다. 다음으로 Stm32 보드에 있는 LED3,4를 활성화하는 코드가 있고, 마지막으로 HMC5883L과 통신할 i2c를 초기화 한다. MCU가 마스터가 되기 때문에 ownAddress값은 0이다.

void setup(void)
{
  gpio_config_t gpio;
	
  // 클럭을 초기화하고, delay, tick 기능이 수행될 수 있게 초기화한다.
  systemInit();
	
  // PA10,9(Rx,Tx)포트를 시리얼포트로 초기화 한다.
  SerialSetup();
	
  // printf()함수를 사용할 수 있게 초기화한다.
  init_printf(NULL, putc);
  printf( "Init baseflight i2c\n" );
	
  // Stm32 보드에 있는 LED3,4를 활성화 한다.
  gpio.pin = Pin_All;
  gpio.speed = Speed_2MHz;
  gpio.mode = Mode_Out_PP;
  gpioInit(GPIOC, &gpio);
	
  // HMC5883L과 통신할 i2c를 초기화 한다.
  i2cInit(I2CDEV_1, 0x00); // Init Master i2c 
  printf( "init i2c\n");
}

센서 구조체

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

지자기 센서 초기화, hmc5883lDetect(), Mag_init()

지자기 센서와 연결되었는지 확인하는 방법은 Identification Register A의 정보가 'H' 값인지 판단하는 것이다. Identification Register A,B,C는 각각 ASCII코드로 H43 값을 가진다.

HMC5883L 센서를 확인한 후, 지자기 센서를 처리하기위한 초기화 과정을 mag.init()함수를 통해 이뤄진다.

bool hmc5883lDetect(sensor_t *mag)
{
    bool ack = false;
    uint8_t sig = 0;

    ack = i2cRead(MAG_ADDRESS, 0x0A, 1, &sig);
    if (!ack || sig != 'H')
        return false;

    mag->init = hmc5883lInit;
    mag->read = hmc5883lRead;
    
    return true;
}

sensor_t mag;  // mag access functions
void Mag_init(void)
{
    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
    LED1_ON;
    mag.init(ALIGN_DEFAULT);
    LED1_OFF;
}

hmc5883lInit(), mag.init() 함수

HMC5883L 센서의 환경설정 코드다.

• HMC58X3_R_CONFA = 0x010 + HMC_POS_BIAS(1)
  • Output Rate = 15 Hz (default)
  • X,Y 축은 양수, Z축은 음수로 바이어스가 적용된다.
• HMC58X3_R_CONFB = 0x60
  • 센싱 범위 +-2.5Ga, Scale Factor = 614
• 500 millisecond 동안 양의 바이어스로 센서 측정
  • HMC58X3_R_MODE = 1
    • Single-Measurement Mode로 센싱한다.
• 500 millisecond 동안 음의 바이어스로 센서 측정
  • HMC58X3_R_CONFA = 0x010 + HMC_NEG_BIAS(2)
  • HMC58X3_R_MODE = 1
    • Single-Measurement Mode로 센싱한다.
• 1 초 동안 측정한 센서 값으로 최종 이득을 계산한다.
  • 측정 범위를 +-2.5g 로 늘렸기 때문에, Scale Factor가 660이 되어야 한다. 그 밖에 계산식은 아직 이해 불가.

magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);

• 마지막으로 환경설정을 복구하고, 센싱에 돌입한다.
  • HMC58X3_R_CONFA = 0x70
    • output rate: 15Hz, normal measurement mode
  • HMC58X3_R_CONFB = 0x20
    • default +-1.2Ga, Scale Factor = 1024
  • HMC58X3_R_MODE = 0
    • Continuous-Measurement Mode

센서 초기화 코드는 다음과 같다.

void hmc5883lInit(sensor_align_e align)
{
    gpio_config_t gpio;
    int16_t magADC[3];
    int i;
    int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
    bool bret = true;           // Error indicator

    if (align > 0)
        magAlign = align;
    
    gpio.pin = Pin_12;
    gpio.speed = Speed_2MHz;
    gpio.mode = Mode_IN_FLOATING;
    gpioInit(GPIOB, &gpio);

    delay(50);
    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
    // Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
    // The new gain setting is effective from the second measurement and on.
    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
    delay(100);
    hmc5883lRead(magADC);

    for (i = 0; i < 10; i++) {  // Collect 10 samples
        i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
        delay(50);
        hmc5883lRead(magADC);       // Get the raw values in case the scales have already been changed.

        // Since the measurements are noisy, they should be averaged rather than taking the max.
        xyz_total[X] += magADC[X];
        xyz_total[Y] += magADC[Y];
        xyz_total[Z] += magADC[Z];

        // Detect saturation.
        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
            bret = false;
            break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
        }
        LED1_TOGGLE;
    }

    // Apply the negative bias. (Same gain)
    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
    for (i = 0; i < 10; i++) {
        i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
        delay(50);
        hmc5883lRead(magADC);               // Get the raw values in case the scales have already been changed.

        // Since the measurements are noisy, they should be averaged.
        xyz_total[X] -= magADC[X];
        xyz_total[Y] -= magADC[Y];
        xyz_total[Z] -= magADC[Z];

        // Detect saturation.
        if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
            bret = false;
            break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
        }
        LED1_TOGGLE;
    }

    magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
    magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
    magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);

    // leave test mode
    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70);   // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
    i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20);   // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
    i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00);    // Mode register             -- 000000 00    continuous Conversion Mode
    delay(100);

    if (!bret) {                // Something went wrong so get a best guess
        magGain[X] = 1.0f;
        magGain[Y] = 1.0f;
        magGain[Z] = 1.0f;
    }
}

센서 정보 가져오기

지자기 센서 정보를 읽어와 magGain[]을 곱해서 저장한다.

void hmc5883lRead(int16_t *magData)
{
    uint8_t buf[6];
    int16_t mag[3];

    i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
    // During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
    // After calibration is done, magGain is set to calculated gain values.
    mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
    mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
    mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
    
    alignSensors(mag, magData, magAlign);	
}

Main() 함수

HMC5883 센서와 연결되었는지 확인하고, 초기화 과정을 거친 후, 센서 정보를 가져온다. i2c통신이 인터럽트 방식으로 처리되기 때문에 코드가 깔끔해졌다. 센서와의 i2c통신은 drv_i2c에서 처리하고, 이를 받아서 원하는 정보로 바꾸는것은 drv_hmc5883l에서 처리하고 있다.

sensor_t mag;  // mag access functions

void Mag_init(void)
{
    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
    LED1_ON;
    mag.init(ALIGN_DEFAULT);
    LED1_OFF;
}

int main()
{
  setup();
	
  // HMC5883L과 연결되었는지 확인.
  if (!hmc5883lDetect(&mag))
  {
     return 1;
  }
	
  // HMC5883L 지자기센서 초기화
  Mag_init();
  printf( "Init Magnetometer\n" );
		
  while(1)
  {
    int16_t Mag[3];

    // 지자기 센서정보를 i2c통신을 통해 읽어온다.
    mag.read(Mag);

    print_byte('S');
    print_short( Mag[ 0] );
    print_short( Mag[ 1] );
    print_short( Mag[ 2] );
		
    delayMicroseconds(10000);		
  }
}