IMU 센서 활용 — MPU6050·LSM6DSO·Sensor Fusion

1
#define MPU_ADDR 0x68
2

3
uint8_t mpu_read(uint8_t reg) {
4
    uint8_t v;
5
    i2c_read_reg(MPU_ADDR, reg, &v, 1);
6
    return v;
7
}
8

9
void mpu_write(uint8_t reg, uint8_t val) {
10
    uint8_t buf[2] = {reg, val};
11
    i2c_write(MPU_ADDR, buf, 2);
12
}
13

14
int mpu_init(void) {
15
    if (mpu_read(0x75) != 0x68) return -1;
16

17
    mpu_write(0x6B, 0x80);        // reset
18
    delay_ms(100);
19
    mpu_write(0x6B, 0x01);        // wake up, gyro clock
20
    mpu_write(0x1A, 0x03);        // DLPF: 44 Hz BW
21
    mpu_write(0x19, 9);           // sample rate 1kHz / (1+9) = 100 Hz
22
    mpu_write(0x1B, (1 << 3));    // gyro ±500 °/s
23
    mpu_write(0x1C, (1 << 3));    // accel ±4 g
24
    return 0;
25
}
26

27
typedef struct {
28
    float ax, ay, az;   // g
29
    float gx, gy, gz;   // °/s
30
    float temp_c;
31
} mpu_data_t;
32

33
void mpu_read_all(mpu_data_t *d) {
34
    uint8_t buf[14];
35
    i2c_read_reg(MPU_ADDR, 0x3B, buf, 14);
36

37
    int16_t ax = (buf[0]  << 8) | buf[1];
38
    int16_t ay = (buf[2]  << 8) | buf[3];
39
    int16_t az = (buf[4]  << 8) | buf[5];
40
    int16_t t  = (buf[6]  << 8) | buf[7];
41
    int16_t gx = (buf[8]  << 8) | buf[9];
42
    int16_t gy = (buf[10] << 8) | buf[11];
43
    int16_t gz = (buf[12] << 8) | buf[13];
44

45
    d->ax = ax / 8192.0f;   // ±4 g → 8192
46
    d->ay = ay / 8192.0f;
47
    d->az = az / 8192.0f;
48
    d->gx = gx / 65.5f;     // ±500 °/s → 65.5
49
    d->gy = gy / 65.5f;
50
    d->gz = gz / 65.5f;
51
    d->temp_c = t / 340.0f + 36.53f;
52
}

1
typedef struct {
2
    float bx, by, bz;
3
} gyro_bias_t;
4

5
void gyro_calibrate(gyro_bias_t *b) {
6
    // 보드를 정지시킨 상태에서 호출
7
    float sum_x = 0, sum_y = 0, sum_z = 0;
8
    const int N = 500;
9

10
    for (int i = 0; i < N; i++) {
11
        mpu_data_t d;
12
        mpu_read_all(&d);
13
        sum_x += d.gx;
14
        sum_y += d.gy;
15
        sum_z += d.gz;
16
        delay_ms(2);
17
    }
18
    b->bx = sum_x / N;
19
    b->by = sum_y / N;
20
    b->bz = sum_z / N;
21
}
22

23
void mpu_read_calibrated(mpu_data_t *d, const gyro_bias_t *b) {
24
    mpu_read_all(d);
25
    d->gx -= b->bx;
26
    d->gy -= b->by;
27
    d->gz -= b->bz;
28
}

1
float atan2_deg(float y, float x) {
2
    return atan2f(y, x) * (180.0f / 3.14159f);
3
}
4

5
void accel_to_rp(const mpu_data_t *d, float *roll, float *pitch) {
6
    *roll  = atan2_deg(d->ay, d->az);
7
    *pitch = atan2_deg(-d->ax, sqrtf(d->ay * d->ay + d->az * d->az));
8
}

1
typedef struct {
2
    float roll, pitch;
3
    float dt;
4
    float alpha;   // 0.98 등
5
} compl_t;
6

7
void compl_update(compl_t *f, const mpu_data_t *d) {
8
    float roll_acc, pitch_acc;
9
    accel_to_rp(d, &roll_acc, &pitch_acc);
10

11
    // gyro integration
12
    f->roll  += d->gx * f->dt;
13
    f->pitch += d->gy * f->dt;
14

15
    // blend
16
    f->roll  = f->alpha * f->roll  + (1 - f->alpha) * roll_acc;
17
    f->pitch = f->alpha * f->pitch + (1 - f->alpha) * pitch_acc;
18
}

1
// MPU INT pin = PA0 EXTI0
2
void mpu_int_init(void) {
3
    mpu_write(0x37, 0x20);   // INT pin: latched, clear on read
4
    mpu_write(0x38, 0x01);   // data ready interrupt enable
5

6
    EXTI->IMR  |= (1u << 0);
7
    EXTI->RTSR |= (1u << 0);
8
    NVIC_EnableIRQ(EXTI0_IRQn);
9
}
10

11
volatile int data_ready;
12

13
void EXTI0_IRQHandler(void) {
14
    EXTI->PR = (1u << 0);
15
    data_ready = 1;
16
}
17

18
// main
19
while (1) {
20
    if (data_ready) {
21
        data_ready = 0;
22
        mpu_read_all(&d);
23
        process(&d);
24
    }
25
}