E3-04 IMU生値取得：加速度とジャイロの生値を周期的に読んでログに出す

シリーズ：実験シリーズ（フェーズ3）
対応ロードマップ：フェーズ3 / E3-04
この記事で扱う範囲：E3-03 で加速度X軸の2バイト連続読みができたので、今回は 加速度3軸とジャイロ3軸の生データをまとめて読み、周期的にUARTログへ出す ところまで進める。

1. 目的

今回は、IMUから 加速度3軸とジャイロ3軸の生値 を読み、一定周期でUARTログに出します。
E3-03では、加速度X軸だけを例にして、

2バイトを連続で読む
上位バイトと下位バイトを1つの16bit値にする
int16_t として正負のある値にする

ところまで確認しました。

今回はその考え方を広げて、

加速度X/Y/Z
ジャイロX/Y/Z

をまとめて読みます。

今回の目的は、値をgやdpsなどの物理単位に変換することではありません。
まずは、IMUの値が周期的に読めて、ボードを動かすと数値が変わること を確認します。

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バイト連続で読める

2-2. 使用するもの

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

2-3. 今回の前提条件

接続条件は、E3-03 と同じ下記内容になります。

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

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

今回は、加速度3軸とジャイロ3軸の生データを読むために、次のレジスタを使います。

データ	上位バイト	下位バイト
加速度X	`ACCEL_DATA_X1 = 0x1F`	`ACCEL_DATA_X0 = 0x20`
加速度Y	`ACCEL_DATA_Y1 = 0x21`	`ACCEL_DATA_Y0 = 0x22`
加速度Z	`ACCEL_DATA_Z1 = 0x23`	`ACCEL_DATA_Z0 = 0x24`
ジャイロX	`GYRO_DATA_X1 = 0x25`	`GYRO_DATA_X0 = 0x26`
ジャイロY	`GYRO_DATA_Y1 = 0x27`	`GYRO_DATA_Y0 = 0x28`
ジャイロZ	`GYRO_DATA_Z1 = 0x29`	`GYRO_DATA_Z0 = 0x2A`

今回読む範囲は、0x1F から 0x2A までの 12バイト です。
E3-03では、0x1F から2バイトだけ読みました。
今回は同じ考え方で、0x1F から 12バイト連続で読む 形にします。

2-5. 今回の動作イメージ

今回は、起動後に次の流れで動作します。

UART を Open する
I2C Master を Open する
加速度とジャイロを使えるように最低限の設定を書く
0x1F から 12バイト連続で読む
12バイトを、6個の int16_t に変換する
加速度X/Y/Z、ジャイロX/Y/Zを UART に出す
500msごとに繰り返す

今回確認したいのは、
「IMUの複数データをまとめて読み、周期的にログで見られること」 です。

3. 今回の変更点

3-1. 配線変更

今回は配線変更はありません。
E3-03 と同じ下記内容になります。

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

今回は配線変更はありません。

3-2. 設定変更

FSP の I2C Master 設定は、E3-03 と同じ下記のままで進めます。

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

3-3. コード変更

今回は次の処理を追加・変更します。

加速度だけでなく、ジャイロも使えるように設定する
0x1F から 12バイト連続で読む
読み出した12バイトを、次の6個の値にまとめる
- accel_x
- accel_y
- accel_z
- gyro_x
- gyro_y
- gyro_z
その値を 500msごとにUARTへ出力する

前回の E3-03 では、加速度X軸だけを1回読みました。
今回は、IMUの値を周期的に読み続ける ところが大きな違いです。

4. 手順

4-1. E3-03 と同じ条件で接続する

まず、E3-03 と同じ下記条件で、QCIOT-ICM42688Pと評価ボードを接続します。

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

4-2. E3-03 のプロジェクトをベースにする

今回は、E3-03 のプロジェクトをベースにします。
つまり、次の状態から始めます。

UARTログが出せる
I2C Master が Open できる
スレーブアドレス 0x68 に設定できる
加速度X軸の2バイト読みができる

4-3. Tera Term でUARTログを見られる状態にする

E3-03 と同じように、USB-UART変換ケーブルを接続し、Tera Termでログを見られる状態にします。
今回のログは複数行で出るため、Tera Termの画面を開いた状態で実行します。

4-4. 加速度とジャイロを有効にする

E3-03では、加速度だけを有効にしました。
今回は、加速度に加えてジャイロも有効にします。
使う設定レジスタは次の3つです。

PWR_MGMT0：加速度とジャイロをONにする
ACCEL_CONFIG0：加速度の範囲と更新周期を決める
GYRO_CONFIG0：ジャイロの範囲と更新周期を決める

今回は、設定値の細かい意味を全部追いません。
まずは、

加速度を読める状態にする
ジャイロを読める状態にする
読みやすい周期でデータを更新する

くらいの理解で十分です。

4-5. `0x1F` から12バイト読む

E3-03では、次のように読みました。

先頭レジスタ：0x1F
読むバイト数：2バイト
得られる値：加速度X軸だけ

今回は、次のように読みます。

先頭レジスタ：0x1F
読むバイト数：12バイト
得られる値：加速度X/Y/Z、ジャイロX/Y/Z

このように、連続したレジスタに並んでいるデータは、まとめて読むことができます。

4-6. 500msごとにログ出力する

今回は、値が動くことを確認したいので、読み取りを1回で終わらせず、500msごとに繰り返します。
最初から速い周期にすると、ログが流れすぎて読みにくくなります。
そのため、入門では 500msくらい が確認しやすいです。

5. コード

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

#include "hal_data.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.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)

/* 設定値
 * PWR_MGMT0 = 0x0F
 *   GYRO_MODE  = LN
 *   ACCEL_MODE = LN
 *
 * GYRO_CONFIG0 = 0x08
 *   GYRO_FS_SEL = 代表設定
 *   GYRO_ODR    = 100Hz
 *
 * ACCEL_CONFIG0 = 0x68
 *   ACCEL_FS_SEL = ±2g
 *   ACCEL_ODR    = 100Hz
 */
#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)

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;

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 int16_t make_int16(uint8_t upper, uint8_t lower);
static void print_imu_raw_data(const imu_raw_data_t * p_data);

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)
        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_data))
            {
                print_imu_raw_data(&imu_data);
            }
            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 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_raw_data(const imu_raw_data_t * p_data)
{
    char msg[160];

    if (NULL == p_data)
    {
        return;
    }

    snprintf(msg, sizeof(msg),
             "ACC X=%6d Y=%6d Z=%6d | GYRO X=%6d Y=%6d Z=%6d\r\n",
             p_data->accel_x,
             p_data->accel_y,
             p_data->accel_z,
             p_data->gyro_x,
             p_data->gyro_y,
             p_data->gyro_z);

    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");
    }
}

#include "hal_data.h"
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.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)

/* 設定値
 * PWR_MGMT0 = 0x0F
 *   GYRO_MODE  = LN
 *   ACCEL_MODE = LN
 *
 * GYRO_CONFIG0 = 0x08
 *   GYRO_FS_SEL = 代表設定
 *   GYRO_ODR    = 100Hz
 *
 * ACCEL_CONFIG0 = 0x68
 *   ACCEL_FS_SEL = ±2g
 *   ACCEL_ODR    = 100Hz
 */
#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)

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;

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 int16_t make_int16(uint8_t upper, uint8_t lower);
static void print_imu_raw_data(const imu_raw_data_t * p_data);

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)
        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_data))
            {
                print_imu_raw_data(&imu_data);
            }
            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 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_raw_data(const imu_raw_data_t * p_data)
{
    char msg[160];

    if (NULL == p_data)
    {
        return;
    }

    snprintf(msg, sizeof(msg),
             "ACC X=%6d Y=%6d Z=%6d | GYRO X=%6d Y=%6d Z=%6d\r\n",
             p_data->accel_x,
             p_data->accel_y,
             p_data->accel_z,
             p_data->gyro_x,
             p_data->gyro_y,
             p_data->gyro_z);

    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");
    }
}

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

6-1. 12バイトをまとめて読む

今回の中心は、次の部分です。

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

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

IMU_REG_ACCEL_DATA_X1 は 0x1F です。
IMU_RAW_DATA_LENGTH は 12 です。

つまり、

0x1F を先頭にする
そこから12バイト連続で読む

という意味です。
E3-03の「2バイト読む」と同じ考え方で、読む数を12バイトに増やしています。

6-2. 12バイトを6個の値に分ける

読み出した12バイトは、次のように並んでいます。

配列	意味
`raw[0]`, `raw[1]`	加速度X
`raw[2]`, `raw[3]`	加速度Y
`raw[4]`, `raw[5]`	加速度Z
`raw[6]`, `raw[7]`	ジャイロX
`raw[8]`, `raw[9]`	ジャイロY
`raw[10]`, `raw[11]`	ジャイロZ

そのため、コードでは次のように6個の値に変換しています。

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]);

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]);

最初は、

2バイトで1つの値
12バイトなので6個の値

と分かれば十分です。

6-3. `make_int16()` は2バイトを1つにまとめる関数

E3-03では、次のような処理を直接書きました。

accel_x = (int16_t)(((uint16_t)accel_raw[0] << 8) | accel_raw[1]);

accel_x = (int16_t)(((uint16_t)accel_raw[0] << 8) | accel_raw[1]);

今回は同じ処理を何回も使うため、関数にしています。

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

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

この関数は、

上位バイトを左に8bitずらす
下位バイトとつなげる
int16_t として返す

という処理をしています。

6-4. 500msごとに読む理由

今回は、while (1) の中で毎回読むのではなく、500msごとに読んでいます。
理由は、ログを見やすくするためです。
IMUはもっと速い周期でも読めますが、最初から速くすると、Tera Termの画面にログが大量に流れてしまいます。
今回の目的は「値が動くことを確認する」なので、まずは500ms周期で十分です。

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

    if (imu_read_raw_data(&imu_data))
    {
        print_imu_raw_data(&imu_data);
    }
    else
    {
        uart_print("IMU raw read error\r\n");
    }
}

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

    if (imu_read_raw_data(&imu_data))
    {
        print_imu_raw_data(&imu_data);
    }
    else
    {
        uart_print("IMU raw read error\r\n");
    }
}

6-5. 今回は物理単位に変換しない

今回出している値は、生値です。
そのため、ログに出る値は、まだ

何g
何dps
何度傾いている

という値ではありません。
今回は、

ボードを傾けると加速度の値が変わる
ボードを動かすとジャイロの値が変わる
静止しているとジャイロは0付近に近づく

という確認までで十分です。

7. 実行結果

7-1. 確認したいログ

実行すると、Tera Termに次のようなログが出ればOKです。

IMU raw read start
IMU init OK
ACC X=  -120 Y=   340 Z= 16320 | GYRO X=    12 Y=   -20 Z=     5
ACC X=  -118 Y=   345 Z= 16310 | GYRO X=     8 Y=   -18 Z=     4
ACC X=  2400 Y=  -530 Z= 15820 | GYRO X=   150 Y=  -320 Z=    40

IMU raw read start
IMU init OK
ACC X=  -120 Y=   340 Z= 16320 | GYRO X=    12 Y=   -20 Z=     5
ACC X=  -118 Y=   345 Z= 16310 | GYRO X=     8 Y=   -18 Z=     4
ACC X=  2400 Y=  -530 Z= 15820 | GYRO X=   150 Y=  -320 Z=    40

数値は、置き方や動かし方で変わります。
上の数値と一致する必要はありません。

7-2. 加速度で確認すること

ボードを静かに置いた状態では、加速度の3軸のうち、どれか1つの軸が大きめの値になることがあります。
これは、重力の影響を受けているためです。
今回の段階では、正確な単位変換はしません。
まずは、

ボードを傾けると ACC X/Y/Z の値が変わる
向きを変えると、大きく出る軸が変わる

ことを確認します。

7-3. ジャイロで確認すること

ジャイロは、回転の動きに反応します。
そのため、ボードを静かに置いた状態では、GYRO X/Y/Z は0付近に近い値になるはずです。
一方で、ボードを手で回すように動かすと、値が一時的に大きく変わります。
今回確認したいのは、

静止中はジャイロ値が比較的小さい
動かした瞬間にジャイロ値が変わる
動かし方によって反応する軸が変わる

という点です。

7-4. 今回確認できればよいこと

今回の段階では、次が確認できれば十分です。

0x1F から 12バイト連続で読める
12バイトを6個の int16_t に変換できる
加速度X/Y/Z、ジャイロX/Y/Zをログに出せる
ボードを傾けたり動かしたりすると、値が変わる

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

8-1. `IMU raw read error` になる

原因

E3-03 の2バイト読みが安定していない
12バイト読みへ変更したときに、受信バッファのサイズが足りていない
先頭レジスタ番号を書いたあと、読み取りに進む流れが崩れている
I2Cの送受信完了を待たずに次の処理へ進んでいる
タイムアウト時間が短すぎる

対策

まず E3-03 の加速度X軸2バイト読みが安定して成功する状態に戻す
raw 配列が12バイト以上あるか確認する
imu_read_register_bytes(IMU_REG_ACCEL_DATA_X1, raw, 12U) の形になっているか確認する
コールバックで TX_COMPLETE / RX_COMPLETE を受けているか確認する
最初は500ms周期など、ゆっくりした周期で読む

8-2. `IMU init error` になる

原因

PWR_MGMT0 への書き込みが失敗している
GYRO_CONFIG0 または ACCEL_CONFIG0 への書き込みが失敗している
I2Cアドレスが0x68になっていない
E3-01 / E3-02 の基本通信が崩れている
生成コードの名前と、コード中の名前が合っていない

対策

まず E3-01 に戻り、ACK at 0x68 が出るか確認する
次に E3-02 に戻り、WHO_AM_I が読めるか確認する
設定レジスタへの書き込みを1つずつ確認する
g_i2c_master0_ctrl や g_i2c_master0_cfg の名前が、自分のFSP生成コードと一致しているか確認する
FSP設定変更後に Generate Project Content を実行する

8-3. 加速度は変わるが、ジャイロが変わらない

原因

ジャイロを有効にする設定が入っていない
PWR_MGMT0 が加速度だけONの値のままになっている
GYRO_CONFIG0 の設定を書いていない
ボードを傾けているだけで、回転させていない

対策

PWR_MGMT0 が、加速度とジャイロの両方をONにする値になっているか確認する
GYRO_CONFIG0 への書き込み処理が入っているか確認する
ジャイロ確認時は、傾けるだけでなく、手で軽く回すように動かしてみる
ログの GYRO X/Y/Z が一瞬でも変わるかを見る

8-4. 値は出るが、どの軸が何を意味するか分からない

原因

IMUの軸方向をまだ確認していない
ボード上の向きと、ログ上のX/Y/Zが頭の中で対応していない
加速度とジャイロの違いを同時に見ようとして混乱している

対策

まず加速度だけを見る
ボードを1方向ずつ傾けて、どの値が大きく変わるか確認する
次にジャイロだけを見る
ボードを1方向ずつ回して、どの値が大きく変わるか確認する
今回は軸方向の完全理解より、値が動くことを優先する

8-5. ログが速すぎて読めない

原因

読み取り周期が短すぎる
while(1) の中で毎回ログを出している
UART送信が追いつかないほどログを出している

対策

まずは500ms周期にする
見にくい場合は1000ms周期にする
while(1) の中で無条件にログ出力しない
1回のログを1行にまとめる

8-6. 値が大きすぎる／正負がおかしい

原因

上位バイトと下位バイトの順番が逆になっている
uint16_t のまま表示している
12バイトの並びと、変換先の軸がずれている
raw[10] / raw[11] など、配列の添字を間違えている

対策

make_int16(上位バイト, 下位バイト) の順番になっているか確認する
最終的に int16_t で受ける
raw[0] から raw[11] までの対応表を見ながら確認する
まずは加速度X/Y/Zだけ出し、その後ジャイロを追加する

9. 今回わかったこと

今回の実験で大事なのは、
IMUのデータは、複数のレジスタに並んでいて、それをまとめて読むことで効率よく取得できる ということです。
E3-03では、加速度X軸だけを2バイト読みました。
今回は、同じ考え方で12バイト読み、6個の値に分けました。
今回できるようになったことは、次の通りです。

0x1F から12バイト連続で読む
2バイトずつ int16_t に変換する
加速度3軸、ジャイロ3軸をログに出す
周期的にIMU値を確認する
ボードを動かすと値が変わることを確認する

まだ、今回の値は「生値」です。
そのため、このままでは姿勢角にはなっていません。
ただし、姿勢推定や安全停止へ進むためには、まずこの 生値を安定して取れること が土台になります。

10. 次回やること

次回は、E3-06 オフセット補正（簡易） に進みます。
今回のログを見ると、静止していてもジャイロが完全に0にならなかったり、加速度にも少し偏りが見えることがあります。
次回は、静止状態の値を何回か読み、平均値を使って簡単な補正を行います。
次回のゴールは、
静止時のズレを少し減らし、補正という考え方に慣れること です。

11. 関連リンク

E3-00：I2C配線の“詰まりどころ”を先に潰す
E3-01：I2C接続確認：0x68 に対して応答があるか確認
E3-02：1レジスタ読み：WHO_AM_I を読む
E3-03：連続読み：加速度X軸を2バイト読む
E3-06：オフセット補正（簡易）
基礎シリーズ：GNDとは？なぜ基準点が必要なのか？
基礎シリーズ：UARTログ入門
基礎シリーズ：タイマで周期処理
基礎シリーズ：C言語の型と数値（予定）