フェーズ3

E3-06 ピッチ/ロール推定（最小）：加速度から傾きをそれっぽく出してみる

シリーズ：実験シリーズ（フェーズ3）
対応ロードマップ：フェーズ3 / E3-06
この記事で扱う範囲：E3-05 でIMU値のズレを補正する考え方を確認しました。今回は、IMUの加速度X/Y/Zを使って、ボードの傾きである ピッチ角 と ロール角 を計算し、UARTログに表示します。

1. 目的

今回は、IMUの加速度値から ピッチ角 と ロール角 を計算します。
E3-04では、IMUから加速度3軸とジャイロ3軸の生値を読みました。
E3-05では、静止時のズレを平均して引く、簡易オフセット補正を行いました。
今回は、その次のステップとして、加速度の値を使って、

  • ボードが前後に傾いているか
  • ボードが左右に傾いているか

を数値で見えるようにします。
今回のゴールは、正確な姿勢推定ではありません。
まずは、

  • 加速度には重力の向きが含まれている
  • 重力の向きから傾きが分かる
  • 傾きを角度としてログに出せる

という流れを体験することです。

2. 前提・環境

2-1. 前提

この記事は、次の内容が終わっている前提で進めます。

  • フェーズ0：ビルド / 書き込み / main到達 / UARTログ出力
  • E2-02：1msごとの時刻を作る
  • E2-03：周期処理
  • E2-05：状態で整理（IDLE / RUN / ERROR）
  • E3-00：I2C配線の“詰まりどころ”を先に潰す
  • E3-01：0x68 に対して応答があるか確認する
  • E3-02：WHO_AM_I を1バイト読める
  • E3-03：加速度X軸を2バイト連続で読める
  • E3-04：加速度3軸とジャイロ3軸の生値を周期的に読める
  • E3-05：静止時のズレを平均して引く考え方を確認している

2-2. 使用するもの

  • 評価ボード：RTK7EKA8M2S00001BE
  • e² studio + FSP
  • Tera Term
  • IMUモジュール：QCIOT-ICM42688P（PMOD BOARD ICM-42688-P）
  • USB-UART変換ケーブル（またはUSB-UART変換モジュール）
  • ジャンパ線（UART接続用）

2-3. 今回の前提条件

接続条件は、E3-04 / E3-05 と同じ下記の条件です。

  • 通信方式：I2C
  • 接続方法：QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
  • 評価ボード側設定：SW4-1 = OFF、SW4-2 = ON
  • AD0：Low（J4 にジャンパキャップあり）
  • I2Cアドレス：0x68
  • I2C通信速度：100kHz

2-4. 今回使うレジスタ

今回は、加速度X/Y/Zを使います。
ただし、コードは E3-04 / E3-05 と同じように、0x1F から 0x2A までの12バイトを読みます。

データ上位バイト下位バイト
加速度XACCEL_DATA_X1 = 0x1FACCEL_DATA_X0 = 0x20
加速度YACCEL_DATA_Y1 = 0x21ACCEL_DATA_Y0 = 0x22
加速度ZACCEL_DATA_Z1 = 0x23ACCEL_DATA_Z0 = 0x24
ジャイロXGYRO_DATA_X1 = 0x25GYRO_DATA_X0 = 0x26
ジャイロYGYRO_DATA_Y1 = 0x27GYRO_DATA_Y0 = 0x28
ジャイロZGYRO_DATA_Z1 = 0x29GYRO_DATA_Z0 = 0x2A

今回のピッチ/ロール計算では、主に次の3つを使います。

accel_xaccel_yaccel_z

ジャイロ値は、今回の角度計算には使いません。
ジャイロを使った姿勢推定や、加速度とジャイロを組み合わせる処理は、もう少し後の段階で扱います。

3. 今回の変更点

3-1. 配線変更

今回は配線変更はありません。
E3-04 / E3-05 と同じ下記内容です。

  • QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
  • 評価ボード側は SW4-1 = OFF、SW4-2 = ON
  • J4 にジャンパキャップあり

3-2. 設定変更

FSP の I2C Master 設定は、E3-04 / E3-05 と同じです。

  • 通信方式：I2C Master
  • アドレス幅：7bit
  • 通信速度：100kHz
  • スレーブアドレス：0x68
  • Callback：i2c_master_callback

3-3. コード変更

今回は、E3-04 / E3-05 のコードに、次の処理を追加します。

  • 加速度X/Y/Zを読み取る
  • 加速度X/Y/Zからピッチ角を計算する
  • 加速度X/Y/Zからロール角を計算する
  • ピッチ角とロール角をUARTログに出す

今回の中心は、次の考え方です。

加速度X/Y/Zに含まれる重力の向きから、ボードの傾きを計算する

E3-05では、加速度の補正に注意が必要と説明しました。
理由は、加速度には静止時でも 重力 が入るためです。

今回は、その重力を邪魔なものとして消すのではなく、
重力の向きを利用して傾きを求める 方向に進みます。

4. ピッチ/ロールとは何か

4-1. ロールとは

ロールは、ざっくり言うと 左右方向の傾き です。
たとえば、評価ボードを机の上に置いた状態から、

  • 左側を持ち上げる
  • 右側を持ち上げる

ように傾けたときに変わる角度を、この記事ではロールとして扱います。

4-2. ピッチとは

ピッチは、ざっくり言うと 前後方向の傾き です。
たとえば、評価ボードを机の上に置いた状態から、

  • 手前側を持ち上げる
  • 奥側を持ち上げる

ように傾けたときに変わる角度を、この記事ではピッチとして扱います。

4-3. 軸の向きはボードの置き方で見え方が変わる

ここで注意があります。
ピッチ/ロールの符号や、どちらに傾けたときにプラスになるかは、

  • IMUモジュールの向き
  • 評価ボードへの取り付け向き
  • 自分が「前」と決めた向き

によって変わります。
そのため、この記事では最初から厳密な向きを決めすぎません。
まずは、

  • 前後に傾けたら PITCH が大きく変わる
  • 左右に傾けたら ROLL が大きく変わる
  • 水平に戻すと、だいたい0付近に戻る

という見方で確認します。

5. 計算の考え方

5-1. 加速度には重力が入っている

ボードを静止させていても、加速度センサの値は0になりません。
たとえば、机の上に置いたときに次のような値になることがあります。

ACC X= -120 Y= 340 Z= 16320

この Z=16320 のような値は、異常ではありません。
ボードの置き方によって、Z軸方向に重力が見えているためです。
つまり、加速度センサは静止中でも、

今、重力がどの軸方向に見えているか

を教えてくれます。
この重力の見え方を使うと、ボードの傾きを計算できます。

5-2. 今回使う式

今回は、次の式でピッチ角とロール角を計算します。

roll  = atan2f(accel_y, accel_z) * 180.0f / PI;pitch = atan2f(-accel_x, sqrtf(accel_y * accel_y + accel_z * accel_z)) * 180.0f / PI;

難しく見えますが、最初は次の理解で大丈夫です。

  • atan2f()：比率から角度を求める関数
  • sqrtf()：平方根を求める関数
  • 180.0f / PI：ラジアンを度に変換するための係数

atan2f() が返す角度は、最初はラジアンという単位です。
Tera Termに表示するときは、見やすいように 度（deg） に変換します。

5-3. なぜg単位に変換しなくても計算できるのか

加速度センサの値は、本来は設定に応じて g などの単位に変換できます。
ただし今回のピッチ/ロール計算では、加速度の 比率 を使います。
たとえば、すべての軸の値を同じ倍率で変換するだけなら、角度の計算結果は大きく変わりません。
そのため今回は、初心者向けの最小実装として、
生値のままピッチ/ロールを計算します。
もちろん、将来的には、

  • センサのレンジ設定
  • LSB/g
  • 単位変換
  • オフセット補正
  • フィルタ処理

を整理した方がよいです。
ただし今回は、姿勢の数値化を体験することを優先します。

6. 手順

6-1. E3-04 / E3-05 と同じ条件で接続する

まず、これまでと同じ条件で、QCIOT-ICM42688P と評価ボードを接続します。

  • QCIOT-ICM42688P の J1 を評価ボードの Pmod1（J26）へ接続
  • SW4-1 = OFF
  • SW4-2 = ON
  • J4 にジャンパキャップあり

6-2. プログラムを書き込む

今回のコードを書き込みます。
コードの中心は、次の3つです。

  • imu_read_raw_data()
    加速度3軸とジャイロ3軸を読む
  • imu_calc_pitch_roll()
    加速度X/Y/Zからピッチ/ロールを計算する
  • print_imu_angle_data()
    加速度とピッチ/ロールをログに出す

6-3. Tera Termを開く

E3-04 / E3-05 と同じように、Tera TermでUARTログを確認します。
通信条件は、自分のプロジェクト設定に合わせます。
これまでの実験と同じ設定でログが出ていれば、そのままで問題ありません。

6-4. ボードを水平に置いてログを見る

まず、評価ボードを机の上に静かに置きます。
水平に近い状態で、次のようなログが出ることを確認します。

ACC X=  -120 Y=   340 Z= 16320 | PITCH=   0.4 deg ROLL=   1.2 deg

完全に0になる必要はありません。
机の傾き、ボードの取り付け向き、センサの個体差で少しズレます。

6-5. 前後に傾ける

次に、ボードをゆっくり前後に傾けます。
このとき、PITCH の値が大きく変わればOKです。

ACC X=  5200 Y=   200 Z= 15400 | PITCH= -18.7 deg ROLL=   0.7 deg

6-6. 左右に傾ける

次に、ボードをゆっくり左右に傾けます。
このとき、ROLL の値が大きく変わればOKです。

ACC X=  -100 Y= -6200 Z= 15000 | PITCH=   0.4 deg ROLL= -22.5 deg

7. コード

※下記は考え方を分かりやすくするためのシンプルな例です。
※ I2Cインスタンス名、UARTインスタンス名、コールバック名は、自分のプロジェクトに合わせて置き換えてください。
※ FSPの設定や使用しているI2Cモジュールにより、R_IIC_MASTER_... の部分は環境に合わせて調整してください。
※ atan2f() / sqrtf() を使うため、math.h をインクルードします。

#include "hal_data.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <math.h>

#define IMU_I2C_ADDR                 (0x68U)

/* ICM-42688-P Register */
#define IMU_REG_ACCEL_DATA_X1        (0x1FU)
#define IMU_REG_PWR_MGMT0            (0x4EU)
#define IMU_REG_GYRO_CONFIG0         (0x4FU)
#define IMU_REG_ACCEL_CONFIG0        (0x50U)

/* 0x1F から 0x2A まで読むので 12バイト */
#define IMU_RAW_DATA_LENGTH          (12U)

/* 設定値 */
#define IMU_PWR_MGMT0_ACCEL_GYRO_LN  (0x0FU)
#define IMU_GYRO_CONFIG0_100HZ       (0x08U)
#define IMU_ACCEL_CONFIG0_2G_100HZ   (0x68U)

#define I2C_TIMEOUT_COUNT            (100000U)
#define IMU_LOG_INTERVAL_MS          (500U)

#define IMU_RAD_TO_DEG               (57.2957795f)

typedef struct st_imu_raw_data
{
    int16_t accel_x;
    int16_t accel_y;
    int16_t accel_z;
    int16_t gyro_x;
    int16_t gyro_y;
    int16_t gyro_z;
} imu_raw_data_t;

typedef struct st_imu_angle
{
    float pitch_deg;
    float roll_deg;
} imu_angle_t;

static volatile bool g_i2c_done  = false;
static volatile bool g_i2c_error = false;
static volatile uint32_t g_ms_count = 0U;

static void uart_print(const char * p_text);

static bool imu_write_register_1byte(uint8_t reg_addr, uint8_t value);
static bool imu_read_register_bytes(uint8_t reg_addr, uint8_t * p_buffer, uint32_t length);
static bool imu_init(void);
static bool imu_read_raw_data(imu_raw_data_t * p_data);
static void imu_calc_pitch_roll(const imu_raw_data_t * p_raw, imu_angle_t * p_angle);

static int16_t make_int16(uint8_t upper, uint8_t lower);
static void print_imu_angle_data(const imu_raw_data_t * p_raw, const imu_angle_t * p_angle);

void i2c_master_callback(i2c_master_callback_args_t * p_args)
{
    if (NULL == p_args)
    {
        return;
    }

    switch (p_args->event)
    {
        case I2C_MASTER_EVENT_ABORTED:
        {
            g_i2c_error = true;
            g_i2c_done  = true;
            break;
        }

        case I2C_MASTER_EVENT_RX_COMPLETE:
        case I2C_MASTER_EVENT_TX_COMPLETE:
        {
            g_i2c_error = false;
            g_i2c_done  = true;
            break;
        }

        default:
        {
            break;
        }
    }
}

/* E2-02 / E2-03 で作った 1msカウンタ用のコールバック例 */
void timer0_callback(timer_callback_args_t * p_args)
{
    if (NULL == p_args)
    {
        return;
    }

    if (TIMER_EVENT_CYCLE_END == p_args->event)
    {
        g_ms_count++;
    }
}

void hal_entry(void)
{
    fsp_err_t err;
    imu_raw_data_t imu_raw;
    imu_angle_t imu_angle;
    uint32_t last_log_ms = 0U;

    err = R_SCI_B_UART_Open(&g_uart0_ctrl, &g_uart0_cfg);
    if (FSP_SUCCESS != err)
    {
        while (1)
        {
            ;
        }
    }

    uart_print("IMU pitch/roll start\r\n");

    err = R_IIC_MASTER_Open(&g_i2c_master0_ctrl, &g_i2c_master0_cfg);
    if (FSP_SUCCESS != err)
    {
        uart_print("I2C open error\r\n");
        while (1)
        {
            ;
        }
    }

    err = R_IIC_MASTER_SlaveAddressSet(&g_i2c_master0_ctrl,
                                       IMU_I2C_ADDR,
                                       I2C_MASTER_ADDR_MODE_7BIT);
    if (FSP_SUCCESS != err)
    {
        uart_print("Slave address set error\r\n");
        while (1)
        {
            ;
        }
    }

    if (!imu_init())
    {
        uart_print("IMU init error\r\n");
        while (1)
        {
            ;
        }
    }

    uart_print("IMU init OK\r\n");

    while (1)
    {
        if ((g_ms_count - last_log_ms) >= IMU_LOG_INTERVAL_MS)
        {
            last_log_ms += IMU_LOG_INTERVAL_MS;

            if (imu_read_raw_data(&imu_raw))
            {
                imu_calc_pitch_roll(&imu_raw, &imu_angle);
                print_imu_angle_data(&imu_raw, &imu_angle);
            }
            else
            {
                uart_print("IMU raw read error\r\n");
            }
        }
    }
}

static bool imu_init(void)
{
    if (!imu_write_register_1byte(IMU_REG_PWR_MGMT0, IMU_PWR_MGMT0_ACCEL_GYRO_LN))
    {
        return false;
    }

    /* 加速度・ジャイロをOFFからONにした直後は少し待つ */
    for (volatile uint32_t i = 0; i < 50000U; i++)
    {
        __asm volatile ("nop");
    }

    if (!imu_write_register_1byte(IMU_REG_GYRO_CONFIG0, IMU_GYRO_CONFIG0_100HZ))
    {
        return false;
    }

    if (!imu_write_register_1byte(IMU_REG_ACCEL_CONFIG0, IMU_ACCEL_CONFIG0_2G_100HZ))
    {
        return false;
    }

    return true;
}

static bool imu_read_raw_data(imu_raw_data_t * p_data)
{
    uint8_t raw[IMU_RAW_DATA_LENGTH];

    if (NULL == p_data)
    {
        return false;
    }

    if (!imu_read_register_bytes(IMU_REG_ACCEL_DATA_X1, raw, IMU_RAW_DATA_LENGTH))
    {
        return false;
    }

    p_data->accel_x = make_int16(raw[0],  raw[1]);
    p_data->accel_y = make_int16(raw[2],  raw[3]);
    p_data->accel_z = make_int16(raw[4],  raw[5]);
    p_data->gyro_x  = make_int16(raw[6],  raw[7]);
    p_data->gyro_y  = make_int16(raw[8],  raw[9]);
    p_data->gyro_z  = make_int16(raw[10], raw[11]);

    return true;
}

static void imu_calc_pitch_roll(const imu_raw_data_t * p_raw, imu_angle_t * p_angle)
{
    float ax;
    float ay;
    float az;

    if ((NULL == p_raw) || (NULL == p_angle))
    {
        return;
    }

    ax = (float)p_raw->accel_x;
    ay = (float)p_raw->accel_y;
    az = (float)p_raw->accel_z;

    /*
     * 最小実装：
     * 加速度の生値をそのまま使って、重力方向から傾きを求める。
     *
     * roll  : 左右方向の傾き
     * pitch : 前後方向の傾き
     *
     * 符号や向きは、IMUモジュールの取り付け向きで変わる。
     */
    p_angle->roll_deg  = atan2f(ay, az) * IMU_RAD_TO_DEG;
    p_angle->pitch_deg = atan2f(-ax, sqrtf((ay * ay) + (az * az))) * IMU_RAD_TO_DEG;
}

static int16_t make_int16(uint8_t upper, uint8_t lower)
{
    return (int16_t)(((uint16_t)upper << 8) | lower);
}

static bool imu_write_register_1byte(uint8_t reg_addr, uint8_t value)
{
    fsp_err_t err;
    uint8_t write_buf[2];
    uint32_t timeout;

    write_buf[0] = reg_addr;
    write_buf[1] = value;

    g_i2c_done  = false;
    g_i2c_error = false;

    err = R_IIC_MASTER_Write(&g_i2c_master0_ctrl, write_buf, 2U, false);
    if (FSP_SUCCESS != err)
    {
        return false;
    }

    timeout = I2C_TIMEOUT_COUNT;
    while ((false == g_i2c_done) && (timeout > 0U))
    {
        timeout--;
    }

    if ((0U == timeout) || (true == g_i2c_error))
    {
        return false;
    }

    return true;
}

static bool imu_read_register_bytes(uint8_t reg_addr, uint8_t * p_buffer, uint32_t length)
{
    fsp_err_t err;
    uint32_t timeout;

    if ((NULL == p_buffer) || (0U == length))
    {
        return false;
    }

    /* 1) 読みたい先頭レジスタ番号を書く */
    g_i2c_done  = false;
    g_i2c_error = false;

    err = R_IIC_MASTER_Write(&g_i2c_master0_ctrl, ®_addr, 1U, true);
    if (FSP_SUCCESS != err)
    {
        return false;
    }

    timeout = I2C_TIMEOUT_COUNT;
    while ((false == g_i2c_done) && (timeout > 0U))
    {
        timeout--;
    }

    if ((0U == timeout) || (true == g_i2c_error))
    {
        return false;
    }

    /* 2) 指定バイト数だけ連続で読む */
    g_i2c_done  = false;
    g_i2c_error = false;

    err = R_IIC_MASTER_Read(&g_i2c_master0_ctrl, p_buffer, length, false);
    if (FSP_SUCCESS != err)
    {
        return false;
    }

    timeout = I2C_TIMEOUT_COUNT;
    while ((false == g_i2c_done) && (timeout > 0U))
    {
        timeout--;
    }

    if ((0U == timeout) || (true == g_i2c_error))
    {
        return false;
    }

    return true;
}

static void print_imu_angle_data(const imu_raw_data_t * p_raw, const imu_angle_t * p_angle)
{
    char msg[200];

    if ((NULL == p_raw) || (NULL == p_angle))
    {
        return;
    }

    snprintf(msg, sizeof(msg),
             "ACC X=%6d Y=%6d Z=%6d | PITCH=%7.2f deg ROLL=%7.2f deg\r\n",
             p_raw->accel_x,
             p_raw->accel_y,
             p_raw->accel_z,
             p_angle->pitch_deg,
             p_angle->roll_deg);

    uart_print(msg);
}

static void uart_print(const char * p_text)
{
    fsp_err_t err;

    err = R_SCI_B_UART_Write(&g_uart0_ctrl, (uint8_t *) p_text, strlen(p_text));
    if (FSP_SUCCESS != err)
    {
        while (1)
        {
            ;
        }
    }

    /*
     * UART送信完了待ちの簡易版。
     * 本格的にはUARTコールバックで送信完了を待つ形にする。
     */
    for (volatile uint32_t i = 0; i < 1000000U; i++)
    {
        __asm volatile ("nop");
    }
}

8. コードの見方（今回大事なところだけ）

8-1. 今回追加した構造体

今回は、ピッチ角とロール角をまとめるために、次の構造体を追加しています。

typedef struct st_imu_angle
{
    float pitch_deg;
    float roll_deg;
} imu_angle_t;

pitch_deg はピッチ角です。
roll_deg はロール角です。
deg は degree の略で、角度の「度」を意味します。

8-2. 加速度値を float に変換する

imu_read_raw_data() で読んだ加速度値は int16_t です。

int16_t accel_x;
int16_t accel_y;
int16_t accel_z;

しかし、角度計算では小数を扱います。
そのため、計算前に float に変換しています。

ax = (float)p_raw->accel_x;
ay = (float)p_raw->accel_y;
az = (float)p_raw->accel_z;

ここでは、単位変換までは行っていません。
生値をそのまま小数計算に使える形にしているだけです。

8-3. ロール角を計算する

ロール角は、次の式で計算しています。

p_angle->roll_deg = atan2f(ay, az) * IMU_RAD_TO_DEG;

ここでは、Y軸とZ軸の加速度の関係から、左右方向の傾きを求めています。
ボードを左右に傾けると、重力がY軸方向にも見えるようになります。
その変化を使って、ロール角を計算しています。

8-4. ピッチ角を計算する

ピッチ角は、次の式で計算しています。

p_angle->pitch_deg = atan2f(-ax, sqrtf((ay * ay) + (az * az))) * IMU_RAD_TO_DEG;

ここでは、X軸方向の加速度と、Y/Z方向の合成値を使って、前後方向の傾きを求めています。
sqrtf((ay * ay) + (az * az)) は、Y軸とZ軸を合わせた大きさを求めています。
最初は式を完全に理解できなくても大丈夫です。
この記事では、次のように捉えれば十分です。

X軸方向に重力がどれくらい見えているかで、前後の傾きを求めている

8-5. atan2f() と sqrtf() について

今回、次の数学関数を使っています。

atan2f()
sqrtf()

そのため、先頭で次をインクルードしています。

#include <math.h>

もしビルド時に atan2f や sqrtf に関するリンクエラーが出る場合は、数学ライブラリのリンク設定が必要になることがあります。
その場合は、e² studio のプロジェクト設定で、数学ライブラリをリンクする設定を確認します。
環境によっては、-lm の追加が必要になる場合があります。

8-6. 加速度のオフセット補正は今回は使わない

E3-05では、加速度にもオフセット補正を入れました。
ただし今回は、ピッチ/ロール計算に使う加速度は、基本的に 生値 を使います。
理由は、加速度には重力が入っているからです。
ピッチ/ロール計算では、この重力の向きを利用します。
もし静止時の加速度平均をそのまま全部引いてしまうと、重力成分まで消してしまい、傾き計算に使いにくくなります。
そのため、今回の最小実装では、

  • ジャイロ補正：前回の考え方として理解する
  • 加速度の重力成分：今回のピッチ/ロール計算に使う

と分けて考えます。

9. 実行結果

9-1. 水平に置いたとき

まず、ボードを机の上に置きます。
このとき、ログは次のようになります。

IMU pitch/roll startIMU init OKACC X=  -120 Y=   340 Z= 16320 | PITCH=   0.42 deg ROLL=   1.19 degACC X=  -130 Y=   360 Z= 16310 | PITCH=   0.46 deg ROLL=   1.26 degACC X=  -110 Y=   330 Z= 16330 | PITCH=   0.39 deg ROLL=   1.16 deg

ピッチ/ロールが完全に0にならなくても問題ありません。

見るポイントは、

  • 値が極端に大きくないこと
  • 静止しているときに大きく暴れないこと
  • 少し揺れても、だいたい同じ範囲に戻ること

です。

9-2. 前後に傾けたとき

次に、ボードを前後に傾けます。

ACC X=  5200 Y=   200 Z= 15400 | PITCH= -18.65 deg ROLL=   0.74 degACC X=  8300 Y=   180 Z= 13900 | PITCH= -30.85 deg ROLL=   0.74 degACC X=  3000 Y=   210 Z= 16000 | PITCH= -10.61 deg ROLL=   0.75 deg

前後に傾けたときに、主に PITCH が変わればOKです。

9-3. 左右に傾けたとき

次に、ボードを左右に傾けます。

ACC X=  -100 Y= -6200 Z= 15000 | PITCH=   0.35 deg ROLL= -22.47 degACC X=  -140 Y= -9100 Z= 13400 | PITCH=   0.49 deg ROLL= -34.18 degACC X=  -120 Y= -3000 Z= 16000 | PITCH=   0.42 deg ROLL= -10.62 deg

左右に傾けたときに、主に ROLL が変わればOKです。

9-4. 符号が逆でも慌てない

ボードを前に傾けたつもりなのに、PITCH がマイナスになる場合があります。

これは異常とは限りません。

符号は、

  • IMUの軸向き
  • ボードの向き
  • 自分が前後左右をどう決めたか
  • 式の書き方

で変わります。

今回の段階では、

傾けた方向に応じて、角度の値が変わる

ことを確認できればOKです。

10. ハマりポイント／原因と対策

10-1. IMU raw read error になる

原因

  • I2C読み取りが失敗している
  • E3-04の生値取得が安定していない
  • レジスタアドレスを書いたあとに読み取りへ進めていない
  • I2Cコールバックで完了イベントを受け取れていない
  • タイムアウト待ちが短すぎる

対策

  • まず E3-04 に戻り、加速度/ジャイロの生値が読める状態に戻す
  • ACC X= ... Y= ... Z= ... が出ることを確認する
  • R_IIC_MASTER_Write() のあとに、完了待ちが入っているか確認する
  • R_IIC_MASTER_Read() のあとに、完了待ちが入っているか確認する
  • I2C_MASTER_EVENT_TX_COMPLETE / I2C_MASTER_EVENT_RX_COMPLETE を受けているか確認する

10-2. ビルドエラーになる

原因

  • math.h をインクルードしていない
  • atan2f() や sqrtf() の関数名を間違えている
  • 数学ライブラリがリンクされていない
  • float の変数宣言を追加し忘れている
  • imu_angle_t の構造体定義を追加していない

対策

  • 先頭に #include <math.h> があるか確認する
  • atan2f / sqrtf のつづりを確認する
  • imu_angle_t の構造体定義があるか確認する
  • imu_calc_pitch_roll() の宣言と定義があるか確認する
  • atan2f / sqrtf のリンクエラーの場合は、数学ライブラリのリンク設定を確認する

10-3. ピッチ/ロールが nan になる

原因

  • 計算に使っている加速度値が異常になっている
  • I2C読み取りに失敗した値を使っている
  • accel_x / accel_y / accel_z が正しく更新されていない
  • メモリの扱いを間違えている

対策

  • まず加速度の生値をログに出して確認する
  • ACC X/Y/Z が極端におかしくないか確認する
  • imu_read_raw_data() が成功したときだけ角度計算する
  • p_raw や p_angle が NULL でないか確認する
  • 読み取り失敗時は角度計算せず、エラーログを出す

10-4. 水平に置いても0度にならない

原因

  • 机が完全に水平ではない
  • IMUモジュールが評価ボードに対して少し傾いている
  • センサ値にノイズや個体差がある
  • ボードの向きと式の前提が完全には一致していない

対策

  • 完全に0度にならなくても問題ないと考える
  • まずは数度以内ならOKとする
  • ボードを同じ向きで置いて、値が大きく暴れないかを見る
  • 必要になったら、後の回で角度オフセットを考える
  • 今回は「傾けたら値が変わる」ことを優先して確認する

10-5. 前後に傾けてもロールが変わる／左右に傾けてもピッチが変わる

原因

  • ボードを完全に1方向だけに傾けるのが難しい
  • IMUの軸と自分が考えている前後左右が一致していない
  • モジュールの取り付け向きが想定と違う
  • 式の符号や軸の対応が、実際のボード向きと合っていない

対策

  • まずは1方向ずつ、ゆっくり傾ける
  • どの方向に傾けると、どの値が大きく変わるかメモする
  • 必要なら accel_x / accel_y の使い方を入れ替える
  • 符号が逆なら、式の -ax を ax にするなど、後で調整する
  • 今回は厳密な軸合わせより、変化の確認を優先する

10-6. 値が細かく揺れて読みにくい

原因

  • 加速度センサにはノイズがある
  • 手で持つと微妙に揺れている
  • 机やケーブルの振動が入っている
  • 500msごとの表示でも、瞬間値をそのまま出している

対策

  • ボードを机に置いて確認する
  • 手で持つ場合は、できるだけゆっくり傾ける
  • 完全に一定値にならなくても問題ないと考える
  • 次回以降、移動平均などで値を少しなめらかにする
  • 今回は、値が少し揺れることもセンサの特徴として確認する

10-7. 角度が期待より大きすぎる／小さすぎる

原因

  • ボードの傾け方と軸の対応が合っていない
  • 加速度値が正しく読めていない
  • 上位バイト/下位バイトの結合を間違えている
  • int16_t への変換が正しくできていない

対策

  • E3-04に戻り、加速度X/Y/Zの生値が自然に変化するか確認する
  • make_int16(raw[0], raw[1]) の順番を確認する
  • ACCEL_DATA_X1 から12バイト連続で読んでいるか確認する
  • ボードを90度近く傾けたとき、主にどの軸が大きくなるか確認する

11. 今回わかったこと

今回の実験で大事なのは、
加速度センサの値から、ボードの傾きを数値として出せる ということです。

E3-05では、加速度には重力が入るため、補正には注意が必要だと説明しました。
今回は、その重力を使って、ピッチ角とロール角を計算しました。

今回できるようになったことは、次の通りです。

  • 加速度X/Y/Zを使って傾きを計算する
  • atan2f() と sqrtf() を使ってピッチ/ロールを求める
  • 計算した角度をUARTログに出す
  • ボードを傾けると、ピッチ/ロールの値が変わることを確認する
  • 姿勢を「なんとなく」ではなく、数値として見られるようにする

今回の計算は、あくまで最小実装です。

手で動かしているときや、ロボットが振動しているときは、加速度には重力以外の成分も入ります。
そのため、今回の値をそのまま本格的な制御に使うには注意が必要です。

ただし、2足歩行ロボットの安全停止や姿勢補正に進む前に、
姿勢を数値化する入口 としては十分な一歩です。

12. 次回やること

次回は、E3-07 異常姿勢判定→安全停止 に進みます。

今回、ピッチ角とロール角をログに出せるようになりました。
次回は、その角度にしきい値を付けて、

傾きが大きすぎる → 異常姿勢として停止する

という形に進めます。

これにより、IMUの値を読むだけでなく、
ロボットを壊さないための安全停止の入口につなげていきます。

13. 関連リンク

  • E3-00：I2C配線の“詰まりどころ”を先に潰す
  • E3-01：I2C接続確認：0x68 に対して応答があるか確認
  • E3-02：1レジスタ読み：WHO_AM_I を読む
  • E3-03：連続読み：加速度X軸を2バイト読む
  • E3-04：IMU生値取得：加速度とジャイロの生値を周期的に読んでログに出す
  • E3-05：オフセット補正（簡易）：静止時のズレを平均して引いてみる
  • E3-07：異常姿勢判定→安全停止
  • 基礎シリーズ：GNDとは？なぜ基準点が必要なのか？
  • 基礎シリーズ：UARTログ入門
  • 基礎シリーズ：タイマで周期処理
  • 基礎シリーズ：C言語の型と数値（予定）
  • 基礎シリーズ：センサと姿勢推定の入口（予定）

-フェーズ3