StampFlyの新しいファームの詳細設計書

StampFlyの詳細設計書を作ったのでこちらに転記します。 基本的にはこれらの文章はClaude codeが吐き出したものをちょっとだけ修正して載せているだけです。Githubのレポジトリにも乗っているものですが、いつ頃作ったのかの記録としてブログにも残しておこうと思います。

vehicle_new 詳細設計書

Note: English version follows after the Japanese section. / 日本語の後に英語版があります。

1. 概要

本文書はvehicle_newの詳細設計を定義する。アーキテクチャ設計書に基づき、実装レベルの仕様を記述する。

項目	内容
Pub-Subフレームワーク	トピック定義、バッファ方式、API
状態遷移テーブル	onExit/onEnterコールバック
制御インターフェース定義	IController
状態推定インターフェース定義	IEstimator
パラメータシステム	マクロ1行定義、バリデーション、コールバック
センサ観測スイッチ	パラメータ連動、推定内部で判断
ディレクトリ構造	ESP-IDFプロジェクト配置
メモリ配置	パーティション、Blackbox

2. Pub-Subフレームワーク

設計方針

• StampFlyの制約に最適化した独自の軽量設計
• 同一MCU内のタスク間通信のみ
• シリアライズ不要（構造体メモリ直接共有）
• 全トピックはコンパイル時に確定
• 内部バッファ方式はデータフローの特性に応じて選択

トピック定義

// topics.hpp — 全トピックの定義（1箇所）
// All topic definitions (single location)
// トピック追加はここに1行追加するだけ
// Adding a topic requires only one line here

#include "topic.hpp"
#include "data_types.hpp"

// Topic<DataType, BufferPolicy, BufferSize>
inline Topic<ImuData,         RingBuffer, 8>  sensor_imu;
inline Topic<TofData,         Queue, 2>       sensor_tof;
inline Topic<FlowData,        Queue, 2>       sensor_flow;
inline Topic<MagData,         Queue, 2>       sensor_mag;
inline Topic<BaroData,        Queue, 2>       sensor_baro;
inline Topic<PowerData,       Latest, 1>      sensor_power;
inline Topic<StateEstimate,   Latest, 1>      estimate_state;
inline Topic<SystemMode,      Latest, 1>      system_mode;
inline Topic<CommandSetpoint, Latest, 1>      command_setpoint;
inline Topic<ControlOutput,   Latest, 1>      control_output;
inline Topic<MotorOutput,     Latest, 1>      actuator_motor;
inline Topic<SystemAlert,     Queue, 4>       system_alert;

バッファ方式（3種）

方式	特性	用途
RingBuffer	Lock-free SPSC、全サンプル保持、ISR安全	IMU→推定、全データ→ログ
Queue	FreeRTOS Queue、バッファリング	低レートセンサ（ToF/Flow/Mag/Baro）
Latest	最新値上書き、共有メモリ	推定値→制御、推定値→テレメトリ

API

// Publish (producer side)
// 発行（生産者側）
ImuData data = readSensor();
sensor_imu.publish(data);

// Subscribe with callback (consumer side, at init)
// コールバックで購読（消費者側、初期化時に登録）
sensor_imu.subscribe([this](const ImuData& d) {
    this->onImuData(d);
});

// Poll latest value
// 最新値のポーリング
auto data = estimate_state.latest();

トピック追加手順

1. data_types.hpp に構造体を追加
2. topics.hpp に Topic<> を1行追加
3. 発行側で publish()、購読側で subscribe() を呼ぶ

3. 状態遷移テーブル

onExit/onEnterコールバック

遷移	onExit（旧状態を出る時）	onEnter（新状態に入る時）
INIT → IDLE_GROUND	—	キャリブレーション管理を起動
IDLE_GROUND → IDLE_HELD	（リザーブ）	（リザーブ）
IDLE_HELD → IDLE_GROUND	（リザーブ）	（リザーブ）
IDLE_GROUND → ARMED_GROUND	（リザーブ）	全PIDリセット、ESKFリセット、ブザー(arm音)
ARMED_GROUND → TAKEOFF	（リザーブ）	離着陸MGR: 離陸シーケンス開始、高度目標セット
TAKEOFF → FLYING	離着陸MGR: シーケンス終了	ESKF位置/速度リセット、バイアスフリーズ解除
FLYING(サブモード切替)	旧サブモードのコントローラリセット	新サブモードの初期化（高度キャプチャ等）
FLYING → LANDING	（リザーブ）	離着陸MGR: 着陸シーケンス開始
FLYING → ARMED_GROUND	高度/位置コントローラリセット	ESKFホールド
FLYING → IDLE_GROUND	高度/位置コントローラリセット	モーター停止、ESKFリセット、ブザー(disarm音)
LANDING → IDLE_GROUND	離着陸MGR: シーケンス終了	モーター停止、ESKFリセット、バイアスフリーズ
ARMED_GROUND → IDLE_GROUND	（リザーブ）	モーター停止、ブザー(disarm音)

「リザーブ」は実装・テスト時に必要に応じて追加する。

4. 制御インターフェース定義

PID/MPC/LQRを差替可能にするための統一インターフェース。

// Control interface definition
// 制御インターフェース定義

class IController {
public:
    virtual ~IController() = default;

    // Called every control cycle (400Hz)
    // 制御周期ごとに呼ばれる（400Hz）
    virtual ControlOutput compute(
        const StateEstimate& state,      // Current estimated state
        const CommandSetpoint& setpoint, // Target setpoint
        float dt                         // Time step [s]
    ) = 0;

    // Reset internal state (integrators, filters, etc.)
    // 内部状態リセット（積分器、フィルタ等）
    virtual void reset() = 0;

    // Mode change notification
    // モード変更通知
    virtual void onModeChange(uint8_t new_mode) = 0;
};

5. 状態推定インターフェース定義

ESKF/EKF/Complementary Filterを差替可能にするための統一インターフェース。

// Estimation interface definition
// 状態推定インターフェース定義

class IEstimator {
public:
    virtual ~IEstimator() = default;

    // IMU prediction step (400Hz)
    // IMU予測ステップ（400Hz）
    virtual void predict(const ImuData& imu, float dt) = 0;

    // Sensor observation updates (async, called when data arrives)
    // センサ観測更新（非同期、データ到着時に呼ばれる）
    virtual void updateTof(const TofData& tof) = 0;
    virtual void updateFlow(const FlowData& flow) = 0;
    virtual void updateMag(const MagData& mag) = 0;
    virtual void updateBaro(const BaroData& baro) = 0;

    // Get current state estimate
    // 現在の推定値取得
    virtual StateEstimate getState() const = 0;

    // Reset full state
    // 全状態リセット
    virtual void reset() = 0;

    // Reset position and velocity only
    // 位置・速度のみリセット
    virtual void resetPositionVelocity() = 0;
};

センサ観測スイッチ

センサ観測のON/OFFはパラメータシステムと連携し、推定コンポーネント内部で判断する。

sensor.tof → 推定コンポーネントに常に届く
           → 推定内部で eskf.use_tof パラメータを確認
           → false なら観測更新をスキップ
           → ログには常に記録される

パラメータ変更時のコールバックで推定コンポーネントの内部マスクを即時更新する。これにより段階的にセンサを有効化しながらデバッグが可能。

6. パラメータシステム

設計方針

マクロ1行でパラメータの定義・バリデーション・コールバック・NVS永続化を全て完結させる。

パラメータ定義

// params.def — 全パラメータ定義（1箇所）
// All parameter definitions (single location)
// パラメータ追加はここに1行追加するだけ
//
//              名前                    デフォルト   min      max      コールバック
PARAM_FLOAT("rate.roll.kp",           1.365e-3f,  0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("rate.roll.ti",           0.7f,       0.01f,   100.0f,  on_pid_changed)
PARAM_FLOAT("rate.roll.td",           0.01f,      0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("rate.pitch.kp",         1.995e-3f,  0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("rate.pitch.ti",         0.7f,       0.01f,   100.0f,  on_pid_changed)
PARAM_FLOAT("rate.pitch.td",         0.01f,      0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("rate.yaw.kp",           5.31e-3f,   0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("rate.yaw.ti",           1.6f,       0.01f,   100.0f,  on_pid_changed)
PARAM_FLOAT("rate.yaw.td",           0.01f,      0.0f,    1.0f,    on_pid_changed)
PARAM_FLOAT("eskf.process.accel_noise", 0.3f,    0.01f,   10.0f,   on_eskf_changed)
PARAM_FLOAT("eskf.obs.tof_noise",    0.03f,      0.001f,  1.0f,    on_eskf_changed)
PARAM_BOOL ("eskf.use_tof",          true,       0,       1,       on_eskf_mask_changed)
PARAM_BOOL ("eskf.use_flow",         true,       0,       1,       on_eskf_mask_changed)
PARAM_BOOL ("eskf.use_baro",         false,      0,       1,       on_eskf_mask_changed)
PARAM_BOOL ("eskf.use_mag",          false,      0,       1,       on_eskf_mask_changed)
// ... 他のパラメータも同様に追加

アクセスAPI

// Read parameter value
// パラメータ値の読み取り
float kp = params::get<float>("rate.roll.kp");

// Write parameter value (from WiFi/CLI)
// パラメータ値の書き込み（WiFi/CLIから）
// → バリデーション → 値更新 → コールバック実行 → NVS保存
params::set("rate.roll.kp", 2.0e-3f);

// List all parameters (for CLI)
// 全パラメータ一覧（CLI用）
params::list();
// Output: "rate.roll.kp = 1.365e-3 [0.0, 1.0]"

// Save all to NVS / Load from NVS
// NVS一括保存・読み込み
params::save();
params::load();

// Reset to defaults
// デフォルト値にリセット
params::resetAll();

命名規則

3階層: 機能.対象.パラメータ

rate.roll.kp              # Rate control, roll axis, proportional gain
rate.pitch.ti             # Rate control, pitch axis, integral time
attitude.roll.kp          # Attitude control, roll axis, proportional gain
altitude.alt.kp           # Altitude control, altitude loop, Kp
altitude.vel.kp           # Altitude control, velocity loop, Kp
position.pos.kp           # Position control, position loop, Kp
eskf.process.gyro_noise   # ESKF, process noise, gyro
eskf.obs.tof_noise        # ESKF, observation noise, ToF
eskf.gate.tof_innov       # ESKF, innovation gate, ToF
eskf.use_tof              # ESKF, sensor enable, ToF
safety.impact.accel_threshold  # Safety, impact detection, accel threshold

7. ディレクトリ構造

HALコンポーネントはvehicle_new内にコピー（完全独立）。

firmware/vehicle_new/
├── CMakeLists.txt                 # ESP-IDF project root
├── partitions.csv
├── sdkconfig.defaults
├── docs/
│   ├── requirements.md            # 要件定義書
│   ├── requirements.tex/pdf       # 要件定義書（PDF版）
│   ├── architecture.md            # アーキテクチャ設計書
│   └── detailed_design.md         # 詳細設計書（本文書）
├── main/
│   ├── CMakeLists.txt
│   ├── main.cpp                   # app_main(), INIT逐次実行
│   └── config.hpp                 # 固定パラメータ（constexpr）
├── components/
│   ├── sf_core/                   # 基盤: Pub-Sub, パラメータ, データ型
│   │   ├── include/
│   │   │   ├── topic.hpp          # Topic<T> テンプレート
│   │   │   ├── topics.hpp         # 全トピック定義
│   │   │   ├── data_types.hpp     # 全構造体定義
│   │   │   ├── params.hpp         # パラメータシステム
│   │   │   └── params.def         # パラメータ定義（1箇所）
│   │   └── params.cpp
│   ├── sf_state/                  # 状態管理
│   │   └── include/
│   │       ├── state_manager.hpp  # 状態遷移、onExit/onEnter
│   │       └── flight_state.hpp   # enum定義
│   ├── sf_estimator/              # 状態推定インターフェース
│   │   └── include/
│   │       └── estimator.hpp      # IEstimator定義
│   ├── sf_estimator_eskf/         # ESKF実装
│   ├── sf_controller/             # 制御インターフェース
│   │   └── include/
│   │       └── controller.hpp     # IController定義
│   ├── sf_controller_pid/         # PID実装
│   ├── sf_actuator/               # ミキサー＋モーター出力
│   ├── sf_command/                # コマンド処理
│   ├── sf_failsafe/               # フェイルセーフ
│   ├── sf_takeoff_landing/        # 離着陸マネージャー
│   ├── sf_logger/                 # データロガー＋Blackbox
│   ├── sf_telemetry/              # テレメトリ
│   ├── sf_notify/                 # LED/ブザー通知
│   ├── sf_calibration/            # キャリブレーション管理
│   ├── sf_comm/                   # 通信（ESP-NOW/UDP/TCP）
│   ├── sf_hal_bmi270/             # IMUドライバ（コピー）
│   ├── sf_hal_bmm150/             # 地磁気ドライバ（コピー）
│   ├── sf_hal_bmp280/             # 気圧ドライバ（コピー）
│   ├── sf_hal_vl53l3cx/           # ToFドライバ（コピー）
│   ├── sf_hal_pmw3901/            # OptFlowドライバ（コピー）
│   ├── sf_hal_motor/              # モータードライバ（コピー）
│   ├── sf_hal_led/                # LEDドライバ（コピー）
│   ├── sf_hal_buzzer/             # ブザードライバ（コピー）
│   ├── sf_hal_button/             # ボタンドライバ（コピー）
│   └── sf_hal_power/              # 電源モニタドライバ（コピー）
├── tasks/
│   ├── tasks.hpp                  # タスク関数宣言
│   ├── imu_task.cpp               # IMU + 状態推定
│   ├── control_task.cpp           # 制御 + アクチュエーション
│   ├── state_task.cpp             # 状態管理（イベント駆動）
│   ├── flow_task.cpp              # OptFlow
│   ├── mag_task.cpp               # 地磁気
│   ├── baro_task.cpp              # 気圧
│   ├── comm_task.cpp              # 通信 + コマンド処理
│   ├── tof_task.cpp               # ToF + 離着陸マネージャー
│   ├── telemetry_task.cpp         # テレメトリ
│   ├── power_task.cpp             # 電源 + フェイルセーフ
│   ├── button_task.cpp            # ボタン入力
│   ├── notify_task.cpp            # LED/ブザー
│   ├── cli_task.cpp               # CLI + パラメータ
│   └── log_task.cpp               # データロガー + Blackbox
└── examples/                      # サンプル集（チュートリアル的コメント）
    ├── blink_led/
    ├── read_imu/
    ├── pid_motor/
    ├── eskf_sim/
    ├── tof_altitude/
    └── espnow_pair/

8. メモリ配置

パーティションテーブル

# Name,    Type, SubType, Offset,   Size,     Flags
nvs,       data, nvs,     0x9000,   0x6000,           # 24KB — パラメータ永続化
phy_init,  data, phy,     0xF000,   0x1000,           # 4KB  — WiFi/BT校正
factory,   app,  factory, 0x10000,  0x300000,          # 3MB  — ファームウェア
storage,   data, spiffs,  0x310000, 0x200000,          # 2MB  — Blackbox
# 残り 2MB (0x510000〜0x800000) は未割当（将来用）

Blackbox容量

記録レート	容量	記録時間
400Hz / 128B	50KB/s	約40秒
50Hz / 128B	6.25KB/s	約5分

Blackboxアクセス

• CLI経由: sf log download でUSBシリアル経由転送
• 将来: USBマスストレージも検討可能だが初期実装はCLIで統一

RAMメモリ（ESP32-S3 512KB）

タスクスタックの合計: 約92KB

タスク	スタック
ImuTask	16KB
ControlTask	8KB
StateTask	4KB
FlowTask	8KB
MagTask	8KB
BaroTask	8KB
CommTask	4KB
TofTask	8KB
TelemetryTask	4KB
PowerTask	4KB
ButtonTask	4KB
NotifyTask	4KB
CLITask	8KB
LogTask	4KB
合計	92KB

残り約420KBでPub-Subバッファ、ESKF行列、ヒープ等を賄う。

---

vehicle_new Detailed Design

1. Overview

This document defines the detailed design of vehicle_new, based on the architecture design (architecture.md).

2. Pub-Sub Framework

Design Policy

• Lightweight custom design optimized for StampFly constraints
• Intra-MCU task communication only, no serialization
• All topics determined at compile time
• Internal buffer policy selected per data flow characteristics

Buffer Policies

Policy	Characteristics	Use Case
RingBuffer	Lock-free SPSC, full retention, ISR-safe	IMU→Estimation, All→Logger
Queue	FreeRTOS Queue, buffered	Low-rate sensors
Latest	Latest-value overwrite, shared memory	Estimate→Control

API

topic.publish(data);                    // Producer
topic.subscribe(callback);             // Consumer (callback)
auto data = topic.latest();            // Consumer (polling)

3. State Transition Table

See Japanese section for complete onExit/onEnter callback table. Entries marked (reserved) will be filled during implementation and testing.

4. Control Interface Definition

class IController {
public:
    virtual ~IController() = default;
    virtual ControlOutput compute(const StateEstimate& state,
                                   const CommandSetpoint& setpoint,
                                   float dt) = 0;
    virtual void reset() = 0;
    virtual void onModeChange(uint8_t new_mode) = 0;
};

5. Estimation Interface Definition

class IEstimator {
public:
    virtual ~IEstimator() = default;
    virtual void predict(const ImuData& imu, float dt) = 0;
    virtual void updateTof(const TofData& tof) = 0;
    virtual void updateFlow(const FlowData& flow) = 0;
    virtual void updateMag(const MagData& mag) = 0;
    virtual void updateBaro(const BaroData& baro) = 0;
    virtual StateEstimate getState() const = 0;
    virtual void reset() = 0;
    virtual void resetPositionVelocity() = 0;
};

Sensor Observation Switch

Sensor observation ON/OFF is controlled via parameters and decided internally by the estimation component. Raw data always reaches the estimator and logger; the estimator skips observation updates when the parameter is false.

6. Parameter System

Single-macro definition: one line per parameter covers declaration, validation, callback, and NVS persistence.

PARAM_FLOAT("rate.roll.kp", 1.365e-3f, 0.0f, 1.0f, on_pid_changed)

7. Directory Structure

HAL components are copied into vehicle_new (fully independent). See Japanese section for complete tree.

8. Memory Layout

Partition Table

nvs,       data, nvs,     0x9000,   0x6000,    # 24KB
phy_init,  data, phy,     0xF000,   0x1000,    # 4KB
factory,   app,  factory, 0x10000,  0x300000,   # 3MB
storage,   data, spiffs,  0x310000, 0x200000,   # 2MB Blackbox
# Remaining 2MB unallocated (future use)

Task Stack Summary

Total task stacks: ~92KB out of 512KB RAM. Remaining ~420KB for Pub-Sub buffers, ESKF matrices, heap, etc.