ESP32ファームウェアと通信プロトコルの仕組み

自律走行の計画・判断を担う上位コンピュータ(Raspberry Pi 5)と、実際にモーターを駆動する低レベル制御(ESP32)は、役割を明確に分離している。上位側が高度な処理でクラッシュしたり通信が途絶えたりしても、モーター駆動の最終防衛ラインだけは独立して動き続けるようにするためである。

ESP32開発ボードの一般的な例

ESP32開発ボードの一例(実機ではなく一般的な参考写真)。newbotではFreenove製のESP32開発ボードを使用している。

画像: Edwiyanto, Wikimedia Commons(CC BY-SA 4.0)

役割分担: 高度な判断とリアルタイム制御の分離

Raspberry Pi 5は自己位置推定・地図生成・経路計画といった計算量の多い処理を担う。ESP32はエンコーダの読み取りとモーターのPWM制御という、遅延の少なさが要求されるリアルタイム性の高い処理に専念する。両者はUSBシリアル経由で、115200bpsのボーレート・改行区切りのテキストプロトコルで通信する。

シリアルプロトコル: 40ms周期のコマンド

Pi5からESP32へは、40ms周期で以下のコマンドを送り続ける。

V,<左輪mm/s>,<右輪mm/s> — 速度指令(自律モード時のみ有効)
X0 / X1 — 非常停止の解除/発動(毎周期送信。発動中はモードに関係なく強制停止)
L,<前進上限mm/s> — 前進速度上限(毎周期送信。-1は無制限、0は前進禁止)
P — コントローラーの新規ペアリング要求(Bluetooth鍵を消去して再スキャン)

速度指令より前進上限を先に送ることで、同じ周期内で新しい速度指令が古い上限値のまま通過してしまう抜け道を防いでいる。

ESP32からPi5へは、同じく40ms周期で車輪の回転量・経過時間・現在の動作モード・バッテリー電圧・コントローラー接続状態を返す。

二重のwatchdog

通信が途絶えた場合の安全側フォールバックを、ESP32側とPi5側の両方に持たせている。ESP32側は自律モード時300ms、手動モード時500msでコマンド未受信を検知すると自律的に安全側へ倒れる。Pi5側も500msで相手の応答途絶を検知する。どちらか一方だけでなく双方に持たせているのは、単一の監視機構だけに安全性を委ねないための設計である。

コントローラーのペアリング

手動操作用のコントローラー(Nintendo Switch Proコントローラー、Bluepad32経由)のペアリングは、ESP32ファーム側の役割として実装している。未接続の間は自動的にスキャン待機する既定動作のため、Pi5の起動→通信確立という順序だけで「起動すればペアリング待機状態になる」という単純な運用にできている。

`BP32.setup()`だけでは不十分だった実機ペアリングのつまずき

Bluepad32のサンプルコードではBP32.setup()を呼ぶだけでコントローラーが接続できるように見えるが、実機では新規ペアリングが成立しなかった。原因はBP32.setup()が新規Bluetooth接続の受け入れを既定で行わない点にあり、明示的にBP32.enableNewBluetoothConnections(true)を呼ぶ必要があった。加えて、一度ペアリングに失敗したコントローラーは古いBluetooth鍵をProコントローラー側が保持し続け、再試行しても弾かれることがあったため、シリアル経由でPコマンドを送るとforgetBluetoothKeys()を呼んで鍵を消去し、ペアリングをやり直せるようにした(前節のコマンド一覧にあるPはこの役割)。ダッシュボード側にも「ペアリングやり直し」ボタンを設け、/procon_pairing_resetサービスからこのコマンドをESP32へ送れるようにしている。

Bluetoothのハードウェア要件にも制約がある。Nintendo Switch ProコントローラーはBluetooth Classic (BR/EDR) で通信しており、BLE(Bluetooth Low Energy)専用のチップでは接続できない。そのため使用するESP32モジュールは、BR/EDRを含む無印のESP32(WROOM-32等)に限られ、BLEのみをサポートするESP32-S3やESP32-C3では動作しない。

実際のペアリング手順は次の通りである。

ESP32を起動し、シリアルログでBP32 setup complete, waiting for controller...を確認する
Proコントローラーの同期ボタン(上部のマイナスボタン脇の小さなボタン)を長押しし、LEDを点滅させてペアリングモードに入れる
シリアルログにCONTROLLER CONNECTED, Model: ...のような接続完了メッセージが出れば成功
接続に失敗する場合はPコマンドで鍵を消去してから再試行する

ファームウェア書き込み

esptool.pyを使い、ボーレート921600・フラッシュモードdioでビルド済みバイナリを書き込む。開発中は書き込み後にシリアルモニタでログを確認し、上記のペアリング完了メッセージと、40ms周期のテレメトリ送信が安定して出ているかを目視で確認する運用にしている。

`CLAUDE.md`に記録した実運用の罠

実機投入までの過程で、ファームウェアの挙動に関連して次のような問題を踏み、対処してきた。

deploy.shがファームウェアを同期していなかった問題: Pi5側のdeploy.shはROS2パッケージ群を同期するものの、ESP32へ書き込む.inoファイルはリポジトリの同期対象に含まれていなかった。このため、Pi5に実際に搭載されていたファームウェア(e-stop・Bluepad32・エンコーダ4逓倍化に対応した版)が、リポジトリ上には反映されないまま長期間乖離していたことが判明した。発覚時にリポジトリに残っていたのは非常機構を一切持たない161行の最小版で、実機に書き込まれていた版とは行数も機能も大きく異なっていた。デプロイスクリプトの対象範囲を「ROS2ノードだけで完結する」と思い込んでいたことが原因で、ハードウェアに書き込む成果物も同期対象に含めるべきという教訓になった。
MANUALモードでの速度指令上書きバグ: 手動操作(MANUALモード)中に、本来なら無視されるはずの自律系からの速度指令(Vコマンド)がモーター出力を上書きしてしまう不具合があった。原因調査では、実際のノード構成(twist_muxやesp32_bridgeの実際の配線)を再現せずに単体でコードを追ったため、当初の仮説が的外れだった。最終的に、実際に動いているノード同士の通信をros2 topic echoで直接観測して初めて、想定と異なる経路でコマンドが流れ込んでいることが判明した。仮説だけで原因を絞り込まず、実際に動いている系を観測することが、この種のバグ調査で最も効くという教訓が残っている。
テレメトリの列数固定: ESP32からPi5へ送るテレメトリ行は列数(現在6列)が固定である前提で上位側がパースしている。ファームウェア側でデバッグ用の列を安易に追加すると、パーサーが黙って誤動作する(例外を出さずに値がずれる)ため、列を増減する際は送信側・受信側を同時に変更する運用にしている。

障害物ガードのクランプが「宣言だけされて一度も効いていなかった」事故

上位から送られる前進速度上限(Lコマンド)とは別に、旋回速度を制限するRコマンドを2026年8月13日に追加した。ところが実装したのは受信して変数に代入するところまでで、その値を実際にモーター出力へ適用する関数を書き忘れていた。つまりrotationLimitMmSという変数は宣言と代入だけがあり、コード上どこからも参照されない状態のまま本番のガードロジックとして扱われていた。

この状態のまま、Nav2の自動リカバリ動作(その場で回転して行き詰まりを脱するSpin)の最中に車体側面が障害物へ接触する事故が起きている。旋回ガードを実装したはずなのに旋回が野放しだったことが直接の原因だった。ログにはTurning 1.57 for spin behaviorという記録が12秒近く残っていたが、角速度成分は常に無制限のまま通過しており、車輪の応答としては旋回していなかったと考えられる。「configとして持っている」ことと「実際にモーターへ効いている」ことの間には、実装漏れという形で簡単に溝ができるという教訓を残した事故である。

修正後のapplyRotationLimit()は、前進成分 $v$ と旋回成分 $w$ を左右輪速度 $l,r$ から分解し、 $w$ だけをクランプしてから合成し直す。前進側のapplyForwardLimit()と対になる実装である。

void applyRotationLimit(float &l, float &r) {
  float limit = rotationLimitMmS;
  if (millis() - lastLimitMillis > LIMIT_TIMEOUT_MS) {
    limit = LIMIT_FAILSAFE_MM_S;   // 指令途絶時は低速フェイルセーフへ
  }
  if (limit < 0.0f) return;        // 無制限
  if (allAxesBlocked()) {
    limit = ROTATION_ESCAPE_CREEP_MM_S;  // 全軸拘束時は脱出用クリープを残す
  }
  float v = (l + r) / 2.0f;
  float w = (r - l) / 2.0f;
  if (w > limit) w = limit;
  else if (w < -limit) w = -limit;
  else return;
  l = v - w;
  r = v + w;
}

さらに2026年8月14日、4件目の接触事故として後方への衝突が発生している。前進と旋回のクランプは実装済みだったが、後退方向(v<0)には一切のクランプがかかっていなかったためで、Nav2のBackUpリカバリ動作が引き金になった。前進・旋回を厳しく制限すればするほど、Nav2は「進めない」と判断してBackUpを選びやすくなる、という皮肉な連鎖もここで確認している。対策として、applyReverseLimit()を新設し、後退成分にも同じ枠組みでクランプを掛けるようにした。

前進・旋回・後退の3方向すべてが同時に0(完全拘束)になると、機体はどの方向にも動けず固まる。実際に45秒以上動けなくなり、手で動かして救出した事例も発生している。この反省から、3方向すべてが塞がれた状態をallAxesBlocked()で検知し、その場合は後退方向にだけ低速のクリープ(脱出用の最低限の速度)を残すようにした。後で旋回方向にも同種のクリープ(ROTATION_ESCAPE_CREEP_MM_S)を追加している。これは、Nav2がSpinリカバリを選んだのに旋回上限が0のままで実際には一切回転できていなかった事象を確認したことによる追加対応である。旋回のクリープを後退よりやや控えめな値にしているのは、旋回は車体の角が振れる分、後退よりもクリアランスに余裕が要ると判断したためである。

`L`指令が途絶した場合のフェイルセーフ値

前進上限・旋回上限・後退上限のいずれも、上位からのL/R指令がLIMIT_TIMEOUT_MS(1000ms)を超えて途絶すると、無制限には戻さずLIMIT_FAILSAFE_MM_S(120mm/s)という固定の低速上限へ自律的にフォールバックする実装になっている。上位コンピュータの障害物監視が死んだ可能性を考慮しつつ、手動操作でロボットを退避させられる余地は残す、という折衷点である。

検討中の最終防衛ライン: 物理バンパー

上記のソフトウェア層をすべて信頼しても、衝突を完全にゼロにはできないという前提のもと、Pi5・ROS2・LiDARのいずれの状態にも依存しない独立層として、物理的なバンパースイッチの追加を構想している。設計案では、ノーマルクローズ(NC)接点のマイクロスイッチをESP32のGPIO割り込みに直結し、プルアップとハードウェアデバウンス(RC回路)を組み合わせる。NC接点を選ぶのは、配線が断線した場合にも「押された」側に倒れる(フェイルセーフになる)ようにするためである。ファーム側は割り込みでフラグをラッチし、以後は前進を0にクランプしたまま後退のクリープだけを許可する。解除条件は「後退して物理的に離れる」か明示コマンドのいずれかで、Pi・ROS2・LiDARの生存に一切依存しない点が、既存の「手動操作経路は上位コンピュータに依存しない」という設計思想と一致する。

参考リンク

#ESP32