フェーズ3

E3-07 異常姿勢判定→安全停止：傾きが大きすぎたらRUNからERRORへ移す

シリーズ：実験シリーズ（フェーズ3）
対応ロードマップ：フェーズ3 / E3-07
この記事で扱う範囲：E3-06 で計算したピッチ角・ロール角を使い、しきい値を超えたら 異常姿勢 と判定します。今回は、異常姿勢を検出したら RUN から ERROR へ状態を移し、安全停止処理につなげるところまで確認します。

1. 目的

今回は、IMUから求めたピッチ角・ロール角を使って、 倒れそうな姿勢を検出する入口 を作ります。
E3-06では、加速度X/Y/Zからピッチ角とロール角を計算し、UARTログに表示しました。
今回は、その次のステップとして、

ピッチ角またはロール角が大きすぎる  
↓  
異常姿勢と判定する  
↓  
RUN状態をやめる  
↓  
ERROR状態へ移る  
↓  
安全停止処理を行う

という流れを作ります。
今回のゴールは、正確な転倒判定ではありません。
まずは、

  • 姿勢角にしきい値を付ける
  • しきい値を超えたら異常と判定する
  • 異常時に状態を RUN から ERROR へ移す
  • ERROR 状態では動作を止める

という、安全停止の基本的な流れを体験することです。
将来的にサーボを動かすようになると、
「倒れそうなのに動き続ける」ことは危険です。
そのため、今回の実験は、2足歩行ロボットの安全設計に向けた重要な入口になります。

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：静止時のズレを平均して引く考え方を確認している
  • E3-06：加速度からピッチ/ロールを計算できる

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 / E3-06 と同じ下記条件です。

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

2-4. 今回使う値

今回は、E3-06で計算した次の2つを使います。

pitch_deg
roll_deg

この2つに対して、しきい値を設定します。
たとえば今回は、最小実装として次のようにします。

#define POSTURE_PITCH_LIMIT_DEG    (35.0f)
#define POSTURE_ROLL_LIMIT_DEG     (35.0f)

つまり、

  • ピッチ角の絶対値が35度を超えたら異常
  • ロール角の絶対値が35度を超えたら異常

と判定します。
ここでの35度は、あくまで実験用の値です。

実際のロボットでは、

  • 重心
  • 足の形
  • サーボの動作速度
  • 歩行中の揺れ
  • IMUの取り付け位置
  • どの程度傾いたら危険か

によって調整が必要になります。

3. 今回の変更点

3-1. 配線変更

今回は配線変更はありません。
E3-06と同じ下記の接続です。

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

3-2. 設定変更

FSP の I2C Master 設定は、E3-06と同じ下記の設定です。

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

また、E2-02 / E2-03 と同じように、1ms周期のタイマが動いている前提です。

3-3. コード変更

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

  • アプリの状態を表す app_state_t を追加する
  • エラー理由を表す app_error_t を追加する
  • ピッチ/ロールがしきい値を超えたか判定する
  • 異常姿勢を検出したら RUN から ERROR へ移す
  • ERROR 状態では安全停止処理を行う

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

姿勢角を読むだけで終わらせず、状態遷移につなげる

E3-06までは、ログに角度を出すだけでした。
今回は、その角度を使って、

この姿勢は危ない

とプログラム側で判断します。
これにより、IMUの値が「見るための値」から、 ロボットを守るための「判断材料」に変わります。

4. 異常姿勢とは何か

4-1. 今回の異常姿勢の考え方

今回は、次のどちらかに当てはまったら異常姿勢とします。

|pitch| > 35度
|roll|  > 35度

|pitch| は、ピッチ角の絶対値です。

たとえば、

  • pitch = 40度 → 異常
  • pitch = -40度 → 異常
  • pitch = 10度 → 正常

という意味です。

プラス方向に大きく傾いても、マイナス方向に大きく傾いても、
どちらも危険な傾きとして扱います。

4-2. なぜ絶対値で見るのか

倒れそうかどうかを見る場合、最初は向きよりも 傾きの大きさ が大事です。
たとえば、前に35度以上傾いても、後ろに35度以上傾いても、
どちらも危険な状態と考えられます。
そのため今回は、次のように絶対値で判定します。

if (fabsf(p_angle->pitch_deg) > POSTURE_PITCH_LIMIT_DEG)
{
    異常姿勢
}

fabsf() は、float の絶対値を求める関数です。

4-3. 1回だけ超えたらすぐ止めるべきか

本当の安全停止では、異常を見つけたらすぐ止める方が安全です。
ただし、今回のように加速度から求めた簡易ピッチ/ロールは、
手で動かした瞬間や振動で一時的に大きく変わることがあります。

そこで今回は、初心者向けの実験として、
連続して数回しきい値を超えたら異常 とします。
たとえば、

#define POSTURE_ERROR_CONFIRM_COUNT    (3U)

として、3回連続で異常姿勢なら ERROR に入るようにします。
これにより、1回だけのブレで止まりすぎる問題を少し減らせます。
ただし、実際のロボットでは安全優先なので、
この回数やしきい値は、機体に合わせて慎重に決める必要があります。

5. 状態遷移の考え方

5-1. 今回使う状態

今回は、状態を次のように分けます。

状態意味
APP_STATE_INIT初期化中
APP_STATE_RUNIMUを読み、姿勢を監視している状態
APP_STATE_ERROR異常を検出し、安全停止している状態

今回の中心は、次の遷移です。

RUN → ERROR

5-2. RUN状態で行うこと

RUN 状態では、周期的に次の処理を行います。

IMUを読む
ピッチ/ロールを計算する
異常姿勢か判定する
ログを出力する

正常なら、そのまま RUN を続けます。
異常姿勢が続いたら、ERROR へ移ります。

5-3. ERROR状態で行うこと

ERROR 状態では、安全停止処理を行います。
今回の段階では、まだサーボを動かしていないため、実際にサーボを止める処理はありません。
そのため、今回は次のような「安全停止の入口」として作ります。

安全停止処理を呼ぶ
エラーログを出力する
以後はRUNに戻らない

将来、サーボ制御に進んだら、ここに次のような処理を追加します。

  • PWM出力を安全な値にする
  • サーボの動作指令を止める
  • モーション更新を停止する
  • 必要ならトルクOFF相当の処理を行う
  • BLEやUARTでエラー状態を通知する

今回は、そのための「置き場所」を作るイメージです。

6. 手順

6-1. E3-06と同じ条件で接続する

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

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

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

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

  • imu_read_raw_data()
    加速度3軸とジャイロ3軸を読む
  • imu_calc_pitch_roll()
    加速度X/Y/Zからピッチ/ロールを計算する
  • posture_is_abnormal()
    ピッチ/ロールがしきい値を超えているか判定する
  • safe_stop()
    異常時の停止処理を行う

6-3. Tera Termを開く

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

6-4. ボードを水平に置いて確認する

まず、評価ボードを机の上に静かに置きます。
このとき、STATE=RUN のままログが出続ければOKです。

STATE=RUN ACC X=  -120 Y=   340 Z= 16320 | PITCH=   0.42 deg ROLL=   1.19 deg

完全に0度になる必要はありません。

6-5. ボードをゆっくり傾ける

次に、ボードをゆっくり前後または左右に傾けます。
ピッチまたはロールがしきい値を超えると、異常姿勢としてカウントされます。

STATE=RUN ACC X=  9800 Y=   250 Z= 12800 | PITCH= -37.40 deg ROLL=   1.12 deg
WARN: abnormal posture count=1

6-6. しきい値超えを続ける

傾けた状態を少し続けると、異常姿勢が連続して検出されます。

WARN: abnormal posture count=2
WARN: abnormal posture count=3
ERROR: abnormal posture detected
SAFE STOP

この後、状態は ERROR になり、RUN の処理は行いません。

7. コード

※下記は考え方を分かりやすくするためのシンプルな例です。
※ I2Cインスタンス名、UARTインスタンス名、タイマ名、コールバック名は、自分のプロジェクトに合わせて置き換えてください。
※ atan2f() / sqrtf() / fabsf() を使うため、math.h をインクルードします。
※ 今回の safe_stop() は、まだサーボ停止の本実装ではありません。将来、PWMサーボ制御に進んだときに中身を追加します。

#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)

/* 異常姿勢判定用 */
#define POSTURE_PITCH_LIMIT_DEG      (35.0f)
#define POSTURE_ROLL_LIMIT_DEG       (35.0f)
#define POSTURE_ERROR_CONFIRM_COUNT  (3U)

typedef enum e_app_state
{
    APP_STATE_INIT = 0,
    APP_STATE_RUN,
    APP_STATE_ERROR
} app_state_t;

typedef enum e_app_error
{
    APP_ERROR_NONE = 0,
    APP_ERROR_IMU_INIT,
    APP_ERROR_IMU_READ,
    APP_ERROR_ABNORMAL_POSTURE
} app_error_t;

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 app_state_t g_app_state = APP_STATE_INIT;
static app_error_t g_app_error = APP_ERROR_NONE;
static uint32_t g_posture_error_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 bool posture_is_abnormal(const imu_angle_t * p_angle);
static void app_run_task(void);
static void app_error_task(void);
static void safe_stop(void);

static int16_t make_int16(uint8_t upper, uint8_t lower);
static void print_imu_state_data(const imu_raw_data_t * p_raw, const imu_angle_t * p_angle);
static const char * app_state_to_string(app_state_t state);
static const char * app_error_to_string(app_error_t error);

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;

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

    uart_print("IMU abnormal posture safety stop 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");
        g_app_state = APP_STATE_ERROR;
        g_app_error = APP_ERROR_IMU_INIT;
    }

    if (APP_STATE_ERROR != g_app_state)
    {
        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");
            g_app_state = APP_STATE_ERROR;
            g_app_error = APP_ERROR_IMU_INIT;
        }
    }

    if (APP_STATE_ERROR != g_app_state)
    {
        if (!imu_init())
        {
            uart_print("IMU init error\r\n");
            g_app_state = APP_STATE_ERROR;
            g_app_error = APP_ERROR_IMU_INIT;
        }
        else
        {
            uart_print("IMU init OK\r\n");
            g_app_state = APP_STATE_RUN;
            g_app_error = APP_ERROR_NONE;
        }
    }

    while (1)
    {
        switch (g_app_state)
        {
            case APP_STATE_RUN:
            {
                app_run_task();
                break;
            }

            case APP_STATE_ERROR:
            {
                app_error_task();
                break;
            }

            case APP_STATE_INIT:
            default:
            {
                break;
            }
        }
    }
}

static void app_run_task(void)
{
    static uint32_t last_log_ms = 0U;
    imu_raw_data_t imu_raw;
    imu_angle_t imu_angle;
    char msg[100];

    if ((g_ms_count - last_log_ms) < IMU_LOG_INTERVAL_MS)
    {
        return;
    }

    last_log_ms += IMU_LOG_INTERVAL_MS;

    if (!imu_read_raw_data(&imu_raw))
    {
        uart_print("IMU raw read error\r\n");
        g_app_state = APP_STATE_ERROR;
        g_app_error = APP_ERROR_IMU_READ;
        return;
    }

    imu_calc_pitch_roll(&imu_raw, &imu_angle);
    print_imu_state_data(&imu_raw, &imu_angle);

    if (posture_is_abnormal(&imu_angle))
    {
        g_posture_error_count++;

        snprintf(msg, sizeof(msg),
                 "WARN: abnormal posture count=%lu\r\n",
                 (unsigned long)g_posture_error_count);
        uart_print(msg);

        if (g_posture_error_count >= POSTURE_ERROR_CONFIRM_COUNT)
        {
            uart_print("ERROR: abnormal posture detected\r\n");
            g_app_state = APP_STATE_ERROR;
            g_app_error = APP_ERROR_ABNORMAL_POSTURE;
        }
    }
    else
    {
        g_posture_error_count = 0U;
    }
}

static void app_error_task(void)
{
    static bool first = true;
    char msg[100];

    if (first)
    {
        first = false;

        safe_stop();

        snprintf(msg, sizeof(msg),
                 "STATE=%s ERROR=%s\r\n",
                 app_state_to_string(g_app_state),
                 app_error_to_string(g_app_error));
        uart_print(msg);
    }

    /*
     * 今回は復帰処理を入れない。
     * ERRORに入ったら、安全側として停止したままにする。
     *
     * 将来は、ボタン操作や通信コマンドで
     * エラー解除・再初期化・安全確認後の復帰などを追加する。
     */
}

static void safe_stop(void)
{
    /*
     * 今回はサーボ制御前なので、実際に止める対象はまだない。
     * そのため、ここでは安全停止処理の入口としてログを出す。
     *
     * 将来、PWMサーボ制御に進んだら、ここに次のような処理を追加する。
     * - サーボ出力を停止する
     * - モーション更新を停止する
     * - 安全姿勢に移す
     * - 必要ならトルクOFF相当の処理を行う
     */
    uart_print("SAFE STOP\r\n");
}

static bool posture_is_abnormal(const imu_angle_t * p_angle)
{
    if (NULL == p_angle)
    {
        return true;
    }

    if (fabsf(p_angle->pitch_deg) > POSTURE_PITCH_LIMIT_DEG)
    {
        return true;
    }

    if (fabsf(p_angle->roll_deg) > POSTURE_ROLL_LIMIT_DEG)
    {
        return true;
    }

    return false;
}

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;

    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_state_data(const imu_raw_data_t * p_raw, const imu_angle_t * p_angle)
{
    char msg[220];

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

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

    uart_print(msg);
}

static const char * app_state_to_string(app_state_t state)
{
    switch (state)
    {
        case APP_STATE_INIT:
        {
            return "INIT";
        }

        case APP_STATE_RUN:
        {
            return "RUN";
        }

        case APP_STATE_ERROR:
        {
            return "ERROR";
        }

        default:
        {
            return "UNKNOWN";
        }
    }
}

static const char * app_error_to_string(app_error_t error)
{
    switch (error)
    {
        case APP_ERROR_NONE:
        {
            return "NONE";
        }

        case APP_ERROR_IMU_INIT:
        {
            return "IMU_INIT";
        }

        case APP_ERROR_IMU_READ:
        {
            return "IMU_READ";
        }

        case APP_ERROR_ABNORMAL_POSTURE:
        {
            return "ABNORMAL_POSTURE";
        }

        default:
        {
            return "UNKNOWN";
        }
    }
}

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. 状態を表す app_state_t

今回は、アプリの状態を次のように定義しています。

typedef enum e_app_state
{
    APP_STATE_INIT = 0,
    APP_STATE_RUN,
    APP_STATE_ERROR
} app_state_t;

今回の状態は、次の意味です。

  • APP_STATE_INIT：初期化中
  • APP_STATE_RUN：通常監視中
  • APP_STATE_ERROR：異常検出後、安全停止中

E2-05で扱った状態整理の考え方を、IMUの異常判定に使っています。

8-2. エラー理由を表す app_error_t

異常が起きたとき、ただ ERROR と出すだけだと原因が分かりにくくなります。
そこで、今回はエラー理由も分けています。

typedef enum e_app_error
{
    APP_ERROR_NONE = 0,
    APP_ERROR_IMU_INIT,
    APP_ERROR_IMU_READ,
    APP_ERROR_ABNORMAL_POSTURE
} app_error_t;

たとえば、

  • IMU初期化に失敗した
  • IMU読み取りに失敗した
  • 異常姿勢を検出した

を分けてログに出せるようにしています。
これにより、あとでトラブルが起きたときに、「何が原因で止まったのか」を追いやすくなります。

8-3. 異常姿勢を判定する関数

異常姿勢の判定は、次の関数にまとめています。

static bool posture_is_abnormal(const imu_angle_t * p_angle)
{
    if (NULL == p_angle)
    {
        return true;
    }

    if (fabsf(p_angle->pitch_deg) > POSTURE_PITCH_LIMIT_DEG)
    {
        return true;
    }

    if (fabsf(p_angle->roll_deg) > POSTURE_ROLL_LIMIT_DEG)
    {
        return true;
    }

    return false;
}

ここで重要なのは、判定処理を app_run_task() の中に直接書きすぎず、
posture_is_abnormal() という関数に分けている点です。
こうしておくと、あとで条件を変えたいときに修正しやすくなります。
たとえば将来、

  • ピッチだけ厳しくする
  • ロールだけ緩くする
  • 急に角度が変わったときも異常にする
  • 電圧低下も異常条件に加える

といった変更がしやすくなります。

8-4. fabsf() で絶対値を見る

今回の判定では、次のように fabsf() を使っています。

fabsf(p_angle->pitch_deg)

これは、float の絶対値を求める関数です。
たとえば、

pitch =  40.0 → fabsf(pitch) = 40.0
pitch = -40.0 → fabsf(pitch) = 40.0

となります。
前方向でも後ろ方向でも、大きく傾いたら危険としたいので、 今回は絶対値で判定しています。

8-5. 連続して異常ならERRORにする

今回のコードでは、1回しきい値を超えただけでは、すぐ ERROR にしていません。

g_posture_error_count++;

if (g_posture_error_count >= POSTURE_ERROR_CONFIRM_COUNT)
{
    uart_print("ERROR: abnormal posture detected\r\n");
    g_app_state = APP_STATE_ERROR;
    g_app_error = APP_ERROR_ABNORMAL_POSTURE;
}

POSTURE_ERROR_CONFIRM_COUNT を3にしているため、 3回連続で異常姿勢になったら ERROR へ移ります。
正常姿勢に戻った場合は、カウントを0に戻します。

g_posture_error_count = 0U;

これにより、一瞬だけ値が跳ねた場合に止まりすぎる問題を少し減らします。

8-6. safe_stop() は停止処理の置き場所

今回の safe_stop() は、まだログを出すだけです。

static void safe_stop(void)
{
    uart_print("SAFE STOP\r\n");
}

理由は、今回のフェーズではまだサーボを動かしていないからです。
ただし、ここに safe_stop() を作っておくことが大事です。
今後、PWMサーボ制御に進んだら、ここに停止処理を追加できます。
たとえば、

サーボ出力を止めるモーション更新を止める安全姿勢に移すエラー状態をBLEで通知する

といった処理を、ここに入れていきます。
今回の記事では、まず 安全停止処理の入口 を作ることが目的です。

9. 実行結果

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

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

IMU abnormal posture safety stop start
IMU init OK
STATE=RUN ACC X=  -120 Y=   340 Z= 16320 | PITCH=   0.42 deg ROLL=   1.19 deg
STATE=RUN ACC X=  -130 Y=   360 Z= 16310 | PITCH=   0.46 deg ROLL=   1.26 deg
STATE=RUN ACC X=  -110 Y=   330 Z= 16330 | PITCH=   0.39 deg ROLL=   1.16 deg

この状態では、ピッチ/ロールがしきい値を超えていないため、STATE=RUN のまま動作します。
見るポイントは、次の通りです。

  • STATE=RUN になっている
  • ピッチ/ロールが大きすぎない
  • WARN や ERROR が出ていない

9-2. 少し傾けたとき

次に、ボードを少し傾けます。

STATE=RUN ACC X=  5200 Y=   200 Z= 15400 | PITCH= -18.65 deg ROLL=   0.74 deg
STATE=RUN ACC X=  6200 Y=   180 Z= 14900 | PITCH= -22.58 deg ROLL=   0.69 deg

この程度では、まだ35度を超えていないため、正常扱いです。

9-3. 大きく傾けたとき

さらに大きく傾けると、しきい値を超えます。

STATE=RUN ACC X=  9800 Y=   250 Z= 12800 | PITCH= -37.40 deg ROLL=   1.12 deg
WARN: abnormal posture count=1
STATE=RUN ACC X= 10100 Y=   260 Z= 12500 | PITCH= -38.91 deg ROLL=   1.19 deg
WARN: abnormal posture count=2
STATE=RUN ACC X= 10400 Y=   280 Z= 12200 | PITCH= -40.30 deg ROLL=   1.31 deg
WARN: abnormal posture count=3
ERROR: abnormal posture detected
SAFE STOP
STATE=ERROR ERROR=ABNORMAL_POSTURE

ここで確認したいのは、次の流れです。

RUN中に角度を読む
        ↓
しきい値を超える
        ↓
WARNが出る
        ↓
連続して異常ならERRORになる
        ↓
SAFE STOPが出る

これで、ピッチ/ロールの数値を安全停止へつなげる入口ができました。

9-4. ERRORに入ったら戻らない

今回のコードでは、ERROR に入ったあと、自動で RUN には戻りません。
これは、安全側の考え方です。
実際のロボットでは、倒れた可能性がある状態で勝手に動き始めると危険です。
そのため、今回の段階では、

異常を検出したら止まったまま

にしています。
将来は、

  • ボタンでリセットする
  • BLEからエラー解除コマンドを送る
  • 姿勢が安全範囲に戻ったことを確認してから復帰する
  • サーボ電源やバッテリー状態も確認する

といった復帰処理を追加します。

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

10-1. すぐに ERROR になる

原因

  • しきい値が小さすぎる
  • ボードを水平に置いていない
  • IMUモジュールの取り付け向きが想定と違う
  • ピッチ/ロールの符号や軸の見え方が想定と違う
  • 起動直後の値が安定する前に判定している

対策

  • まず水平に置いたときの PITCH / ROLL を確認する
  • 水平時に何度くらい出ているかメモする
  • 最初はしきい値を大きめにする
    例：35.0f ではなく 45.0f にして試す
  • 起動直後すぐに判定せず、数回ログを見てから確認する
  • E3-06に戻り、ピッチ/ロールが自然に変化しているか確認する

10-2. 大きく傾けても ERROR にならない

原因

  • しきい値が大きすぎる
  • 傾けている方向と、判定している軸が合っていない
  • posture_is_abnormal() が呼ばれていない
  • g_posture_error_count が増えていない
  • POSTURE_ERROR_CONFIRM_COUNT が大きすぎる

対策

  • ログで PITCH / ROLL の値を確認する
  • 実際に何度まで変化しているか見る
  • まずはしきい値を小さめにして試す
    例：35.0f ではなく 20.0f にして動作確認する
  • WARN: abnormal posture count= が出るか確認する
  • POSTURE_ERROR_CONFIRM_COUNT を一時的に 1U にして、判定が動くか確認する

10-3. WARN は出るが ERROR にならない

原因

  • 異常姿勢が連続していない
  • 傾けたり戻したりしているため、カウントが0に戻っている
  • POSTURE_ERROR_CONFIRM_COUNT に到達していない
  • ログ周期が長く、確認しにくい

対策

  • しきい値を超えた姿勢を少し維持する
  • WARN のカウントが増えているか確認する
  • 一時的に POSTURE_ERROR_CONFIRM_COUNT を 1U にする
  • IMU_LOG_INTERVAL_MS を短くして、ログを見やすくする
    例：500U から 200U にする

10-4. IMU raw read error になる

原因

  • I2C読み取りが失敗している
  • E3-04 / E3-06 の生値取得が安定していない
  • レジスタアドレスを書いたあとに読み取りへ進めていない
  • 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-5. ビルドエラーになる

原因

  • math.h をインクルードしていない
  • fabsf() の関数名を間違えている
  • app_state_t / app_error_t の定義を追加していない
  • posture_is_abnormal() の宣言と定義が合っていない
  • safe_stop() の宣言と定義が合っていない
  • 数学ライブラリがリンクされていない

対策

  • 先頭に #include <math.h> があるか確認する
  • fabsf / atan2f / sqrtf のつづりを確認する
  • 関数のプロトタイプ宣言があるか確認する
  • app_state_t と app_error_t の定義があるか確認する
  • atan2f / sqrtf / fabsf のリンクエラーの場合は、数学ライブラリのリンク設定を確認する

10-6. SAFE STOP が出たあと何も動かない

原因

  • 今回のコードでは、ERROR から自動復帰しない作りにしている
  • 安全側として、異常検出後は止まったままにしている
  • 復帰処理をまだ実装していない

対策

  • 今回は正常な動作と考える
  • 再度試す場合は、いったんリセットして起動し直す
  • 将来、復帰処理を追加する場合は、ボタンや通信コマンドで解除する形を検討する
  • ただし、実際のロボットでは、安全確認なしに自動復帰させない方がよい

11. 今回わかったこと

今回の実験で大事なのは、
姿勢角をログ表示で終わらせず、安全停止の判断に使える ということです。
E3-06では、加速度からピッチ/ロールを計算しました。
今回は、その角度にしきい値を付けて、

傾きが大きすぎる → 異常姿勢 → ERROR → SAFE STOP

という流れを作りました。
今回できるようになったことは、次の通りです。

  • ピッチ/ロールにしきい値を付ける
  • fabsf() で傾きの大きさを見る
  • 異常姿勢を検出する
  • 異常が連続したら RUN から ERROR へ移す
  • ERROR 状態で安全停止処理を呼ぶ
  • エラー理由をログに出す

今回の安全停止は、まだ本物のサーボ停止ではありません。
ただし、将来サーボ制御に進んだとき、
safe_stop() の中にサーボ停止処理を追加すれば、今回の考え方をそのまま使えます。

つまり今回は、ロボットを壊さないための状態遷移の土台 を作ったことになります。

12. 次回やること

次回は、ロードマップ上では センサ値を“軽く”なめらかにする処理 に進みます。
今回の異常姿勢判定では、ピッチ/ロールが一瞬だけ大きく変わると、WARN が出ることがあります。
加速度から求めた角度は、手で動かした瞬間や振動で揺れます。
そのため次回は、移動平均などの簡単な方法で、

値のバラつきを少し減らす

ことを確認します。
これにより、異常姿勢判定や今後の姿勢補正で、値の揺れに振り回されにくくなります。

13. 関連リンク

  • E3-00：I2C配線の“詰まりどころ”を先に潰す
  • E3-01：I2C接続確認：0x68 に対して応答があるか確認
  • E3-02：1レジスタ読み：WHO_AM_I を読む
  • E3-03：連続読み：加速度X軸を2バイト読む
  • E3-04：IMU生値取得：加速度とジャイロの生値を周期的に読んでログに出す
  • E3-05：オフセット補正（簡易）：静止時のズレを平均して引いてみる
  • E3-06：ピッチ/ロール推定（最小）：加速度から傾きをそれっぽく出してみる
  • E3-08：センサ値を“軽く”なめらかにする（移動平均など）
  • 基礎シリーズ：状態遷移の超入門（INIT / IDLE / RUN / ERROR）
  • 基礎シリーズ：UARTログ入門
  • 基礎シリーズ：タイマで周期処理
  • 基礎シリーズ：C言語のif / switch
  • 基礎シリーズ：センサと姿勢推定の入口（予定）

-フェーズ3