RON-V #21 — AtomS3RでCAN / OBD-II・IMU・Shutdown制御を統合する

RON-V 連載リンク
← 前の記事：#20 — UPS監視と安全Shutdown
→ 次の記事：#22 — 道路・災害画像AIの構築

L0〜L4の定義追記（2026-10-25）

本文中のコード・試験結果は執筆当時の記録として残しています。画像AI・音声を関連付ける現在の定義は次のとおりです。

レベル	残す内容
L0	Meshtastic向けの23バイト短報。画像AIの候補通知は任意設定
L1	個別の観測・撮影／録音／認識の状態と、詳細への参照
L2	一定時間の観測件数・分類別候補などの集計
L3	画像AIの詳細、録音情報、文字起こし全文
L4	元画像JPEG、元音声WAV、LTEのraw logなど

L0の警報欄は1バイト＝8ビットです。新しいversion=2の定義は、倒木 0x01、冠水 0x02、煙 0x04、道路上の障害物 0x08、路面の穴 0x10。残り3ビットは未割り当てです。0x03は倒木と冠水の組み合わせで、煙の番号ではありません。0x00はL0上では未判定で、安全を示しません。 候補ありでは元イベントの時刻・位置を使い、現在地と混同しません。

発信元はnode_uid、根拠は画像・音声などです。「処理成功」「災害の候補」「人の確認済み」を分けます。初期の確認状態はunreviewed。画像AIの検出0件を「危険なし」とせず、音声の否定も全文へ残します。人が補足・訂正する画面と、音声からの災害自動分類は今後の実装です。

10月25日にL1の関連付け・L2の集計・音声L3/L4の転送処理をコードへ追加しました。新連携の実機確認はこれからです。音声転送とL0候補通知は初期設定では無効で、個別に有効化します。従来のL0 version=1へ、この分類の意味を遡って付けません。

Looker Studioで使用中のSheets summaryはRF測定から直接出力する既存の10列で、L2とは別です。今回の追加で列名・順序・値の意味、既存Grafanaの参照項目は変えません。L1のMesh送信、写真・音声のMesh転送、自動公開は実装していません。

詳細と設定手順：データレベルの定義（リポジトリの閲覧権限が必要です）。

RON-Vの車両側ノードとして使用している M5Stack AtomS3R のプログラムを掲載します。

このスケッチでは、AtomS3Rから車両CANへ接続し、起動直後はListen Onlyでバスを確認した後、OBD-II Mode 01の取得へ移行します。
また、IMUによるSHOCK検出、ボタンによるMARKER、Raspberry PiとのUSB Serial通信、筐体10回ノックによるshutdown要求までを1本に統合しています。

構成

Vehicle CAN / OBD-II
        |
        v
     AtomS3R
        |
        | USB Serial / JSON
        v
 Raspberry Pi 5
        |
        v
      RON-V

AtomS3R PORT AのCANピンは次の構成です。

GPIO2 : CAN_TX
GPIO1 : CAN_RX
CAN   : 500 kbit/s
USB Serial : 115200 bps

起動後はすぐにOBD要求を送信せず、最初の5秒間をListen OnlyとしてCANフレームの存在を確認します。
CANフレームを1件も確認できなかった場合はOBD送信へ移行しません。

主な機能

1. CAN Listen Only → OBD Mode

起動直後は受信専用です。

BOOT
 |
 v
LISTEN ONLY
 |
 | 5秒間 CAN確認
 v
CAN frameあり
 |
 v
OBD Mode

OBD中にTWAIの異常、BUS-OFF、送信失敗などを検出した場合は、安全側へ倒してListen Onlyへ戻します。
一度フォールバックした後は自動でOBD Modeへ復帰せず、AtomS3R再起動まで受信専用としています。

2. OBD-II Mode 01

現在ポーリングしているPIDです。

PID	内容
0x0D	車速
0x0C	エンジンRPM
0x05	冷却水温
0x11	スロットル開度
0x04	エンジン負荷
0x2F	燃料レベル
0x42	ECU電圧
0x5B	Hybrid battery pack remaining life

0x5B はtraction batteryのSOCそのものではないため、コード上でも hv_battery_life として分離しています。

Raspberry Piへは1秒周期でJSONを出力します。

{
  "type": "can",
  "kind": "vehicle",
  "speed": 0.00,
  "rpm": 0.00,
  "coolant": 0.00,
  "throttle": 0.00,
  "engine_load": 0.00,
  "fuel": 0.00,
  "voltage": 0.00,
  "hv_battery_life": 0.00
}

3. IMU SHOCK / MARKER

AtomS3R内蔵IMUから加速度を取得し、閾値を超えた場合はSHOCKイベントをJSONで送信します。

{"type":"imu","event":"shock","acc":1.82}

Button Aを押した場合はMARKERイベントを送信します。

{"type":"button"}

4. 10回ノックによるRaspberry Pi Shutdown要求

筐体を短時間に10回ノックすると、Raspberry Piへshutdown要求を送信します。

{"type":"command","command":"shutdown","source":"imu_knock"}

誤操作対策として、

• 8秒以内に10回
• 1回あたり1.8G以上
• 180ms未満の連続振動は二重カウントしない
• OBD車速が取得できていて3km/hを超えている場合は無効

という条件を入れています。

Raspberry Pi側がshutdown要求を受理すると、

ACK:SHUTDOWN

をAtomS3Rへ返し、LCDを SHUTTING DOWN / Pi ACK 表示へ切り替えます。

5. Raspberry PiのSSID / IP表示

Raspberry PiからUSB Serialで、

NET|SSID|192.168.11.44

または、

IP|192.168.11.44

を送信すると、AtomS3RのLCDへSSIDとIPv4アドレスを表示します。

現在のスケッチ

ファイル名:

AtomS3R_CANGPSTrig6_OBD_IP_Shutdown.ino

[!NOTE]
現段階ではCAN安全側制御を優先し、OBD Mode中に新規 bus_error_count または tx_failed_count を1件検出した時点でListen Onlyへ退避します。
実車評価に応じて、連続エラー回数による判定などへ調整する余地があります。

ソースコード全文

#include <M5Unified.h>
#include "driver/twai.h"

// ========================================
// CAN / OBD-II
//
//起動後3～5秒 Listen
// ↓
// CAN正常確認
// ↓
// OBD Mode
// ↓
// 車速/RPM/水温/etc
// ↓
// json形式で送信　車速、rpmなどRaspi　srial_recive.pyで受信
//TWAI状態を常時監視
// │
// ├─ 正常 → OBD継続
// │
// └─ CAN異常
//      ↓
//    OBD送信停止
//      ↓
//    TWAI停止・再初期化
//      ↓
//    LISTEN ONLY
//      ↓
//    CAN SAFE表示
//      ↓
//    自動ではOBDへ戻らない
// 単発のbus_error_count=1だけでSAFEに入る可能性あるので、1->3程度
//　または条件によってOBD送信に戻るようにするか
// ========================================

// AtomS3R PORT A
// GPIO2 = CAN_TX
// GPIO1 = CAN_RX
#define CAN_TX_PIN GPIO_NUM_2
#define CAN_RX_PIN GPIO_NUM_1

// 2015 VellfireでRAW受信確認済みの設定
static const uint32_t CAN_BITRATE = 500000;

// 起動直後はListen Onlyでバスを確認し、その後OBD送信へ移行
static const unsigned long LISTEN_ONLY_MS = 5000;

// OBD要求間隔
static const unsigned long OBD_REQUEST_INTERVAL_MS = 250;

// 車両データJSON出力間隔
static const unsigned long VEHICLE_OUTPUT_INTERVAL_MS = 1000;

// OBD応答待ち時間
static const unsigned long OBD_RESPONSE_TIMEOUT_MS = 180;

// 標準11bit OBD-II
static const uint32_t OBD_REQUEST_ID = 0x7DF;
static const uint32_t OBD_RESPONSE_ID_MIN = 0x7E8;
static const uint32_t OBD_RESPONSE_ID_MAX = 0x7EF;

enum CanRunMode
{
    CAN_MODE_OFF,
    CAN_MODE_LISTEN,
    CAN_MODE_OBD
};

CanRunMode can_mode = CAN_MODE_OFF;

bool can_ready = false;
unsigned long can_mode_started_ms = 0;

uint32_t listen_frame_count = 0;

// OBD中にTWAI異常を検出したら、安全側へ倒してListen Onlyへ戻す。
// 一度フォールバックした後は自動でOBDへ再移行しない。
// 再度OBDを開始するにはAtomS3Rを再起動する。
bool obd_fault_latched = false;

// TWAIエラー監視用（ドライバをOBDモードで起動した時点の値を基準にする）
uint32_t last_bus_error_count = 0;
uint32_t last_tx_failed_count = 0;

// ========================================
// OBD PID
// ========================================

struct ObdPidDef
{
    uint8_t pid;
    const char* name;
};

// Mode 01 standard PID
// 0x5Bは「HV battery pack remaining life」であり、SOCそのものとは扱わない
const ObdPidDef OBD_PIDS[] =
{
    {0x0D, "speed"},
    {0x0C, "rpm"},
    {0x05, "coolant"},
    {0x11, "throttle"},
    {0x04, "engine_load"},
    {0x2F, "fuel"},
    {0x42, "voltage"},
    {0x5B, "hv_battery_life"}
};

const size_t OBD_PID_COUNT =
    sizeof(OBD_PIDS) / sizeof(OBD_PIDS[0]);

size_t obd_pid_index = 0;
unsigned long last_obd_request_ms = 0;

bool obd_waiting = false;
uint8_t obd_waiting_pid = 0;
unsigned long obd_request_sent_ms = 0;

// ========================================
// 最新OBD値
// ========================================

float obd_speed = NAN;
float obd_rpm = NAN;
float obd_coolant = NAN;
float obd_throttle = NAN;
float obd_engine_load = NAN;
float obd_fuel = NAN;
float obd_voltage = NAN;

// SAE標準PID 0x5B。
// 「充電率SOC」ではなく Hybrid battery pack remaining life として分離。
float obd_hv_battery_life = NAN;

unsigned long last_vehicle_output_ms = 0;

// ========================================
// Canvas
// ========================================

M5Canvas canvas(&M5.Display);

// ========================================
// 状態
// ========================================

bool lastButton = false;

bool shock_armed = true;

bool shock_active = false;
bool marker_active = false;

unsigned long marker_time = 0;
unsigned long shock_time = 0;

float shock_acc = 0.0f;

// ========================================
// IMU 10回ノックによるShutdown要求
// ========================================
//
// Button Aの既存MARKER機能とは完全に独立。
// 筐体を短時間に10回ノックするとRaspberry Piへ
// shutdown要求JSONを送信する。
//
// 誤検知対策:
//   - 8秒以内に10回
//   - 1ノックは1.8G以上
//   - 同一ノックの振動を二重カウントしない
//   - OBD車速が取得できていて3km/h超なら無効
//
// 実車でログを見ながら閾値は調整する。

const float SHUTDOWN_KNOCK_THRESHOLD = 1.8f;
const float SHUTDOWN_KNOCK_RESET     = 1.25f;

const uint8_t SHUTDOWN_KNOCK_TARGET = 10;

const unsigned long SHUTDOWN_KNOCK_WINDOW_MS = 8000;
const unsigned long SHUTDOWN_KNOCK_MIN_INTERVAL_MS = 180;
const unsigned long SHUTDOWN_KNOCK_MAX_GAP_MS = 1500;
const unsigned long SHUTDOWN_KNOCK_DISPLAY_MS = 1200;

bool shutdown_knock_armed = true;
uint8_t shutdown_knock_count = 0;
unsigned long shutdown_knock_first_ms = 0;
unsigned long shutdown_knock_last_ms = 0;
unsigned long shutdown_knock_display_ms = 0;
bool shutdown_requested = false;

// Raspberry Piがshutdown要求を受理したことをLCDへ反映する。
// Pi側から "ACK:SHUTDOWN\n" を返した場合にtrueになる。
bool shutdown_acknowledged = false;

// ========================================
// Raspberry Pi -> AtomS3R Network表示
// ========================================
//
// Piから同じUSB Serialへ、改行付きで次のどちらかを送る。
//
//   NET|SSID|192.168.11.44\n
//   IP|192.168.11.44\n
//
// 通常時は従来の "READY" / CAN状態の2段を使い、
// 1段目にSSID、2段目にIPv4アドレスをラベルなしで表示する。
// SHOCK / MARKER表示中は1段目をイベント表示に切り替えるが、
// 2段目のIPv4アドレスは常時表示する。CAN処理自体は継続するがLCD表示はしない。

static const size_t PI_RX_LINE_SIZE = 160;
char pi_rx_line[PI_RX_LINE_SIZE] = {0};
size_t pi_rx_len = 0;

char network_ssid[33] = "NO SSID";
char network_ip[16] = "NO IP";

// ========================================
// 色
// ========================================

const uint16_t COLOR_NORMAL_BG  = TFT_BLACK;
const uint16_t COLOR_NORMAL_TXT = TFT_WHITE;

const uint16_t COLOR_MARKER_BG  = TFT_GREEN;
const uint16_t COLOR_MARKER_TXT = TFT_BLACK;

const uint16_t COLOR_SHOCK_BG   = TFT_RED;
const uint16_t COLOR_SHOCK_TXT  = TFT_WHITE;

// ShutdownはSHOCK(赤)、MARKER(緑)と明確に区別する。
// 10回ノック成立後は全面を青で固定する。
const uint16_t COLOR_SHUTDOWN_BG  = TFT_BLUE;
const uint16_t COLOR_SHUTDOWN_TXT = TFT_WHITE;

// ========================================
// IMU
// ========================================

const float SHOCK_THRESHOLD = 1.5f;
const float SHOCK_RESET     = 1.2f;

const unsigned long DISPLAY_HOLD_MS = 3000;

// ========================================
// JSON helper
// ========================================

void printJsonFloat(const char* key, float value, bool comma)
{
    Serial.printf("\"%s\":", key);

    if (isnan(value))
    {
        Serial.print("null");
    }
    else
    {
        Serial.print(value, 2);
    }

    if (comma)
    {
        Serial.print(",");
    }
}

// ========================================
// TWAI driver helper
// ========================================

void stopCAN()
{
    if (!can_ready)
    {
        return;
    }

    twai_status_info_t status;
    esp_err_t status_result =
        twai_get_status_info(&status);

    if (status_result == ESP_OK)
    {
        // RUNNING時だけtwai_stop()を呼ぶ。
        // BUS_OFFはそのままuninstall可能。
        if (status.state == TWAI_STATE_RUNNING)
        {
            twai_stop();
        }
    }
    else
    {
        // 状態取得に失敗した場合も停止を試みる。
        twai_stop();
    }

    esp_err_t uninstall_result =
        twai_driver_uninstall();

    if (uninstall_result != ESP_OK)
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"driver_uninstall_fail\","
            "\"error\":%d}\n",
            uninstall_result
        );
    }

    can_ready = false;
    can_mode = CAN_MODE_OFF;
}

// ========================================
// CAN初期化
// ========================================

bool initCAN(CanRunMode mode)
{
    if (can_ready)
    {
        stopCAN();
    }

    twai_mode_t twai_mode =
        (mode == CAN_MODE_LISTEN)
        ? TWAI_MODE_LISTEN_ONLY
        : TWAI_MODE_NORMAL;

    twai_general_config_t g_config =
        TWAI_GENERAL_CONFIG_DEFAULT(
            CAN_TX_PIN,
            CAN_RX_PIN,
            twai_mode
        );

    // 500kbps
    twai_timing_config_t t_config =
        TWAI_TIMING_CONFIG_500KBITS();

    // OBD開始前のListenでは全CANを確認。
    // OBDモードでも応答ID確認のため当面ACCEPT_ALL。
    twai_filter_config_t f_config =
        TWAI_FILTER_CONFIG_ACCEPT_ALL();

    esp_err_t result =
        twai_driver_install(
            &g_config,
            &t_config,
            &f_config
        );

    if (result != ESP_OK)
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"driver_install_fail\","
            "\"error\":%d}\n",
            result
        );

        return false;
    }

    result = twai_start();

    if (result != ESP_OK)
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"start_fail\","
            "\"error\":%d}\n",
            result
        );

        twai_driver_uninstall();

        return false;
    }

    can_ready = true;
    can_mode = mode;
    can_mode_started_ms = millis();

    // OBDモード開始時点のエラーカウンタを基準値として保存
    if (mode == CAN_MODE_OBD)
    {
        twai_status_info_t status;

        if (twai_get_status_info(&status) == ESP_OK)
        {
            last_bus_error_count =
                status.bus_error_count;

            last_tx_failed_count =
                status.tx_failed_count;
        }
        else
        {
            last_bus_error_count = 0;
            last_tx_failed_count = 0;
        }
    }

    if (mode == CAN_MODE_LISTEN)
    {
        Serial.println(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"listen_ready\","
            "\"bitrate\":500000}"
        );
    }
    else
    {
        Serial.println(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"obd_ready\","
            "\"bitrate\":500000}"
        );
    }

    return true;
}

// ========================================
// Listen Only RAW受信
// ========================================

void receiveCANListen()
{
    if (
        !can_ready ||
        can_mode != CAN_MODE_LISTEN
    )
    {
        return;
    }

    twai_message_t message;

    while (
        twai_receive(
            &message,
            0
        ) == ESP_OK
    )
    {
        if (message.rtr)
        {
            continue;
        }

        listen_frame_count++;

        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"raw\","
            "\"id\":\"0x%lX\","
            "\"dlc\":%d,"
            "\"data\":\"",
            message.identifier,
            message.data_length_code
        );

        for (
            int i = 0;
            i < message.data_length_code;
            i++
        )
        {
            Serial.printf(
                "%02X",
                message.data[i]
            );

            if (
                i <
                message.data_length_code - 1
            )
            {
                Serial.print(" ");
            }
        }

        Serial.println("\"}");
    }
}

// ========================================
// OBD中のCAN異常 -> Listen Onlyへ退避
// ========================================

void fallbackToListenOnly(
    const char* reason,
    const twai_status_info_t* status
)
{
    // 最初にラッチして、以後の自動OBD復帰を禁止
    obd_fault_latched = true;
    obd_waiting = false;

    if (status != nullptr)
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"safety\","
            "\"status\":\"fallback_to_listen\","
            "\"reason\":\"%s\","
            "\"state\":%d,"
            "\"tx_err\":%lu,"
            "\"rx_err\":%lu,"
            "\"tx_failed\":%lu,"
            "\"bus_errors\":%lu}\n",
            reason,
            (int)status->state,
            (unsigned long)status->tx_error_counter,
            (unsigned long)status->rx_error_counter,
            (unsigned long)status->tx_failed_count,
            (unsigned long)status->bus_error_count
        );
    }
    else
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"safety\","
            "\"status\":\"fallback_to_listen\","
            "\"reason\":\"%s\"}\n",
            reason
        );
    }

    // 現在のNormal Modeを終了
    stopCAN();

    // Listen再開始時はカウンタを新しく数え直す
    listen_frame_count = 0;

    delay(100);

    // 以後は受信専用。
    // obd_fault_latched=true のため、5秒経過してもOBDへ自動復帰しない。
    if (!initCAN(CAN_MODE_LISTEN))
    {
        Serial.println(
            "{\"type\":\"can\","
            "\"kind\":\"safety\","
            "\"status\":\"listen_restart_fail\"}"
        );
    }
}

// trueを返した場合、このloopではOBD処理を続けない
bool monitorOBDCanHealth()
{
    if (
        !can_ready ||
        can_mode != CAN_MODE_OBD
    )
    {
        return false;
    }

    twai_status_info_t status;

    esp_err_t result =
        twai_get_status_info(&status);

    if (result != ESP_OK)
    {
        fallbackToListenOnly(
            "status_read_fail",
            nullptr
        );

        return true;
    }

    // BUS-OFF / RECOVERING / 想定外STOPPED
    if (status.state == TWAI_STATE_BUS_OFF)
    {
        fallbackToListenOnly(
            "bus_off",
            &status
        );

        return true;
    }

    if (status.state == TWAI_STATE_RECOVERING)
    {
        fallbackToListenOnly(
            "recovering",
            &status
        );

        return true;
    }

    if (status.state != TWAI_STATE_RUNNING)
    {
        fallbackToListenOnly(
            "not_running",
            &status
        );

        return true;
    }

    // バスエラーが1件でも新規発生したら安全側へ退避
    if (
        status.bus_error_count >
        last_bus_error_count
    )
    {
        fallbackToListenOnly(
            "bus_error",
            &status
        );

        return true;
    }

    // OBD要求の送信失敗が1件でも発生したら退避
    if (
        status.tx_failed_count >
        last_tx_failed_count
    )
    {
        fallbackToListenOnly(
            "tx_failed",
            &status
        );

        return true;
    }

    // 正常時は基準値更新
    last_bus_error_count =
        status.bus_error_count;

    last_tx_failed_count =
        status.tx_failed_count;

    return false;
}

// ========================================
// OBD要求送信
// ========================================

bool sendObdPidRequest(uint8_t pid)
{
    if (
        !can_ready ||
        can_mode != CAN_MODE_OBD
    )
    {
        return false;
    }

    twai_message_t tx = {};

    tx.identifier = OBD_REQUEST_ID;
    tx.extd = 0;
    tx.rtr = 0;
    tx.data_length_code = 8;

    // ISO-TP Single Frame:
    // 02 = 2 byte payload
    // 01 = Current Data
    // PID
    tx.data[0] = 0x02;
    tx.data[1] = 0x01;
    tx.data[2] = pid;
    tx.data[3] = 0x00;
    tx.data[4] = 0x00;
    tx.data[5] = 0x00;
    tx.data[6] = 0x00;
    tx.data[7] = 0x00;

    esp_err_t result =
        twai_transmit(
            &tx,
            pdMS_TO_TICKS(20)
        );

    if (result != ESP_OK)
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"obd_error\","
            "\"pid\":\"0x%02X\","
            "\"error\":%d}\n",
            pid,
            result
        );

        return false;
    }

    obd_waiting = true;
    obd_waiting_pid = pid;
    obd_request_sent_ms = millis();

    return true;
}

// ========================================
// OBD値デコード
// ========================================

void decodeObdResponse(
    uint8_t pid,
    const uint8_t* data,
    uint8_t dlc
)
{
    // 標準応答:
    // data[0] = ISO-TP length
    // data[1] = 0x41 (Mode 01 response)
    // data[2] = PID
    // data[3...] = A, B...
    if (dlc < 4)
    {
        return;
    }

    uint8_t A = data[3];
    uint8_t B = (dlc >= 5) ? data[4] : 0;

    switch (pid)
    {
        case 0x0D:
            // Vehicle speed [km/h]
            obd_speed = (float)A;
            break;

        case 0x0C:
            // Engine RPM
            obd_rpm =
                ((256.0f * A) + B) / 4.0f;
            break;

        case 0x05:
            // Coolant temperature [degC]
            obd_coolant =
                (float)A - 40.0f;
            break;

        case 0x11:
            // Throttle position [%]
            obd_throttle =
                (100.0f * A) / 255.0f;
            break;

        case 0x04:
            // Calculated engine load [%]
            obd_engine_load =
                (100.0f * A) / 255.0f;
            break;

        case 0x2F:
            // Fuel level input [%]
            obd_fuel =
                (100.0f * A) / 255.0f;
            break;

        case 0x42:
            // Control module voltage [V]
            obd_voltage =
                ((256.0f * A) + B) / 1000.0f;
            break;

        case 0x5B:
            // Hybrid battery pack remaining life [%]
            // 注意: traction battery SOCとは別概念
            obd_hv_battery_life =
                (100.0f * A) / 255.0f;
            break;

        default:
            break;
    }
}

// ========================================
// OBD応答受信
// ========================================

void receiveOBD()
{
    if (
        !can_ready ||
        can_mode != CAN_MODE_OBD
    )
    {
        return;
    }

    twai_message_t message;

    while (
        twai_receive(
            &message,
            0
        ) == ESP_OK
    )
    {
        if (
            message.rtr ||
            message.extd
        )
        {
            continue;
        }

        if (
            message.identifier <
                OBD_RESPONSE_ID_MIN ||
            message.identifier >
                OBD_RESPONSE_ID_MAX
        )
        {
            continue;
        }

        if (message.data_length_code < 4)
        {
            continue;
        }

        // Mode 01 positive response = 0x41
        if (message.data[1] != 0x41)
        {
            continue;
        }

        uint8_t pid = message.data[2];

        decodeObdResponse(
            pid,
            message.data,
            message.data_length_code
        );

        if (
            obd_waiting &&
            pid == obd_waiting_pid
        )
        {
            obd_waiting = false;
        }
    }
}

// ========================================
// OBDポーリング
// ========================================

void pollOBD(unsigned long now)
{
    if (
        !can_ready ||
        can_mode != CAN_MODE_OBD
    )
    {
        return;
    }

    // 応答が来ないPIDでも止まらないようタイムアウト
    if (
        obd_waiting &&
        now - obd_request_sent_ms >
            OBD_RESPONSE_TIMEOUT_MS
    )
    {
        obd_waiting = false;
    }

    if (obd_waiting)
    {
        return;
    }

    if (
        now - last_obd_request_ms <
        OBD_REQUEST_INTERVAL_MS
    )
    {
        return;
    }

    uint8_t pid =
        OBD_PIDS[obd_pid_index].pid;

    if (sendObdPidRequest(pid))
    {
        last_obd_request_ms = now;
    }

    obd_pid_index++;

    if (obd_pid_index >= OBD_PID_COUNT)
    {
        obd_pid_index = 0;
    }
}

// ========================================
// 車両データJSON出力
// ========================================

void outputVehicleData(unsigned long now)
{
    if (
        can_mode != CAN_MODE_OBD ||
        now - last_vehicle_output_ms <
            VEHICLE_OUTPUT_INTERVAL_MS
    )
    {
        return;
    }

    last_vehicle_output_ms = now;

    Serial.print(
        "{\"type\":\"can\","
        "\"kind\":\"vehicle\","
    );

    printJsonFloat(
        "speed",
        obd_speed,
        true
    );

    printJsonFloat(
        "rpm",
        obd_rpm,
        true
    );

    printJsonFloat(
        "coolant",
        obd_coolant,
        true
    );

    printJsonFloat(
        "throttle",
        obd_throttle,
        true
    );

    printJsonFloat(
        "engine_load",
        obd_engine_load,
        true
    );

    printJsonFloat(
        "fuel",
        obd_fuel,
        true
    );

    printJsonFloat(
        "voltage",
        obd_voltage,
        true
    );

    printJsonFloat(
        "hv_battery_life",
        obd_hv_battery_life,
        false
    );

    Serial.println("}");
}

// ========================================
// Listen Only -> OBD Normal 切替
// ========================================

void updateCanMode(unsigned long now)
{
    // OBD中のCAN異常でListen Onlyへ戻った場合は、
    // 自動でOBDへ再突入させない。
    if (obd_fault_latched)
    {
        return;
    }

    if (
        can_ready &&
        can_mode == CAN_MODE_LISTEN &&
        now - can_mode_started_ms >=
            LISTEN_ONLY_MS
    )
    {
        Serial.printf(
            "{\"type\":\"can\","
            "\"kind\":\"status\","
            "\"status\":\"listen_complete\","
            "\"frames\":%lu}\n",
            listen_frame_count
        );

        // バスから1フレームも受信していない場合は
        // OBD送信へ移行しない
        if (listen_frame_count == 0)
        {
            Serial.println(
                "{\"type\":\"can\","
                "\"kind\":\"status\","
                "\"status\":\"obd_cancel_no_bus\"}"
            );

            return;
        }

        stopCAN();

        delay(100);

        initCAN(CAN_MODE_OBD);
    }
}

// ========================================
// Raspberry Pi -> AtomS3R USB Serial受信
// ========================================

void setNetworkInfo(
    const char* ssid,
    const char* ip
)
{
    if (ssid != nullptr)
    {
        strlcpy(
            network_ssid,
            ssid,
            sizeof(network_ssid)
        );
    }

    if (
        ip != nullptr &&
        ip[0] != '\0'
    )
    {
        strlcpy(
            network_ip,
            ip,
            sizeof(network_ip)
        );
    }
}

void handlePiSerialLine(char* line)
{
    if (
        line == nullptr ||
        line[0] == '\0'
    )
    {
        return;
    }

    // NET|SSID|192.168.11.44
    if (strncmp(line, "NET|", 4) == 0)
    {
        char* saveptr = nullptr;

        strtok_r(
            line,
            "|",
            &saveptr
        );

        char* ssid = strtok_r(
            nullptr,
            "|",
            &saveptr
        );

        char* ip = strtok_r(
            nullptr,
            "|",
            &saveptr
        );

        if (
            ssid != nullptr &&
            ip != nullptr
        )
        {
            setNetworkInfo(
                ssid,
                ip
            );
        }

        return;
    }

    // IP|192.168.11.44
    if (strncmp(line, "IP|", 3) == 0)
    {
        const char* ip = line + 3;

        if (ip[0] != '\0')
        {
            setNetworkInfo(
                nullptr,
                ip
            );
        }

        return;
    }

    // Raspberry Piがshutdown要求を受理したことを通知。
    // Pi側はshutdown処理開始前にこの1行を返す。
    //   ACK:SHUTDOWN\n
    if (strcmp(line, "ACK:SHUTDOWN") == 0)
    {
        shutdown_acknowledged = true;
        return;
    }
}

void receivePiSerial()
{
    while (Serial.available() > 0)
    {
        const char c =
            static_cast<char>(Serial.read());

        if (c == '\n')
        {
            if (pi_rx_len > 0)
            {
                pi_rx_line[pi_rx_len] = '\0';

                handlePiSerialLine(
                    pi_rx_line
                );

                pi_rx_len = 0;
            }

            continue;
        }

        if (c == '\r')
        {
            continue;
        }

        if (
            pi_rx_len <
            PI_RX_LINE_SIZE - 1
        )
        {
            pi_rx_line[pi_rx_len++] = c;
        }
        else
        {
            // 長すぎる入力は破棄して次の行から復帰
            pi_rx_len = 0;
        }
    }
}

// ========================================
// setup
// ========================================

void setup()
{
    auto cfg = M5.config();
    M5.begin(cfg);

    Serial.begin(115200);
    delay(1500);

    // ------------------------------------
    // LCD
    // ------------------------------------

    M5.Display.setRotation(0);
    M5.Display.setBrightness(128);

    canvas.createSprite(
        M5.Display.width(),
        M5.Display.height()
    );

    canvas.setTextDatum(middle_center);

    canvas.fillSprite(TFT_BLACK);

    canvas.setTextColor(
        TFT_WHITE,
        TFT_BLACK
    );

    canvas.setTextSize(2);

    canvas.drawString(
        "START",
        64,
        64
    );

    canvas.pushSprite(0, 0);

    // Raspberry PiへBOOT通知
    Serial.println(
        "{\"type\":\"boot\"}"
    );

    // 最初は必ずListen Only
    can_ready =
        initCAN(CAN_MODE_LISTEN);
}

// ========================================
// loop
// ========================================

void loop()
{
    M5.update();

    // Raspberry PiからのNetwork情報を非ブロッキング受信
    receivePiSerial();

    const unsigned long now = millis();

    // ====================================
    // CAN / OBD
    // ====================================

    if (can_mode == CAN_MODE_LISTEN)
    {
        receiveCANListen();
    }
    else if (can_mode == CAN_MODE_OBD)
    {
        // CAN異常を最優先で監視。
        // 異常検出時はListen Onlyへ戻り、このloopでは送信を続けない。
        bool can_fault =
            monitorOBDCanHealth();

        if (!can_fault)
        {
            receiveOBD();

            // 受信処理後にも状態を再確認してから次のPIDを送る
            can_fault =
                monitorOBDCanHealth();

            if (!can_fault)
            {
                pollOBD(now);
                outputVehicleData(now);
            }
        }
    }

    updateCanMode(now);

    // ====================================
    // IMU取得
    // ====================================

    float ax = 0.0f;
    float ay = 0.0f;
    float az = 0.0f;

    M5.Imu.getAccel(
        &ax,
        &ay,
        &az
    );

    float acc =
        sqrt(
            ax * ax +
            ay * ay +
            az * az
        );

    // ====================================
    // IMU 10回ノック Shutdown検出
    // ====================================
    //
    // 通常のSHOCKイベントとは別の検出器を使う。
    // shock_activeが3秒保持されている間でも、
    // shutdown用ノックは独立して数え続ける。

    // 車速が取得できていて3km/hを超えている間は無効。
    // NAN（OBD未取得）の場合は停止判定不能なので許可する。
    bool shutdown_knock_allowed =
        isnan(obd_speed) ||
        obd_speed <= 3.0f;

    if (!shutdown_knock_allowed)
    {
        // 走行を検出したら途中のノック列を破棄
        shutdown_knock_count = 0;
        shutdown_knock_first_ms = 0;
        shutdown_knock_last_ms = 0;
    }

    // ノック列が途中で止まった場合は最初からやり直す
    if (
        shutdown_knock_count > 0 &&
        (
            now - shutdown_knock_first_ms >
                SHUTDOWN_KNOCK_WINDOW_MS ||
            now - shutdown_knock_last_ms >
                SHUTDOWN_KNOCK_MAX_GAP_MS
        )
    )
    {
        shutdown_knock_count = 0;
        shutdown_knock_first_ms = 0;
        shutdown_knock_last_ms = 0;
    }

    if (
        shutdown_knock_allowed &&
        !shutdown_requested &&
        shutdown_knock_armed &&
        acc >= SHUTDOWN_KNOCK_THRESHOLD
    )
    {
        // IMUのリンギング等で同じ衝撃を二重カウントしない
        bool interval_ok =
            shutdown_knock_count == 0 ||
            now - shutdown_knock_last_ms >=
                SHUTDOWN_KNOCK_MIN_INTERVAL_MS;

        if (interval_ok)
        {
            if (shutdown_knock_count == 0)
            {
                shutdown_knock_first_ms = now;
            }

            shutdown_knock_count++;
            shutdown_knock_last_ms = now;
            shutdown_knock_display_ms = now;

            // デバッグ/記録用。Pi側は未対応でも無視可能。
            Serial.printf(
                "{\"type\":\"shutdown_knock\","
                "\"count\":%u,"
                "\"target\":%u,"
                "\"acc\":%.2f}\n",
                shutdown_knock_count,
                SHUTDOWN_KNOCK_TARGET,
                acc
            );

            if (
                shutdown_knock_count >=
                    SHUTDOWN_KNOCK_TARGET &&
                now - shutdown_knock_first_ms <=
                    SHUTDOWN_KNOCK_WINDOW_MS
            )
            {
                shutdown_requested = true;

                // Raspberry Pi側でこのJSONだけを許可コマンドとして扱う。
                Serial.println(
                    "{\"type\":\"command\","
                    "\"command\":\"shutdown\","
                    "\"source\":\"imu_knock\"}"
                );
            }
        }

        // 一度閾値を超えたら、加速度が十分戻るまで再カウントしない
        shutdown_knock_armed = false;
    }

    if (acc <= SHUTDOWN_KNOCK_RESET)
    {
        shutdown_knock_armed = true;
    }

    // ====================================
    // Button
    // ====================================

    bool nowButton =
        M5.BtnA.isPressed();

    if (
        nowButton &&
        !lastButton
    )
    {
        Serial.println(
            "{\"type\":\"button\"}"
        );

        marker_time = now;
        marker_active = true;
    }

    lastButton = nowButton;

    // ====================================
    // IMU Shock検出
    // ====================================

    if (
        acc > SHOCK_THRESHOLD &&
        shock_armed &&
        !shock_active
    )
    {
        shock_acc = acc;
        shock_time = now;
        shock_active = true;
        shock_armed = false;

        Serial.printf(
            "{\"type\":\"imu\","
            "\"event\":\"shock\","
            "\"acc\":%.2f}\n",
            shock_acc
        );
    }

    // ====================================
    // SHOCK 3秒保持終了
    // ====================================

    if (
        shock_active &&
        now - shock_time >=
            DISPLAY_HOLD_MS
    )
    {
        shock_active = false;
    }

    // ====================================
    // Button 3秒保持終了
    // ====================================

    if (
        marker_active &&
        now - marker_time >=
            DISPLAY_HOLD_MS
    )
    {
        marker_active = false;
    }

    // ====================================
    // SHOCK再検出許可
    // ====================================

    if (
        !shock_active &&
        acc < SHOCK_RESET
    )
    {
        shock_armed = true;
    }

    // ====================================
    // 表示加速度
    // ====================================

    float display_acc = acc;

    if (shock_active)
    {
        display_acc = shock_acc;
    }

    // ====================================
    // LCD 色
    // ====================================

    uint16_t bg_color =
        COLOR_NORMAL_BG;

    uint16_t text_color =
        COLOR_NORMAL_TXT;

    // 表示優先順位:
    // Shutdown(青) > SHOCK(赤) > MARKER(緑) > Normal(黒)
    // 10回目のノックはSHOCK条件にも該当するが、
    // shutdown_requestedを最優先にして必ず青画面へ切り替える。
    if (shutdown_requested)
    {
        bg_color =
            COLOR_SHUTDOWN_BG;

        text_color =
            COLOR_SHUTDOWN_TXT;
    }
    else if (shock_active)
    {
        bg_color =
            COLOR_SHOCK_BG;

        text_color =
            COLOR_SHOCK_TXT;
    }
    else if (marker_active)
    {
        bg_color =
            COLOR_MARKER_BG;

        text_color =
            COLOR_MARKER_TXT;
    }

    // ====================================
    // Canvas描画
    // ====================================

    canvas.fillSprite(
        bg_color
    );

    canvas.setTextColor(
        text_color,
        bg_color
    );

    canvas.setTextDatum(
        middle_center
    );

    // Shutdown成立後は通常画面を描かず、
    // 青背景の専用画面を再起動まで保持する。
    if (shutdown_requested)
    {
        if (shutdown_acknowledged)
        {
            canvas.setTextSize(2);
            canvas.drawString(
                "SHUTTING",
                64,
                43
            );

            canvas.setTextSize(3);
            canvas.drawString(
                "DOWN",
                64,
                78
            );

            canvas.setTextSize(1);
            canvas.drawString(
                "Pi ACK",
                64,
                111
            );
        }
        else
        {
            canvas.setTextSize(2);
            canvas.drawString(
                "SHUTDOWN",
                64,
                42
            );

            canvas.setTextSize(3);
            canvas.drawString(
                "REQUEST",
                64,
                78
            );

            canvas.setTextSize(1);
            canvas.drawString(
                "10/10 KNOCK",
                64,
                111
            );
        }
    }
    else
    {
        // PUSH
        canvas.setTextSize(4);

        canvas.drawString(
            "PUSH",
            64,
            30
        );

        // IMU
        char buf[32];

        snprintf(
            buf,
            sizeof(buf),
            "IMU %.1f",
            display_acc
        );

        canvas.setTextSize(3);

        canvas.drawString(
            buf,
            64,
            80
        );

        // ====================================
        // 状態 / Network表示
        // ====================================
        // 通常時はSSID / IPv4。
        // ノック1～9回は下段へ進捗OFF n/10を表示する。

        if (
            shutdown_knock_count > 0 &&
            now - shutdown_knock_display_ms <
                SHUTDOWN_KNOCK_DISPLAY_MS
        )
        {
            char knock_buf[24];

            snprintf(
                knock_buf,
                sizeof(knock_buf),
                "OFF %u/%u",
                shutdown_knock_count,
                SHUTDOWN_KNOCK_TARGET
            );

            canvas.setTextSize(2);

            canvas.drawString(
                knock_buf,
                64,
                105
            );
        }
        else if (shock_active)
        {
            canvas.setTextSize(2);

            canvas.drawString(
                "SHOCK",
                64,
                105
            );
        }
        else if (marker_active)
        {
            canvas.setTextSize(2);

            canvas.drawString(
                "MARKER",
                64,
                105
            );
        }
        else
        {
            char ssid_buf[32];

            snprintf(
                ssid_buf,
                sizeof(ssid_buf),
                "%.20s",
                network_ssid
            );

            canvas.setTextSize(1);

            canvas.drawString(
                ssid_buf,
                64,
                105
            );
        }

        // IPv4は通常画面では常時表示（ラベルなし）
        canvas.setTextSize(1);

        canvas.drawString(
            network_ip,
            64,
            122
        );
    }

    canvas.pushSprite(
        0,
        0
    );

    // 約200Hz loop上限
    delay(5);
}

まとめ

このAtomS3Rスケッチで、

CAN RAW受信
   +
OBD-II取得
   +
IMU SHOCK
   +
MARKER
   +
Shutdown要求
   +
SSID / IP表示

を1台に統合しました。

Raspberry Pi側ではUSB SerialでJSONを受信し、RON-VのVehicle ReceiverからSQLite・イベント処理・Web表示へ接続します。

次は実車CANでの長時間動作を確認しながら、CANエラー判定や取得PIDを調整していきます。