制御ロジックを FW に置くか Host に置くかの判断

位置/速度 loop・FF・friction は FW、homing HSM・校正 orchestrator は Host。判断基準は「制御周期」「位相要求」「反復サイクル」の 3 点

要点: 1 kHz の loop に位相を合わせる必要があるものは FW。数十 ms の遅延を許容できて iteration cycle を速くしたいものは Host。境界線は「制御周期の中で計算が完結するか」と「反復開発の速さがどれだけ価値を持つか」で決まる。

判断表 #

機能	場所	理由
PID (velocity + position loop)	FW 1 kHz	位相遅れが gain の上限を直接決める
Feed-forward (velocity_ff / duty_ff)	FW 1 kHz	上の loop と同じ tick で加算する
摩擦補償 (coulomb + viscous)	FW 1 kHz	`sign(r_v)` を制御 tick で評価。target 速度基準
電流零点 baseline 校正	FW state machine	静止判定 (duty limiter output 見る) を 1 kHz 内で完結
位置クランプ (limit)	FW	host bug の二重防御。指令異常でも越えない
Homing HSM (BASELINE→SOFT_START→CRUISE→BACKOFF)	Host	判定遅延許容大。iteration cycle が価値
校正 orchestrator (stage 順序 + yaml I/O)	Host	stage 数百 ms〜数秒、file I/O が要る
trajectory generator (Ruckig 等)	Host	100 Hz publish で足りる
BT / スキル / ゲームロジック	Host	ms 遅延の許容

Host 判定の遅延バジェット #

CAN + ROS 2 の典型的な feedback → 判定 → command 経路:

経路	遅延
FW → CAN feedback (100 Hz)	最大 10 ms (aging)
CAN → ROS 2 subscription cb	1-5 ms (local, reliable)
host 判定 → target publish	~1 ms
target → FW 反映	最大 10 ms
合計	~20-30 ms

feedback rate を上げれば減る:

200 Hz → ~15-20 ms
500 Hz → ~10-15 ms (CAN 帯域と DMA 負荷に注意)

Host で OK な判定の条件 #

overshoot 見積 = 動作速度 × 遅延 が機構許容内に収まるか:

Homing: 1 rad/s × 30 ms = 0.03 rad。gear 後で数 mm。stopper のコンプライアンスで吸収可
押し当て (grip): 同上。伸縮性のあるフィンガーで吸収
BT のスキル遷移: ms 単位で早くする意味が無い

判定自体が窓平均なら遅延は薄まる:

例: 「5 frame 連続で v < 閾値」なら 1 frame の遅延は判定を左右しない
N frame 平均は本質的に N × sample_period の window を持つので、1 sample の遅延は無視できる

FW に置くべきの条件 (Host では不可)#

制御 tick 内で位相合成が要る: PID の error → duty 出力は same tick。FF もそこに加える
緊急停止 (< 1 ms 要): watchdog、E-stop、over-current。CAN 経由の判定は間に合わない
高速接触検出: 高速衝突の damage 防止。低速 homing は該当しない
通信断でも動く必要がある: 単体でフェイルセーフな挙動 (soft brake、指令維持 + timeout)

反復サイクルという「もう一つの軸」#

技術的には FW でも Host でも書けるロジックの場合、反復サイクルの速さで場所を決める:

FW 側の 1 iteration: コード編集 → build → flash → 挙動確認 = 数分
Host 側の 1 iteration: コード編集 → colcon build → ros2 run = 30 秒
ROS 2 node の場合 param 変更だけなら ros2 param set で秒

Homing のような「機構ごとにチューニングが要る」ロジックは、Host に置くだけで開発速度が 10x になる。要求性能が Host で足りるなら Host が正解。

「実測してから FW に降ろす」順序 #

新しい機能はまず Host で書く → 実測で遅延問題が見えたら該当部分を FW に降ろす。逆順 (先 FW) は罠:

Host で書いてから「実は要らなかった」と分かる機能が多い
FW に降ろした後に仕様変更が入ると reflash 地獄
遅延の実測データを持たずに設計するのは推測でしかない

例: FF スタックは最初 host 側で軌道 + FF 計算を試作した。位相遅れが 50 ms 出た時点で「velocity_ff / duty_ff は FW に降ろす、加速度換算 (J/Kt) は host 側」と切り分けた。摩擦補償も host 側で試して chatter が出たので FW 化した (FF スタック)。

実測の見方 (host 遅延が効いてるかの判別)#

bag を取って:

feedback.velocity が閾値超過した wall time
target.value が変化した wall time
差が 30 ms 以内で安定していれば Host で問題無い
差が伸びる or ジッタが大きい → CAN feedback rate 上げる / ROS 2 の QoS 見直し / node の spin 頻度

「FW に降ろすべきか」は最後の option。まず feedback rate と QoS で解決できることが多い。