はじめに
タミヤの TT-02 SRX シャーシを使った 1/10 スケールの RC カーを自律走行させています。基本の構成は、FaBo の JetRacer 用基板を Jetson Orin Nano につないで作りました。
写真の左が前です。先端の黒い円筒が LiDAR、その後ろの小さな基板が今回のセンサハブ、中央が Jetson Orin Nano、右の塔にカメラが 2 台付いています。
LiDAR SLAM(走りながら地図を作り、同時にその地図の中での自分の位置を求める手法)で作った地図の上で、AMCL(Adaptive Monte Carlo Localization。自分の位置の候補をたくさんの粒子で表し、LiDAR のスキャンが地図とよく合う粒子を残していくパーティクルフィルタ)が自己位置を推定し、Pure Pursuit(経路上の少し先の点を目標に、そこへ向かう円弧の舵角を決める追従法)で経路をなぞります。制御は 10 Hz で、この 1 周期(0.1 秒)を以下では tick と呼びます。
自己位置推定に使うセンサは 3 つです。
- IMU: ヨーレート(gyro_z)
- 車軸オドメトリ(反射型フォトリフレクタ): 前進速度
- 2D LiDAR(LDROBOT STL-27L): スキャンマッチング(続けて取った 2 枚のスキャンを重ね合わせて、その間の移動量を求める処理。この車では PL-ICP = Point-to-Line ICP という手法を使う)
この 4 か月で、IMU は 3 機種・のべ 6 構成を使いました。センサのつなぎ方も何度か変わり、最終的に 3 つとも RP2040-Zero 1 枚の「センサハブ基板」 にまとめて、USB 1 本で Jetson につなぐ形に落ち着きました。
この記事で書くことは次のとおりです。
- 歴代 IMU の比較と、次に選ぶならどれか(1〜3 章)
- RP2040 に集約した理由と、そのメリット・デメリット(4 章)
- センサハブのハードウェア・ファームウェア・性能(4〜6 章)
- 途中で踏んだ不具合と教訓(7 章)
数値はすべて自分の 1 台での実測です。「未測定」と書いたものは比べていません。IMU は交換のたびに個体・取付位置・温度が変わっているので、比較には限界があります(2.5 節)。
1. この車での IMU の使い方
この車は IMU をセンサフュージョンに使っていません。使うのは gyro_z(ヨーレート)だけで、AMCL の予測(運動モデル)に入れます。絶対的な向きは、LiDAR と地図の照合で決めます。加速度は記録だけしておき、将来、学習モデルの入力に使う予定です。
このため、IMU に求める性能は次の順になります。
- gyro_z のバイアスが安定していること(静止時のオフセットが走行中に動かないこと)
- 振動に強いこと(ブラシモーター、カーペット上の走行)
- ノイズが小さいこと(tick ごとに平均するので、優先度は低い)
必要なレンジも大きくありません。自宅コースでは車速 0.7 m/s・最小回転半径 0.31 m で、ヨーレートの最大は約 130 dps(degrees per second、度/秒)です。
2. 歴代 IMU の比較
2.1 一覧
| # | IMU | 接続 | 取り込み | gyro_z バイアス |
|---|---|---|---|---|
| 1 | BNO055 | I2C | フュージョン出力 | 未測定 |
| 2 | ICM-20948 | I2C | 100 Hz | (記録なし) |
| 3 | ICM-42688-P | I2C | 100 Hz | −0.843 dps |
| 4 | ICM-20948 | I2C | 100 Hz | +0.590 dps |
| 5 | ICM-42688-P | I2C | 100 Hz | −0.924 dps |
| 6 | ICM-42688-P | SPI(RP2040) | 7,930 Hz | −0.905 dps |
それぞれの期間・設定と、交換した理由です。
-
BNO055(5〜6 月)
- チップ内蔵の 9 軸フュージョン(NDOF モード)の出力を使用
- 交換理由: 車体では 6 面較正ができず、加速度の残差で自己位置が飛んだ(2.2 節)
-
ICM-20948(6〜8 月)
- ±500 dps。バイアス補正後の静止時の残差は +0.046 dps
- 交換理由: 加速度を学習入力にする計画のため、6 軸の ICM-42688-P に決めた(2.3 節)
-
ICM-42688-P・I2C 1 個目(8 月下旬)
- ±500 dps / ±4 g、ODR(Output Data Rate。センサが測定値を出す頻度)1 kHz
- バイアス −0.843 dps(std 0.068、n=281、25 ℃)
- 交換理由: LiDAR の取付を変える工事の都合で、一時的に ICM-20948 に戻した
-
ICM-20948・2 個目(9 月上旬)
- バイアス +0.590 dps(std 0.148、起動直後、29 ℃)
- 交換理由: 取付が確定したので ICM-42688-P に戻した
-
ICM-42688-P・I2C 2 個目(9 月上旬〜中旬)
- バイアス −0.924 dps(std 0.044、n=278、24 ℃)。付け直したら −0.956 dps
- 交換理由: 8 kHz を全部取り込み、USB を 1 本にまとめるため、SPI に移した
-
ICM-42688-P・SPI(センサハブ)(9 月中旬〜現在)
- ±1000 dps / ±8 g、ODR 8 kHz、全サンプルを取り込み
- バイアス −0.905 dps(静止 6 分・286 万点、1 秒平均の std 0.003)
2.2 BNO055: フュージョン内蔵チップは車に載せると扱いにくい
BNO055 は、チップの中で 9 軸を融合して、姿勢(クォータニオン)を直接出してくれます。最初はこれを使っていましたが、次の 2 つの問題が出ました。
- 較正ができない: 加速度の較正には「6 つの面をそれぞれ下にして静止させる」手順が要ります。RC カーの車体は裏返して固定できず、磁気の較正しか完了しませんでした
- 中が見えない: 加速度の較正残差(約 0.5 m/s²)が重力の漏れとして「線形加速度」に乗り、運動モデルがそれを速度として積分しました。偽の速度が上限に張り付き、推定位置が 55 m 先まで飛びました
そこで設計の方針を変えました。**IMU は生のヨーレートだけを使う。融合は自分でやる。絶対方位は LiDAR に任せる。**この使い方では、フュージョン内蔵チップの利点は消え、中身が見えないという欠点だけが残ります。
2.3 ICM-20948 から ICM-42688-P へ
どちらも TDK InvenSense のチップです(ICM-20948 は磁気センサ付きの 9 軸、ICM-42688-P は 6 軸)。ICM-42688-P に切り替えた理由は、性能ではなく次の 2 つです。
- 加速度を将来学習モデルの入力にするなら、データを集めるときと推論するときでセンサを一致させる必要がある。長く使えるチップに早めに決めておきたかった
- 別の開発機で、ICM-42688-P を SPI で 8 kHz 読み出した実績があった
2.4 実測からわかったこと
(a) バイアスは「チップごと、取付ごと」の値
ICM-42688-P の 3 個体(I2C の 2 個 + SPI 基板の 1 個)の静止バイアスは −0.84〜−0.96 dps、ICM-20948 は +0.59 dps でした。同じチップを付け直しただけでも 0.03 dps 変わります。
この値は設定ファイルに定数として持っており、取付を変えたら必ず測り直す運用にしています。
(b) 取付の向きで符号が反転する
基板を裏返して付けると、gyro_z の符号が反転します。そこで取付のたびに、手で車を 90° 左に回し、積分値が +90° 付近(実測で倍率 1.01)になることを確かめています。
なお、z 軸回りに 180° 回して付けただけなら、gyro_z の符号は変わりません(加速度の x・y は反転します)。
(c) レンジを狭めたら、ノイズが「増えた」
ICM-42688-P は、最初はチップの既定値 ±2000 dps で使っていました。静止時の gyro_z は、隣り合う 2 つの値(0.061 dps 刻み)の間を行き来するだけでした。これはノイズではなく、量子化が見えていたということです。
±500 dps に狭めると、標準偏差は 0.048 → 0.068 dps に「悪化」しました。実際には、量子化で隠れていた本当のノイズが見えるようになっただけです。レンジが違う構成どうしで、標準偏差を比べてはいけません。
(d) I2C では 8 kHz を取れない
| バス | 8 kHz での使用率 | 実用上の最大 ODR |
|---|---|---|
| I2C 400 kHz | 235 %(不可) | 2 kHz |
| I2C 1 MHz | 約 95 % | 4 kHz |
| SPI 4 MHz | 約 21 % | 8 kHz |
| SPI 8 MHz | 約 10 % | 8 kHz |
さらに、Jetson の Python から smbus2 で読む方法では、100 Hz が現実的な上限でした。8 kHz を全部取るために、SPI + MCU(RP2040)に移しました(4 章以降)。
(e) 走行中のジャイロは、LiDAR の推定とよく一致する
SPI 化した後の自律走行(400 tick = 約 40 秒)で、バイアス補正後の gyro_z を積分すると 2,334° でした。同じ区間で AMCL が推定した向きの変化は 2,325° です。差は 0.4 %、tick ごとの相関は 0.914 でした。
(f) 温度の影響は、まだ測れていない
データとして残っているのは、25.1 → 26.4 ℃ でバイアスが 0.006 dps 動いた 1 点だけです。起動から数十分にわたる温度ドリフトは、どの個体でも測っていません。
2.5 公平に比べられていない点
- 個体が毎回違う。バイアスには、個体差・取付・温度がすべて混ざっている
- レンジが揃っていない(±500 dps と ±1000 dps)ので、標準偏差を直接比べられない
- 測定の条件が揃っていない
- サンプル数は 278 点〜286 万点
- 温度は 24〜29 ℃、起動直後と起動 15 分後が混在
- 1 サンプルの標準偏差と、1 秒平均の標準偏差が混在
- 自己位置推定の精度(AMCL の補正量)は、IMU を替えたときに地図も作り直していたので、IMU だけの効果とは言えない
- BNO055 は生ジャイロを使っていなかったので、同じ指標がない
3. 次に選ぶならどの IMU か
3.1 選び方の考え方
この車では、IMU は gyro_z を AMCL の予測に使うだけです。向きの誤差は、LiDAR と地図の照合で毎 tick 補正されます。
仮にバイアスが 0.05 dps ずれていても、1 分で 3° です。これは AMCL が十分に補正できる量です。実際の走行でも、ジャイロの積分と AMCL の向きは 0.4 % で一致しています(2.4 節 (e))。
つまり、いまのボトルネックはチップの性能ではなく、較正の運用です。
- バイアスを定数として持っていて、取付のたびに手で測り直している
- 温度ドリフトを測っていない
- 取付位置の実測が残っている(加速度を使う前に必要)
3.2 当面は ICM-42688-P を使い続ける
- SPI での 8 kHz 読み出しが実機で動いていて、欠落 0 を確認済み
- 加速度を学習入力に使うなら、集めたデータと同じセンサであること自体に価値がある。チップを替えると、それまでに集めたデータとの一貫性が崩れる
- 次にやることはチップの交換ではなく、次の 2 つ
- 走行前の静止区間から、バイアスを自動で推定する
- 温度を記録してドリフトを測る(8 kHz の生ログには温度も入っている)
3.3 次の候補: ICM-45686
乗り換えるなら、同じ TDK InvenSense の ICM-45686 を候補にします。まだ自分で測っていないので、この記事では性能の数値は挙げません。
判断の材料は、すでに手元にある 8 kHz の生ログです。モーターの回転に同期した振動がバイアスに乗っているとわかったら、ICM-45686 を実測して比べます。
センサハブにしてあるので、乗り換えるときはチップを差し替えて、ファームウェアの IMU 部分を書き換えるだけです。USB のプロトコルと Jetson 側は変えずに済みます(4.2 節)。
3.4 避けたほうがよいもの
フュージョン内蔵のチップ(BNO055 など)です。2.2 節のとおり、車体では較正の手順を実行できず、中で何が起きているかも見えません。
4. センサハブ基板
ここからは、IMU を 8 kHz で全部取るために作った基板の話です。IMU だけでなく、オドメトリと LiDAR も同じ基板にまとめました。
使った部品と値段(実際に買って使っているもの。値段は 2026 年 9 月 23 日に各ページで表示された税込価格)
| 部品 | 買った先 | 値段 |
|---|---|---|
| RP2040-Zero 互換ボード | Amazon | 847 円 |
| ICM-42688 6 軸 IMU モジュール | AliExpress | 3,513 円 + 送料 527 円 |
| GROVE 赤外線反射センサ v1.2(RPR220) | スイッチサイエンス | 1,009 円 |
| LDROBOT D800 LiDAR キット(STL-27L) | AliExpress | 18,260 円(送料無料) |
- RP2040-Zero は Waveshare 純正ではなく互換ボードです。ピン配置は同じで、ファームウェアも Waveshare 用のボード定義でそのまま動いています
- LiDAR はセールのときに買うと、2,000 円くらい安くなります
- 合計は 2 万 4 千円ほどです(紙のリング、配線材、ユニバーサル基板は除く)
4.1 なぜ集約したか
旧構成では、3 つのセンサが別々の経路で Jetson につながっていました。
- IMU: Jetson の I2C に直結し、Python で 100 Hz のポーリング。時刻は Jetson が読んだ時刻
- オドメトリ: RP2040 + MicroPython の基板から、USB で ASCII の行を送る。GC(ガベージコレクション。使い終わったメモリを自動で回収する処理で、その間はプログラムが止まる)で止まる可能性があり、7.1 節の不具合も起きた
- LiDAR: CP2102 の USB-UART 変換基板。時刻は Jetson が読んだ時刻で、CP2102 のレイテンシタイマ(数〜十数 ms)とスケジューラの揺らぎが乗る
3 m/s で走ると、10 ms の揺らぎは 3 cm の位置ずれになります。一定のずれなら較正で消せますが、揺らぎ(ジッタ)は較正できません。3 つのセンサを 1 つの MCU の時計で打刻すれば、この揺らぎをまとめてなくせます。
4.2 RP2040 に集約するメリットとデメリット
センサを Jetson の I2C・SPI・GPIO に直接つなぐ方法もあります。USB を経由しないので、遅延だけを見れば直結が最短です。それでも MCU に集約して USB でつないだ理由は、次のとおりです。
メリット
-
制御装置を替えても、USB 1 本で持ち込める
- 直結すると、配線もソフトも計算機に縛られます。ピンヘッダの配置、I2C のバス番号(Orin Nano では bus 7)、ピン多重化の設定、電圧レベルは計算機ごとに違います。Jetson の世代を変えたり、PC や別のボードに載せ替えたりすると、配線とドライバをやり直すことになります
- USB CDC(Communications Device Class。USB 機器をシリアルポートとして見せる標準の規格で、Linux ではドライバを入れなくても
/dev/ttyACM*として使える)なら、USB と Python が動く計算機ならどれでも同じように動きます。ポートも/dev/serial/by-id/usb-JetRacer_SensorHub_...という同じ名前で見つかります - 実際に、同じ基板・同じドライバを、開発用の x86 PC と Jetson の両方で、変更なしに動かしています。基板の検証と記録の通し試験は、まず PC で済ませてから車に載せました
- 時刻が MCU の時計で揃う: IMU・オドメトリ・LiDAR を同じ時計で打刻するので、ホスト OS の負荷やスケジューリングの揺らぎが時刻に乗りません
- 取り込みがホストに左右されない: 8 kHz の FIFO(First In First Out。先に入れたものから順に取り出すバッファ。IMU チップの中にあり、読み出されるまでサンプルを貯めておける)の読み出しやエッジの割り込みは MCU が受け持ちます。ホスト側の Python が一瞬止まってもデータは欠けず、欠けたかどうかも後からサンプル番号で確かめられます
- 配線が 1 本になる: 振動する車体で、抜けやすいコネクタと変換基板が減ります
デメリット
- USB の往復の分だけ遅れる: 往復は 0.23〜0.41 ms でした。この車の制御(10 Hz)には無視できる大きさですが、kHz 級の制御ループを回すなら直結が有利です
- 単一障害点になる: MCU が止まると全センサが止まります。ウォッチドッグと LED で対策しています(4.7 節)
- ファームウェアという保守対象が増える: 7.4 節の偽ヘッダのような不具合は、この構成だから生まれたものです
- USB 特有の問題がある: 列挙直後の ModemManager(7.5 節)や、PC で起きた USB の切断(6.2 節)です
- 状態がハブに残る: LiDAR の回転数のように、ハブ経由で設定した状態が次の実行に持ち越されます(7.6 節)
4.3 構成
MCU は RP2040-Zero の互換ボード(Cortex-M0+ × 2、264 KB SRAM、USB 1.1 Full Speed)です。小さくて USB-C が付いていて、安いので選びました。
ピンの配線は次のとおりです。電源は、IMU とオドメトリのセンサが 3V3、LiDAR が 5V(USB の VBUS)です。GND はすべて共通にしています。
実物はこうなっています。ユニバーサル基板の上に RP2040-Zero を載せ、電源(5V・3V3・GND)は基板上でバスにして分岐しています。下の紫の基板が IMU で、車体にねじで固定しています。
4.4 IMU: ICM-42688-P(SPI)
- 接続: SPI0(SCK GP2 / MOSI GP3 / MISO GP4 / CS GP5 / INT1 GP6)、mode 3
- SPI クロック: レジスタ設定時 1 MHz、FIFO のバースト読み出し時 8 MHz
- ODR: gyro・accel とも 8 kHz(実測 7,930 Hz、公称の −0.87 %)
- レンジ: ±1000 dps / ±8 g
- フィルタ: アンチエイリアスフィルタ約 2.1 kHz、UI フィルタ ODR/4
- FIFO: stream モード。accel + gyro + 温度 + タイムスタンプで 16 バイト/サンプル
±1000 dps での 1 LSB は約 0.03 dps です。I2C 時代の ±500 dps より粗くなりますが、8 kHz で取ると 1 秒平均の標準偏差は 0.003 dps で、1 LSB より 1 桁小さくなります。
4.5 オドメトリ: Grove RPR-220(反射型フォトリフレクタ)
推進軸に、白黒 4 対の縞を印刷した紙のリングを巻き、反射センサで読みます。
- 立ち上がり・立ち下がりの両方を数えるので 8 エッジ/回転
- GP7 を内部プルアップにして、両エッジで割り込み
- デバウンスは 300 µs
TT-02 に取り付けるときは、センサ基板の Grove コネクタが邪魔になったので、ニッパーで切り落とし、基板に線を直接はんだ付けして延ばしました。
なお、このセンサの電源電圧の仕様は 3.5〜5.5 V ですが、3.3 V で使っています。5 V で動かすと出力も 5 V になり、RP2040 の入力に直結できないためです。3.3 V でも、ジャッキ上の全開で 900 エッジ/s まで取りこぼさずに数えられています。ただし仕様の範囲外なので、真似するときは自分の個体で確かめてください。
マーカーリングのシート
白黒の帯は、普通紙にレーザープリンタで印刷しています。トナーの黒は赤外線をよく吸うので、反射センサでもはっきり読めます。
シートの使い方です。
- 実寸(100 %)で印刷する。「ページに合わせる」は使わず、上部の 50 mm のバーを定規で測って確かめます
- 紙の帯をシャフトに巻いて、重なったところに印を付け、外周の長さを測ります。シートには外周 31.4〜32.6 mm(軸径 約 10 mm)の 4 通りと、4 対・5 対の組み合わせが並んでいるので、いちばん近い行を選びます
- 灰色の枠線で切ります。右端の点線から先は印刷のないのりしろで、帯の始まりの下に差し込みます(縞どうしを重ねない)
- 縞が軸の周方向に並ぶように巻きます。センサは帯の中央に向け、面から 4〜15 mm(推奨 6 mm)離します
- 白黒の差は、目ではなくセンサの出力で確かめます(赤外線なので)。手で回して、センサの LED が縞ごとに点滅すれば使えます
外周に合わない帯を選ぶと、継ぎ目の縞だけ幅がずれて、1 回転に 1 回の周期的な誤差になります。外周を測ってから選ぶのはこのためです。
較正は、車を床で 2.000 m 押して、増えたエッジ数を数えます。2 回測って 216 / 208 エッジ(平均 212 = 26.5 回転)でした。
- 1 回転あたり 0.0755 m(1 エッジ = 9.4 mm)
- タイヤ径 64 mm から逆算すると、推進軸 : 後輪 ≈ 2.66 : 1
ジャッキに載せて全開にしても、900 エッジ/s まで取りこぼさずに数えられました。
センサは 1 チャンネルなので、回転の向きはわかりません。自律走行では後退しないので、実害はありません。
4.6 LiDAR: LDROBOT STL-27L
- 受信: UART0 RX = GP1、921600 bps 8N1
- パケット: 47 バイト(12 点)、ヘッダ
0x54 0x2C、CRC-8(Cyclic Redundancy Check。データから計算した検査値を付けておき、受け取った側で計算し直して一致しなければ、途中でデータが壊れたとわかる) - 帯域: 10 Hz のとき約 85 KB/s(約 1,800 パケット/s)
- 回転数の制御: GP8 から PWM(5.4 節)
- 電源: USB の VBUS から 5 V。起動時約 540 mA、動作時約 290 mA
- I/O は 3.3 V 系なので、RP2040 に直結できる
4.7 状態表示と保護
- WS2812(GP16): 青 = USB 未接続 / 赤 = IMU 異常 / 黄 = 直近 0.5 秒に欠落 / 緑 = 正常
- ウォッチドッグ 2 秒
センサを 1 つの MCU に集めると、そこが単一障害点になります。LED の色で状態がすぐわかるようにして、ハングしたらウォッチドッグで再起動させます。CP2102 の変換基板は捨てずに保管してあり、LiDAR だけ元の経路に戻すこともできます。
5. ファームウェア(Pico C SDK)
5.1 コアの分担
MicroPython は GC で止まる可能性があります。8 kHz の IMU と LiDAR の透過を同時にこなすため、Pico C SDK + TinyUSB(マイコン向けのオープンソースの USB スタック。Pico SDK に同梱されている)で書き直しました。
- core1: 1 ms ごとに IMU の FIFO の残量を読み、溜まった分を SPI 8 MHz でまとめて読む(割り込みではなくポーリング)
- core0: USB(CDC × 2)、フレーミング、オドメトリの割り込み、LiDAR の受信と透過、コマンド処理、LED、ウォッチドッグ
IMU のデータは core 間のキュー(queue_t、深さ 32)で core0 に渡し、1 ms ごとに 8 サンプル前後を 1 パケットにします。LiDAR は UART の受信割り込みで 8 KB のリングバッファに入れ、CDC1 にそのまま流します。
5.2 USB は CDC × 2 の複合デバイス
.../usb-JetRacer_SensorHub_<serial>-if00
CDC0: 独自プロトコル(COBS + CRC16)
.../usb-JetRacer_SensorHub_<serial>-if02
CDC1: LiDAR の生バイト透過
ポイントは、LiDAR を生バイトのまま別の CDC で透過させることです。Jetson 側の既存の LiDAR パーサ(CRC-8 の検証を含む)は 1 行も変えずに、ポート名を変えるだけで動きます。CDC の「ボーレート」は仮想の値なので、921600 を指定したままでも問題ありません。
5.3 ワイヤ形式(CDC0)
COBS( payload ‖ crc16_le(payload) ) ‖ 0x00
COBS(Consistent Overhead Byte Stuffing。データの中の 0x00 を別の値に置き換える符号化で、0x00 をフレームの区切りとして使えるようになる。増えるのは 254 バイトごとに 1 バイト程度)でパケットを包み、末尾に 0x00 を付けて区切ります。受け取る側は 0x00 まで読めば 1 つのパケットになるので、途中から読み始めても次の 0x00 で同期し直せます。
CRC は CRC-16/CCITT-FALSE です(Python の binascii.crc_hqx(payload, 0xFFFF) と同じ値)。
MCU → Jetson
-
0x11 IMU_BATCH(約 1 kHz): 通し番号、読み出し時刻、先頭サンプル番号、サンプル数、flags、温度、[ax ay az gx gy gz] × n(int16) -
0x02 ODOM(50 Hz): 累積エッジ数、最後のエッジの時刻と周期、送信時刻 -
0x04 LIDAR_MARK(約 56 Hz): CDC1 の何バイト目のパケットが MCU 時刻のいつ届いたか。そのパケットの開始角・タイムスタンプ・CRC も付ける -
0x05 STATUS(1 Hz): ファームウェアの版、IMU の状態、欠落カウンタ、LiDAR の回転速度と PWM duty
Jetson → MCU
-
0x03 SYNC: MCU が受信時刻を付けて返す(時刻合わせ用) -
0x21 LIDAR_PWM: duty 15〜85 % -
0x7F BOOTSEL(BOOTSEL = RP2040 の書き込みモード。基板の BOOT ボタンを押しながら電源を入れたときと同じ状態): 合言葉が一致したら、UF2(USB Flashing Format。RP2040 が USB メモリとして見え、ファームウェアのファイルをコピーするだけで書き込める形式)の書き込みモードで再起動
IMU_BATCH の flags(FIFO のオーバーフロー、バッチの欠落など)とサンプル番号の連続性から、欠落を後で数えられるようにしています。
LIDAR_MARK は 32 パケットごとに送ります。到着時刻は、UART の割り込みに入った時刻から「FIFO に残っていたバイト数 × 10.85 µs」を引いて推定します。
5.4 LiDAR の回転数制御は「1 kHz の PWM」
STL-27L のデータシートには、「20〜50 kHz の PWM で、duty 45〜55 % を 100 ms 以上出すと外部制御モードに入る」とあります。しかし手元の個体は、この方式では反応しませんでした。
そこで、LiDAR を買った youyeetoo(4 章の部品表)のフォーラムで担当者が回答していた方式を試しました。同じ LDROBOT 系列の LD19 についての回答で(2024 年 11 月)、PWM の周波数でモードを選びます(外部制御は 0.5〜1.5 kHz)。これで動きました。1 kHz・duty 50 % を 1〜2 秒出すと外部制御モードに入り、その後は duty と回転数がほぼ比例します。
| duty(1 kHz) | 回転数 |
|---|---|
| 15 % | 4.0 Hz |
| 30 % | 6.5 Hz |
| 40 % | 8.1 Hz |
| 50 % | 9.74 Hz |
| 60 % | 11.4 Hz |
| 70 % | 13.0 Hz |
| 75 % | 13.9 Hz |
| 85 % | 15.3 Hz |
注意点があります。一度外部制御モードに入ると、PWM を Low にしても 10 Hz に戻らず、回転数が不定になります。戻すには LiDAR の電源を入れ直すしかなく、MCU をリセットしても LiDAR の状態は残ります。
そこでファームウェアは、起動した瞬間から 1 kHz・duty 50 % を出し続け、Low にはしないようにしました。duty 50 % なら、どちらのモードでも約 10 Hz になります。
5.5 書き込み
書き込みモード(BOOTSEL)には、ボタンのほか、CDC0 を 1200 bps で開く(Arduino と同じ流儀)か、0x7F BOOTSEL コマンドでも入れます。車体に載せたまま書き換えられます。
6. Jetson 側と性能
6.1 ドライバ
- 読み出し用のスレッド 1 本が、COBS のフレームを切り分け、CRC を確かめ、パケットの種類ごとに振り分けます
- IMU は
numpy.frombuffer(body, dtype="<i2").reshape(n, 6)で一気にデコードします。8 kHz のサンプルを 1 つずつstruct.unpackしていると追いつきません - MCU 時刻から Jetson 時刻への変換: USB の遅延は必ず正の値です。そこで「受信時刻 − MCU 時刻」の直近 100 サンプルの最小値を、2 つの時計のずれとみなします。SYNC の往復時間は、中央値で 0.23〜0.28 ms(PC)、0.41 ms(Jetson)でした
- 記録モードでは、8 kHz の IMU を間引かずに全部残します(約 7 MB/分)。走行後の検定スクリプトは、サンプル番号の欠けや MARK の間隔の異常を見つけたら、終了コード 1 で落ちます
6.2 旧構成との比較
| 項目 | 旧構成 | センサハブ |
|---|---|---|
| 接続 | USB 2 本 + I2C | USB 1 本 |
| IMU の取り込み | 100 Hz | 7,930 Hz |
| 時刻 | Jetson の読み出し時刻 | MCU 時刻 |
| IMU の欠落 | 未測定 | 10 分間で 0 |
| LiDAR の CRC 不良 | 未測定 | 0 |
| CPU 負荷 | 未測定 | 未測定 |
USB の帯域は、見積りで約 210 KB/s です。USB 1.1 Full Speed の実効(約 1.2 MB/s)の 1/6 程度なので、余裕があります。
Jetson に載せての 10 分間の通し試験(カメラ + LiDAR + IMU + オドメトリを同時に記録)の結果です。
- IMU: 615,481 バッチ / 4,880,340 サンプル、欠落 0
- LIDAR_MARK: 34,616 個。隣り合う MARK の間隔はすべて 1,504 バイト(= 47 バイト × 32 パケット)
- オドメトリの欠け 0、USB の切断 0
ただし、x86 の PC に直結したベンチでは、USB の切断が 3 回起きました。Jetson では再現しておらず、原因はまだわかっていません。
7. 踏んだ不具合と教訓
7.1 9 分止めると、その後 9 分間オドメトリが数えなくなる
MicroPython 版のファームウェアでの不具合です。
デバウンスを ticks_diff(t, last) < 300 と書いていました。MicroPython の ticks_diff は符号付きで、およそ ±8.95 分の範囲しか表せません。最後のエッジから 9 分以上たつと、差が負になって < 300 が真になり、エッジが「チャタリング」として捨てられます。
捨てたエッジでは last を更新しないので、そこから 9 分間、すべてのエッジが捨てられ続けます。
短い試験では一度も起きません。車を運んで 9 分以上置いてから走らせたときに初めて起きました。
- 修正:
0 <= d < 300の形にする - C 版: 64 ビットの
time_us_64()の差を符号なしで比べるので、このラップは起きない
7.2 壊れたセンサと止まっている車は、オドメトリだけでは区別できない
7.1 の不具合が起きたとき、融合のロジックは「速度 0」を信じました。そして、正しく前進を測っていたスキャンマッチングの結果を、外れ値として 61 回捨てました。
そこで、次の 3 つが 10 tick 続いたら、オドメトリを無効にするラッチを入れました。
- 前進を指令している
- エッジ数が増えていない
- スキャンマッチングが 0.15 m/s 以上の前進を測っている
同じ走行データを再生すると、スキャンマッチングの棄却は 61 回 → 0 回になりました。ポイントは、独立した 2 つ目の証人を要求することです。
7.3 周期から速度を出すと、停止際のバックラッシュでスパイクする
最初は「最後のエッジ間隔の逆数」で速度を出していました。停止直前にギアの遊びで軸が縞の境目を行き来すると、短い間隔で 2 回エッジが出て、1 tick だけ 1.14 m/s という値が出ました。同じ時刻のスキャンマッチングは 0.02 m/s でした。
そこで、tick の間に増えたエッジ数 ÷ MCU 時刻での経過時間で速度を出すように変えました。偽のエッジが混じっても、1 エッジ分しか狂いません。その代わり、分解能は 10 Hz で 0.094 m/s 刻みになります。
7.4 LiDAR の偽ヘッダ
パケットを数えるとき、ヘッダ 0x54 0x2C を毎回探していました。点群データの中にも同じ 2 バイトが偶然現れることがあり(約 1,500 パケットに 1 回)、それをパケットの始まりと誤認していました。
- 修正: 同期が取れている間はヘッダを探さない(ファームウェア 0.1.3)
- 見つけたきっかけは、MARK の生ログを全部残して、間隔を並べたことです。平均などの集計値だけを見ていたら、気づかなかったと思います
7.5 USB の列挙直後 10〜20 秒は、ModemManager にポートを取られる
Ubuntu では、USB CDC のデバイスが現れると、ModemManager がモデムかどうかを調べるために AT コマンドを送ります。その間はコマンドが壊れ、CDC1 のバイト数にもずれ(17 / 29 バイト)が出ました。
- 運用: 書き込み直後は 25 秒待つ
- 設計: MARK にパケットの中身も載せて、ホストが中身で突き合わせるようにした
udev ルールで ENV{ID_MM_DEVICE_IGNORE}="1" を設定して、ModemManager に無視させる方法もあります。
7.6 LiDAR の回転数は「機器に残る状態」だった
13 Hz を試した走行の後、duty を戻していませんでした。そのため、その後の 10 Hz 前提の走行が、すべて 13 Hz のスキャンで動いていました。5.4 節のとおり、LiDAR は設定を保持します。
スキャンマッチングは、移動量を tick の周期(0.1 秒)で割って速度を出していました。実際のスキャン間隔は 1/13 秒なので、速度を 10/13 ≈ 0.77 倍に過小評価しました。その結果、自己位置が 0.3〜0.5 m 遅れ、壁に接触しました。
エラーは一切出ていませんでした。手がかりは次の 2 つだけです。
- 1 スキャンの点数が減っていた(約 2,200 点 → 約 1,640 点)
- スキャンマッチングとオドメトリの速度の比が 0.77 だった
対策として、次の 4 つを入れました。
- 走行の開始時に必ず duty を送る
- STATUS の duty と実測の回転数が一致するまで、走行を始めない
- 終了時に 50 % へ戻す
- 走行後の検定で、点数から逆算した回転数と tick の周期を突き合わせる
教訓は、外部の機器が覚えている設定は、毎回明示的に設定して、読み返して確かめることです。
8. まとめ
- 3 機種・のべ 6 構成の IMU を使った。生のヨーレートを AMCL の予測に使う用途では、ICM-42688-P で今のところ不足はない
- IMU のバイアスはチップごと・取付ごとの値。レンジを変えると、標準偏差の意味も変わる
- フュージョン内蔵チップ(BNO055)は、車体では較正できず、中身も見えない
- 次にやるのはチップの交換ではなく、起動時のバイアス自動推定と温度ドリフトの測定。次の候補は ICM-45686(未評価)
- 8 kHz を取りこぼさずに取るため、RP2040 で IMU・オドメトリ・LiDAR を USB 1 本に集約した。Jetson で 10 分間、488 万サンプルを欠落 0 で取り込めた
- 遅延だけなら Jetson への直結が最短。それでも USB に集約したのは、制御装置を替えても USB 1 本で持ち込めるから。実際に、同じ基板を PC と Jetson で変更なしに使っている
参考
- ICM-42688-P / ICM-20948 データシート(TDK InvenSense)
- BNO055 データシート(Bosch Sensortec)
- LDROBOT STL-27L データシート / 開発マニュアル
- Raspberry Pi Pico C/C++ SDK、TinyUSB
付録: AI に同じ構成を作らせるためのプロンプト
この記事の構成(RP2040 センサハブのファームウェアと、ホスト側の Python ドライバ)を、コーディング AI(Claude Opus など)に再現させるためのプロンプトです。プロトコルはバイト単位で指定してあるので、この記事のホスト側とそのまま通信できるものができるはずです。
自分の環境に合わせて変えてよいのは、ピン番号・USB の名前・ホストの計算機です。「受け入れ試験」の数値は、この記事の実機での実測値です。
# 依頼
RC カー(1/10、Jetson などの Linux 計算機を搭載)の自己位置推定用に、
IMU・車軸オドメトリ・2D LiDAR を 1 枚の RP2040-Zero に集約し、
USB 1 本(CDC × 2 の複合デバイス)でホストに渡す「センサハブ」を作ってほしい。
成果物は 2 つ:
(1) RP2040 のファームウェア(Pico C SDK 2.x + TinyUSB、C 言語、CMake)
(2) ホスト側の Python ドライバ(pyserial + numpy)と、ベンチ確認用の CLI
以下の仕様はすべて必須。書かれていないことは、質問するか、理由を書いて決めること。
# ハードウェア
- MCU: RP2040-Zero(Waveshare 純正または互換ボード。ビルドは PICO_BOARD waveshare_rp2040_zero)
- IMU: TDK ICM-42688-P を SPI0 で接続
SCK=GP2, MOSI=GP3, MISO=GP4, CS=GP5(GPIO で手動制御),
INT1=GP6(入力に設定するだけで未使用)
- オドメトリ: 反射型フォトリフレクタ
(Grove IR Reflective = RPR-220、デジタル出力)を GP7 に接続。
推進軸に白黒 4 対の縞の紙リングを巻き、両エッジを数える = 8 エッジ/回転
- LiDAR: LDROBOT STL-27L
Tx → GP1(UART0 RX)、921600 bps 8N1、フロー制御なし
PWM 入力 ← GP8(回転数制御)
電源は USB の VBUS(5 V)から。I/O は 3.3 V 系なので直結
- 自己診断用の矩形波出力: GP10(ジャンパで GP7 に入れて試験する)
- 状態表示: WS2812(GP16、PIO で駆動)
# ファームウェアの構成
core1 = IMU 専用。1 ms 周期(busy_wait_until で絶対時刻に合わせる)でポーリング:
1. INT_STATUS(0x2D)から 3 バイトをバースト読みする
(INT_STATUS, FIFO_COUNTH, FIFO_COUNTL。カウントはビッグエンディアン)
2. FIFO_COUNT を 16 の倍数に切り下げ(上限 2080 バイト)、
FIFO_DATA(0x30)からまとめて読む
3. 16 バイトのパケットごとに:
- ヘッダの bit7 が 1 なら FIFO が空なので終了
- (hdr & 0x60) != 0x60 なら解析異常フラグを立てて終了
- それ以外は、ヘッダの次の 12 バイトから accel xyz, gyro xyz
(各 int16 ビッグエンディアン)、オフセット 13 から温度(int8)を取り出す
4. サンプルを IMU_BATCH(最大 32 サンプル)に詰め、読み出しのたびに
core0 へ queue_t(深さ 32)で渡す。キューが満杯ならそのバッチは捨て、
次のバッチに BATCH_DROPPED を立てる(seq は送れたときだけ進める)
5. INT_STATUS の FIFO_FULL(0x02)が立っていたら、FIFO を flush し
(SIGNAL_PATH_RESET=0x02)、次のバッチに FIFO_OVERFLOW を立てる
core0 = USB(tud_task)、フレーミング、コマンド受信、オドメトリの送信(50 Hz)、
STATUS の送信(1 Hz)、LiDAR の透過、LED、ウォッチドッグ(2 秒)
main の最初に LiDAR の PWM を開始すること(GP8 を浮かせない。理由は後述)
# ICM-42688-P の初期化
SPI mode 3。設定時は 1 MHz、FIFO の読み出し時は 8 MHz。
書き込みは (reg & 0x7F, val)、読み出しは reg | 0x80。書き込みのたびに 10 µs 待つ。
1. BANK_SEL(0x76)=0、DEVICE_CONFIG(0x11)=0x01(ソフトリセット)、2 ms 待つ
2. WHO_AM_I(0x75)が 0x47 でなければ失敗
(IMU_DOWN を立て、1 秒ごとに再試行)
3. アンチエイリアスフィルタ(DELT=39, DELTSQR=1536, BITSHIFT=4 ≒ 2.1 kHz)
Bank1: 0x0C=39, 0x0D=1536&0xFF, 0x0E=(4<<4)|(1536>>8)
Bank2: 0x03=39<<1, 0x04=1536&0xFF, 0x05=(4<<4)|(1536>>8)
4. Bank0:
GYRO_CONFIG0(0x4F)=0x23(±1000 dps, ODR 8 kHz)
ACCEL_CONFIG0(0x50)=0x23(±8 g, ODR 8 kHz)
GYRO_ACCEL_CONFIG0(0x52)=0x11
FIFO_CONFIG1(0x5F)=0x4F、FIFO_CONFIG2(0x60)=16、FIFO_CONFIG3(0x61)=0
FIFO_CONFIG(0x16)=0x40(stream)
5. 0x4F / 0x50 / 0x5F を読み返し、一致しなければ失敗
6. PWR_MGMT0(0x4E)=0x0F(low noise)、45 ms 待つ、FIFO を flush、
1 ms 待つ、SPI を 8 MHz に上げる
注意: 実際の ODR は公称 8 kHz より 0.87 % 遅い(実測 7,930 Hz)。
dt を公称値から作らず、時刻から求めること。
# オドメトリ
- GP7 は内部プルアップの入力。立ち上がり・立ち下がりの両エッジで割り込む
- デバウンスは 300 µs。time_us_64() の差を符号なしで比べ、
(now - t_last) < 300 なら捨てる
(符号付きで比べる実装は、長時間止めた後にすべてのエッジを捨てる
不具合を生むので禁止)
- 保持する値: 累積エッジ数、最後のエッジの時刻、直前 2 エッジの間隔。
読むときは割り込みを止めて、まとめて写す
# LiDAR(STL-27L)の透過
パケットは 47 バイト(多バイトはリトルエンディアン):
0: 0x54 | 1: 0x2C | 2-3: 速度 deg/s | 4-5: 開始角 0.01°
| 6-41: 12 点(距離 u16 mm + 強度 u8)| 42-43: 終了角
| 44-45: タイムスタンプ ms(30000 で折り返す)| 46: CRC-8(多項式 0x4D)
- UART0 の RX FIFO 割り込み(しきい値 1/2)で、受けたバイトを 8 KB の
リングバッファに入れる。あふれたバイトは捨てて drop_bytes に数える
- core0 はリングの中身を CDC1 にそのまま流す
(ホストの既存の LiDAR パーサを変えずに使うため)。
CDC1 が未接続の間もリングは読み捨てる。
ホストが CDC1 を開き直したら、ストリームのオフセットを 0 に戻す
- パケットの同期: パケットの外にいるときだけ 0x54 0x2C をヘッダとして
受け付ける。パケットの中(47 バイトを数え終わるまで)ではヘッダを探さない
(点群データの中にも同じ 2 バイトが約 1/1,500 の確率で現れ、
数え間違いの原因になる)
- 32 パケットごとに LIDAR_MARK を 1 つ作る:
t_us = 割り込みに入った時刻
−(同じ割り込みで 0x54 より後に読んだバイト数 × 10.85 µs)
byte_offset = CDC1 ストリームの中での 0x54 の位置
そのパケットの速度・開始角・タイムスタンプ・CRC も入れる
(ホストが中身で突き合わせるため)。
CRC バイトが届いたら MARK を 16 個のリングに積み、core0 が送る
# LiDAR の回転数制御(重要)
- データシートにある「20〜50 kHz の PWM」の方式は、手元の個体では効かなかった。
1 kHz の PWM を使う
- 1 kHz・duty 50 % を 1〜2 秒出すと外部制御モードに入り、
その後は duty に比例して回転数が変わる:
15 %=4.0 Hz, 30 %=6.5, 40 %=8.1, 50 %=9.74,
60 %=11.4, 70 %=13.0, 75 %=13.9, 85 %=15.3
- 一度外部制御モードに入ると、PWM を Low にしても 10 Hz に戻らず、
回転数が不定になる(LiDAR の電源を入れ直すまで戻らない。MCU を
リセットしても残る)
→ ファームウェアは起動した瞬間から 1 kHz・duty 50 % を出し続け、
Low には絶対にしない
- PWM は wrap=9999(duty の分解能 0.01 %)とし、クロックの分周で 1 kHz を作る。
受け付ける duty は 15〜85 %
# USB
- TinyUSB の複合デバイス、CDC × 2。VID 0x2E8A / PID 0x000A。
Manufacturer "JetRacer"、Product "SensorHub"、Serial = RP2040 の固有 ID
→ Linux では /dev/serial/by-id/usb-JetRacer_SensorHub_<serial>-if00(CDC0)
と -if02(CDC1)で見える
- CDC の TX バッファ 4096 バイト、RX 512 バイト
- CDC0 が開かれていなければ、フレームは黙って捨てる。
TX の空きが足りなければ捨てて cdc0_tx_drops を数える
- CDC0 を 1200 bps で開かれたら、UF2 ブートローダへ再起動する(Arduino と同じ流儀)
# CDC0 のワイヤ形式
フレーム = COBS( payload ‖ crc16_le(payload) ) ‖ 0x00
- CRC-16/CCITT-FALSE(多項式 0x1021、初期値 0xFFFF、反転なし、xorout 0)
= Python の binascii.crc_hqx(payload, 0xFFFF)。
CRC はリトルエンディアンで payload の後ろに付ける
- 多バイトのフィールドはすべてリトルエンディアン、packed(詰め物なし)
- 受信側は 0x00 までを 1 フレームとし、COBS を戻して CRC が合わなければ捨てる。
512 バイトを超えたら捨てて、次の 0x00 から同期し直す
## MCU → ホスト(先頭 1 バイトが型)
0x11 IMU_BATCH(ヘッダ 18 バイト + 12 × n バイト)
u8 type, u16 seq, u64 t_read_us(FIFO を読み終えた MCU 時刻),
u32 first_index(先頭サンプルの通し番号), u8 n(1〜32), u8 flags,
i8 temp(℃ = temp / 2.07 + 25),
i16 × 6 × n(ax ay az gx gy gz の生の値。
±1000 dps なら 32.8 LSB/dps、±8 g なら 4096 LSB/g)
flags: 0x01 FIFO_OVERFLOW, 0x02 BATCH_DROPPED,
0x04 PARSE_ANOMALY, 0x08 IMU_DOWN
0x02 ODOM(27 バイト、50 Hz)
u8 type, u16 seq, u64 t_us(最後のエッジの MCU 時刻、0 = まだない),
u32 total_count, u32 last_period_us(直前 2 エッジの間隔、0 = 不明),
u64 t_now_us(このパケットを作った MCU 時刻)
0x03 SYNC 応答(13 バイト)
u8 type, u32 ping_id(そのまま返す), u64 t_rx_us(SYNC コマンドを復号した MCU 時刻)
0x04 LIDAR_MARK(30 バイト、約 56 Hz)
u8 type, u16 seq, u64 t_us, u64 byte_offset, u16 speed_dps, u16 start_angle,
u16 lidar_ts_ms, u8 crc, u32 drop_bytes(累積)
0x05 STATUS(52 バイト、1 Hz)
u8 type, u8 fw_major, u8 fw_minor, u8 fw_patch, u32 uptime_ms,
u8 imu_ok, u8 imu_whoami, u16 imu_odr_hz, u16 gyro_fs_dps, u8 accel_fs_g,
u32 imu_samples, u32 imu_fifo_overflows, u32 imu_batch_drops,
u32 cdc0_tx_drops, u32 lidar_bytes, u32 lidar_packets, u32 lidar_drop_bytes,
u16 lidar_speed_dps(最新パケットの速度), u8 lidar_pwm_duty_pct,
u32 odom_count, u16 selftest_hz
## ホスト → MCU
0x03 SYNC + u32 ping_id → SYNC 応答を返す
0x21 LIDAR_PWM + u8 duty_pct 15〜85。範囲外は無視
0x22 SELFTEST + u16 hz + u8 mode
hz=0 で停止。mode 0 = GP10 に出す / 1 = GP7 自身を駆動
0x7F BOOTSEL + u32 0xB007B007 一致したら UF2 ブートローダへ再起動
長さが合わないコマンドは無視する。
# LED(WS2812)
青 = USB 未接続 / 赤 = IMU 異常 /
黄 = 欠落カウンタのどれかが直近 0.5 秒に増えた / 緑 = 正常
# ホスト側の Python ドライバ
- 読み出しスレッド 1 本が pyserial で CDC0 を読み、0x00 区切りで切り分け、
COBS を戻し、CRC を確かめ、型ごとに振り分ける。
最新値のラッチ(ODOM, STATUS, SYNC)と、取りこぼさないキュー
(IMU_BATCH, LIDAR_MARK)を持つ
- IMU の本体は numpy.frombuffer(body, dtype="<i2").reshape(n, 6) で一括デコードする
- MCU 時刻 → ホストの monotonic 時刻の変換:
受信のたびに(受信時刻 − MCU 時刻)を記録し、直近 100 個の最小値をオフセットにする
(USB の遅延は必ず正なので、最小値が本当のずれに最も近い)。
ODOM の t_now_us と SYNC 応答で更新する
- 速度は「tick の間に増えたエッジ数 × (m_per_rev / 8) ÷ MCU 時刻での経過時間」で出す。
最後のエッジ間隔の逆数は使わない(停止際のバックラッシュで、1 tick だけ
大きなスパイクが出る)。最後のエッジから 200 ms を超えたら 0
- 生ログ: IMU バッチを全部バイナリで保存する
(先頭にマジック。レコードごとに seq・t_read_us・first_index・n・flags・temp・
受信時刻・サンプル)。MARK は JSON Lines、STATUS とオフセットはメタデータの JSON
- 検定スクリプト: first_index の連続性、flags、MARK の byte_offset の差
(すべて 47 × 32 = 1504 のはず)、フレーム間隔を調べ、
異常が 1 つでもあれば終了コード 1
- LiDAR の回転数の確認: 走行を始める前に必ず duty を送る。
STATUS の lidar_pwm_duty_pct が一致し、lidar_speed_dps / 360 が期待値の
±10 % に入るまで開始しない。終了時は duty 50 % に戻す
(回転数はハブと LiDAR に残る状態なので、毎回設定して読み返す)
- ベンチ用 CLI: 秒ごとの IMU サンプル数と欠け、odom、LiDAR のバイト数・
CRC 不良・MARK の整合、STATUS を表示する。
オプションで、SYNC 往復の計測、LiDAR の duty の変更、自己診断の周波数の設定、
BOOTSEL への再起動
# 運用上の注意(実装に反映すること)
- Ubuntu では、列挙直後の 10〜20 秒は ModemManager が両方の CDC を開いて
AT コマンドを送る。その間のコマンドは壊れて捨てられ、CDC1 のバイト数にも
ずれが出る。MARK の中身(開始角・タイムスタンプ・CRC)で、ホストがずれを
求めること。udev で ENV{ID_MM_DEVICE_IGNORE}="1" を付ける手順も README に書く
- オドメトリは 1 チャンネルなので、回転の向きはわからない(後退も正の値になる)
# 受け入れ試験(この数値で合否を判定する)
- IMU: 7,900〜7,960 サンプル/s、first_index の欠け 0、flags 0、
静止時の accel z が約 1 g
- LiDAR: CDC1 に約 84〜85 KB/s、CRC-8 の不良 0、MARK は 56〜57 個/s で整合 100 %
- 10 分間の連続記録で、IMU の欠け 0、MARK の byte_offset の差がすべて 1504
- SYNC の往復時間: 中央値 0.5 ms 未満
- 自己診断: GP10 → GP7 のジャンパで 100 / 400 / 1000 Hz を入れ、
エッジ数が 1 秒あたり 2 × 周波数になる
- LiDAR の duty を 70 % にすると約 13 Hz、50 % に戻すと約 9.7〜9.9 Hz
(STATUS の lidar_speed_dps で確かめる)
# 進め方
1. まず構造体の定義(proto.h)と、ホスト側の encode / decode を書き、
両者の往復テストを作る
2. ファームウェアを、IMU → オドメトリ → LiDAR 透過 → MARK → PWM の順に
1 つずつ足し、段ごとにベンチ CLI で確かめる
3. 最後に受け入れ試験を実行し、結果を表で報告する
(実施しなかった項目は「未実施」と書く)