StampFly勉強会2026
1.9K Views
March 02, 26
スライド概要
■ドローンやロボットを自作することを通じて制御や関連技術の生涯勉強情報を提供■工学博士■防大航空宇宙→筑波大博士■陸自→対戦車誘導弾等の装備品開発→高専教員→大学教員■ロボットランサー優勝→マイクロマウスニューテクノロジー賞受賞■指導者としてつくばチャレンジバンナム賞→飛行ロボコンマルチコプタ部門１位等々■北海道函館出身
関連スライド
StampFlyで学ぶマルチコプタ制御
伊藤恒平
369.9K
2025StampFly勉強会
伊藤恒平
78.7K
各ページのテキスト
資料ダウンロード / Slide Download Docswell スライド資料はこちらからダウン ロードできます https://www.docswell.com/s/Kouhei_Ito/ KLV77G-2026-03-02-010945 伊藤 恒平 StampFly 勉強会 2026 2/4
GitHub リポジトリ / GitHub Repository stampfly_ecosystem ファームウェア・ツール・資料の 全ソースコード https://github.com/M5Fly-kanazawa/ stampfly_ecosystem 伊藤 恒平 StampFly 勉強会 2026 3/4
リポジトリの更新 / Sync Repository 通常の更新 git pull でリモートの最新変更を取り込む 1 2 cd ~/ stampfly_ecosystem git pull git pull でエラーが出た場合 ローカルの変更がリモートと競合すると pull が失敗する。その場合はローカルを破棄してリモート に強制一致させる: git fetch origin git reset --hard origin/main 3 git clean -fd 1 2 注意: ローカルの変更・追加ファイルはすべて失われます。 伊藤 恒平 StampFly 勉強会 2026 4/4
Lesson 0 環境セットアップ Environment Setup StampFly Workshop
ワークショップカリキュラム / Workshop Curriculum Day Day 1 Day 2 Day 3 Day 4 Day 5 AM Lesson 0–2 3–5 6–8 9 – 11 StampFly Workshop テーマ 環境セットアップ + モータ + コントローラ LED + IMU + 初フライト モデリング + システム同定 + PID 制御 姿勢推定 + Python SDK + 競技会準備 ホバリングタイム競技会 Lesson 0: 環境セットアップ 2 / 18
StampFly ハードウェア / Hardware 項目 MCU IMU 気圧 ToF 質量 通信 バッテリ StampFly Workshop 仕様 ESP32-S3 (M5Stamp S3) BMI270 (6 軸 400 Hz) BMP280 VL53L3CX 37 g（バッテリ含む） ESP-NOW + WiFi LiPo 1S 3.7 V Lesson 0: 環境セットアップ 3 / 18
エコシステム全体像 / Ecosystem Overview tools/ analysis/ 開発ツール データ解析 sf CLI Genesis 3D 物理 control/ 制御設計 PID, MPC, SIL protocol/ 通信仕様 StampFly Workshop control/ — 制御設計 Jupyter / Python simulator/ シミュレータ firmware/ 組込みコード firmware/ — 組込みコード simulator/ — 3D シミュレータ analysis/ — データ解析 tools/ — sf CLI ESP32-S3 / ESP-IDF / C++ protocol/ — 通信仕様 SSOT: YAML → コード自動生成 Lesson 0: 環境セットアップ 4 / 18
開発ツール sf CLI / Development Tool カテゴリ ビルド 診断 ログ シミュレータ キャリブレーション コマンド例 sf build / sf flash sf doctor / sf monitor sf log wifi / sf log viz sf sim run sf cal gyro/accel/mag 説明 ファームウェア開発 デバッグ テレメトリ取得・可視化 仮想環境で練習 センサ校正 ワークフロー sf lesson switch N → sf lesson build → sf lesson flash StampFly Workshop Lesson 0: 環境セットアップ 5 / 18
Windows 環境構築 (1/2) / Windows Setup 確認コマンド (CMD) git --version python --version 期待される結果 git version 2.x Python 3.10 以上 なければ winget install Git.Git winget install Python.Python.3.12 インストール手順（CMD で実行） 1. リポジトリをクローン 2. install.bat を実行（ESP-IDF + sfcli 自動インストール） 3. setup_env.bat で開発環境をアクティベート git clone https :// github .com/M5Fly - kanazawa / stampfly_ecosystem cd stampfly_ecosystem install .bat setup_env .bat StampFly Workshop Lesson 0: 環境セットアップ 6 / 18
Windows 環境構築 (2/2) / Driver & Verification USB シリアルドライバ CH9102F ドライバをインストール（M5Stack 製品用） https://docs.m5stack.com/en/download 動作確認 sf doctor で環境診断 — すべて OK になれば完了 macOS / Linux ユーザー install.sh + setup_env.sh を使用してください StampFly Workshop Lesson 0: 環境セットアップ 7 / 18
今日のゴール / Today’s Goal 目標 ワークショップファームウェアをビルド・書き込み・動作確認する 1 sf lesson build でビルド 2 sf lesson flash で書き込み 3 シリアルモニタで "Hello StampFly!" を確認 StampFly Workshop Lesson 0: 環境セットアップ 8 / 18
開発フロー / Development Flow sf lesson switch N sf lesson build sf lesson flash Serial Monitor student.cpp → user_code.cpp ESP-IDF compile USB flash + monitor verify output modify code & rebuild StampFly Workshop Lesson 0: 環境セットアップ 9 / 18
sf CLI とは / What is sf CLI? StampFly 開発専用のコマンドラインツール ESP-IDF のコマンドをシンプルに実行できる コマンド sf doctor sf lesson build sf lesson flash sf lesson switch N sf monitor StampFly Workshop 説明 環境診断 ワークショップビルド 書き込み＋モニタ レッスン切り替え シリアルモニタ Lesson 0: 環境セットアップ 10 / 18
ワークショップの構造 / Workshop Structure 学生が実装する関数は 2 つだけ！ setup() — 起動時に 1 回呼ばれる loop_400Hz(dt) — 400Hz（2.5ms 毎）で呼ばれる ハードウェアの複雑さは ws:: 名前空間で隠蔽 FreeRTOS, SPI/I2C, センサーフュージョンを意識する必要なし StampFly Workshop Lesson 0: 環境セットアップ 11 / 18
ファームウェアの全体像 / Firmware Architecture user_code.cpp ← You are here! setup() / loop_400Hz() 皆さんのコードはここ！ user_code.cpp を編集するだけ sensor read, filter, motor ws::gyro_x() と書くだけで SPI 通信→フィルタ→バイアス補正を 全部やってくれる ws:: API gyro_x(), motor_set_duty() Vehicle Components IMU, baro, motor driver xTaskCreate, xQueueSend 下のレイヤーはワークショップ中 意識する必要なし ESP-IDF / FreeRTOS Tasks, timers, queues SPI, I2C, GPIO Hardware ESP32-S3, BMI270, BMP280 StampFly Workshop Lesson 0: 環境セットアップ 12 / 18
400Hz ループの裏側 / Behind the 400Hz Loop ESP Timer 2500 µs IMU Task Read BMI270 Go! loop_400Hz (Your Code) hardware timer interrupt SPI read + bias correction data ready user_code.cpp runs here repeats every 2.5 ms = 400 Hz ポイント loop_400Hz(dt) は常に IMU データ更新後に呼ばれる— 学生はタイ マー管理不要、ただ関数を書くだけ！ StampFly Workshop Lesson 0: 環境セットアップ 13 / 18
実習: Hello StampFly / Hands-on
1
# include "workshop_api.hpp"
2
void setup ()
{
5
ws:: print("Hello StampFly!");
6 }
3
4
7
void loop_400Hz(float dt)
{
10
// Nothing to do in Lesson 0
11 }
8
9
user_code.cpp
手順
1. sf lesson switch 0
2. 上のコードを user_code.cpp に書く
3. sf lesson build && sf lesson flash
StampFly Workshop
Lesson 0: 環境セットアップ
14 / 18
作業の進め方 / Workflow 1 ファイル構造 firmware/workshop/ ├── lessons/ │ ├── lesson_00/student.cpp │ ├── lesson_01/student.cpp │ └── ... └── main/ └── user_code.cpp ← 編集！ sf lesson switch N テンプレートを user_code.cpp にコ ピー 2 user_code.cpp を編集 3 sf lesson build 4 sf lesson flash 注意 student.cpp はテンプレートなので直接編集しない！ switch で何度でもリセットできる StampFly Workshop Lesson 0: 環境セットアップ 15 / 18
コントローラのセットアップ / Controller Setup コントローラのビルドと書き込み 1 コントローラを USB 接続 2 sf build controller 3 sf flash controller 確認 LCD に起動画面が表示されれば OK Lesson 2 でペアリングして使用する StampFly Workshop Lesson 0: 環境セットアップ 16 / 18
シミュレータで遊ぶ / Try the Simulator USB HID モードに切替 1 画面タッチでメニューを開く 2 Comm: を選択 3 USB HID に切替 → 自動再起動 操作 スロットル ロール / ピッチ ヨー 説明 上昇 傾き 回転 アクロモード（角速度制御）で飛行 コマンド sf sim run sf sim run -w ringworld 注意 終了後は Comm: を ESP-NOW に戻すこと（実機飛行用） StampFly Workshop Lesson 0: 環境セットアップ 17 / 18
チェックポイント / Checkpoint 確認事項 sf doctor がエラーなしで通る ビルドが成功し "Hello StampFly!" が表示される コントローラのビルド・書き込みが完了した シミュレータが起動し操縦を試した 次のレッスン Lesson 1: モータ制御 — モータを回してプロペラの動きを確認 StampFly Workshop Lesson 0: 環境セットアップ 18 / 18
Lesson 1 モータ制御 Motor Control StampFly Workshop
今日のゴール / Today’s Goal 目標 モータの番号と配置を理解し、PWM で個別制御する モータ番号 M1–M4 の位置を覚える ARM / DISARM の仕組みを理解する PWM デューティ比でモータ回転数を制御 StampFly Workshop Lesson 1: モータ制御 2/8
モータ配置 / Motor Layout FL FR M4 CW Front M1 CCW M2 CW ESP32 M3 CCW RL RR CW (clockwise) CCW (counter-clockwise) 対角のモータが同じ方向に回転（トルクバランス） M1(FR), M3(RL) = CCW / M2(RR), M4(FL) = CW StampFly Workshop Lesson 1: モータ制御 3/8
PWM とは / What is PWM? PWM Duty Cycle 0% T Duty Cycle 25% 50% Pulse Width Modulation （パルス幅変調） Duty 0% = 停止 Duty 100% = 最大回転 API: motor_set_duty(id, duty) id=1–4, duty=0.0–1.0 100% StampFly Workshop Lesson 1: モータ制御 4/8
ARM / DISARM とは / What is ARM? 安全スイッチ / Safety Switch ARM = モーター出力を有効化（回転可能になる） DISARM = モーター出力を無効化（強制停止） コードから arm() で ARM できる 重要 / Important arm() を呼ばないとモーターは回らない！ setup() 内で最初に呼ぶこと StampFly Workshop Lesson 1: モータ制御 5/8
モータ制御 API 関数 arm() disarm() motor_set_duty(id, duty) motor_set_all(duty) motor_stop_all() 説明 モーター出力を有効化 モーター出力を無効化 個別モータ設定 全モータ一括 全モータ停止 引数 — — id=1–4, duty=0.0–1.0 duty=0.0–1.0 — 安全注意 / Safety duty は 0.15 以下で！ 機体を手で押さえてテスト！ StampFly Workshop Lesson 1: モータ制御 6/8
実習: モータを順番に回す / Hands-on
1
2
# include "workshop_api.hpp"
static uint32_t timer = 0;
3
void setup () {
ws:: print("Lesson 1: Motor Control");
6
ws::arm (); // Enable motor output
7 }
8 void loop_400Hz(float dt) {
9
timer ++;
10
// 800 ticks = 2 seconds at 400 Hz
11
int motor_id = (timer / 800) % 4 + 1;
12
ws:: motor_stop_all ();
13
ws:: motor_set_duty(motor_id , 0.1f);
14 }
4
5
user_code.cpp
StampFly Workshop
Lesson 1: モータ制御
7/8
チェックポイント / Checkpoint 確認事項 arm() なしではモータが回らないことを確認した M1 が FR（右前）のプロペラを回す M1→M2→M3→M4 の順に回転する 次のレッスン Lesson 2: コントローラ入力 — スティックでモータを操作 StampFly Workshop Lesson 1: モータ制御 8/8
Lesson 2 コントローラ入力 Controller Input StampFly Workshop
今日のゴール / Today’s Goal 目標: スティック値を変数に読み取り、演算でモータを個 別制御する ESP-NOW 無線通信の仕組みを理解 チャンネル設定で他の受講生との混信を回避 変数と四則演算でスティック → モータ Duty を計算 オープンループ手動操縦の限界を体感 StampFly Workshop Lesson 2: コントローラ入力 2 / 12
ESP-NOW 通信フロー / Communication Flow Controller → ESP-NOW → StampFly API Controller Throttle Yaw Left Stick StampFly Workshop Wireless Right Stick ESP-NOW 2.4 GHz Roll Pitch StampFly rc_throttle() rc_roll() rc_pitch() rc_yaw() 0.0 – 1.0 -1.0 – +1.0 -1.0 – +1.0 -1.0 – +1.0 Lesson 2: コントローラ入力 3 / 12
コントローラ各部 / Controller Layout (MODE 3) Alt Hold / Manual (予定) Power SW Pitch l Roll ↔ STABILIZE / ACRO MODE 3 Throttle l Yaw ↔ Push: Flip Push: ARM (予定) M5 Button (LCD) タップでペアリング等 StampFly Workshop Lesson 2: コントローラ入力 4 / 12
コントローラ API 関数 set_channel(ch) rc_throttle() rc_roll() rc_pitch() rc_yaw() is_armed() motor_set_duty(id, v) 説明 WiFi チャンネル設定 スロットル ロール ピッチ ヨー ARM 状態 モータ個別制御 StampFly Workshop Lesson 2: コントローラ入力 値域 1, 6, 11 0.0 – 1.0 -1.0 – +1.0 -1.0 – +1.0 -1.0 – +1.0 true / false id=1–4, v=0.0–1.0 5 / 12
ペアリング手順 / Pairing Step 操作 1 コントローラの M5 ボタンを押しながら電源 ON → LCD に “Pairing mode...” 表示 2 StampFly のボタンを 3 秒長押し → 青 LED 高速点滅 + ビープ音 3 ペアリング完了 → ビープ音が鳴り、次回から自動接続 注意 教室では一組ずつペアリングする（ブロードキャスト通信のた め近くの機体と干渉する可能性） うまくいかない場合: 両方を再起動して Step 1 からやり直す StampFly Workshop Lesson 2: コントローラ入力 6 / 12
チャンネル設定 / Channel Setting 教室で複数の StampFly を同時に飛ばすと、同じチャンネル同士で混 信が起きる。チャンネルを分けて回避する。 Ch 1 6 11 割り当て例 グループ A（緑） グループ B（黄） グループ C（赤） StampFly Workshop 設定方法 set_channel(ch) を setup() 内で 呼ぶ 機体とコントローラで同じ番号にする こと！ Lesson 2: コントローラ入力 7 / 12
モータ配置とミキシング / Motor Layout & Mixing FL FR M4 CW Front M1 CCW M2 CW ESP32 M3 CCW RL CW (clockwise) StampFly Workshop Motor M1 FR M2 RR M3 RL M4 FL T + + + + Roll − − + + Pitch + − − + Yaw + − + − 直感的に理解する RR Pitch+ → 前傾 → 前 (M1,M4) 強 + 後 (M2,M3) 弱 Roll+ → 右傾 → 右 (M1,M2) 弱 + 左 (M3,M4) 強 CCW (counter-clockwise) Lesson 2: コントローラ入力 8 / 12
オープンループ制御 / Open-Loop Control Open-Loop Control Controller −1.0 ∼ +1.0 Stick Scaling × gain 0.0 ∼ 1.0 Motor PWM duty StampFly Drone No Feedback! 問題点 フィードバックがないため、外乱（風・重心ずれ）に対応できない！ → Lesson 5/8 で PID 制御を導入して解決 StampFly Workshop Lesson 2: コントローラ入力 9 / 12
実習: 手動ミキシング (1/2) / Hands-on: Setup
1
2
# include "workshop_api.hpp"
static uint32_t tick = 0;
3
void setup () {
ws:: print("L2: Open -Loop Control");
6
ws::arm ();
// Enable motor output
7
ws:: set_channel (1);
// 1, 6, or 11
8 }
4
5
user_code.cpp — setup
チャンネル番号
講師が指定した番号（1 / 6 / 11）に書き換えること！
コントローラも同じチャンネルに設定する
StampFly Workshop
Lesson 2: コントローラ入力
10 / 12
実習: 手動ミキシング (2/2) / Hands-on: Loop
void loop_400Hz(float dt) {
tick ++;
3
float t = ws:: rc_throttle ();
4
float r = ws:: rc_roll () * 0.3f;
5
float p = ws:: rc_pitch () * 0.3f;
6
float y = ws:: rc_yaw ()
* 0.3f;
7
ws:: motor_set_duty (1, t - r + p + y); // FR
8
ws:: motor_set_duty (2, t - r - p - y); // RR
9
ws:: motor_set_duty (3, t + r - p + y); // RL
10
ws:: motor_set_duty (4, t + r + p - y); // FL
11
if (tick % 80 == 0)
12
ws:: print("T=%.2f R=%.2f P=%.2f Y=%.2f",
13
t, r, p, y);
14 }
1
2
user_code.cpp — loop
StampFly Workshop
Lesson 2: コントローラ入力
11 / 12
チェックポイント / Checkpoint 確認事項 指定チャンネルで混信なく通信できた コントローラとペアリングできた スロットルで全モータが均等に回る ピッチスティックで前後モータの回転差が出る 次のレッスン Lesson 3: LED 制御 — システム状態を LED で可視化 StampFly Workshop Lesson 2: コントローラ入力 12 / 12
今日のゴール / Today’s Goal 目標 LED でシステムの状態（ARM / DISARM）とバッテリー残量を可視 化する WS2812 アドレサブル LED の制御 状態遷移の考え方（ステートマシン） バッテリー電圧のモニタリング StampFly Workshop Lesson 3: LED 制御 2/7
LED 状態遷移 / LED State Machine is_armed() == true Power ON DISARM ARM Green Battery gradient 3.3 V = Red → 4.2 V = Green is_armed() == false led_color(0, 255, 0) led_color(r, g, 0) StampFly Workshop Lesson 3: LED 制御 3/7
LED 制御 API 関数 disable_led_task() led_color(r, g, b) is_armed() battery_voltage() 説明 システム LED 更新を無効化 LED 色設定 ARM 状態確認 バッテリー電圧 引数 — 各 0–255 true / false 3.0–4.2 V WS2812 LED RGB フルカラー（各チャンネル 8bit = 1677 万色） 1 本のデータ線で制御（ESP32 RMT ペリフェラル使用） led_color() は裏表両方の LED を同じ色に設定 StampFly Workshop Lesson 3: LED 制御 4/7
バッテリー電圧と色 / Battery Voltage & Color 3.3 V 危険 3.75 V 4.2 V 満充電 LiPo バッテリー: 3.3V（空）〜 4.2V（満充電） 3.3V 以下で使用禁止！ バッテリーが損傷します StampFly Workshop Lesson 3: LED 制御 5/7
実習: ARM 状態で LED 色を変える / Hands-on
# include "workshop_api.hpp"
void setup () {
3
ws:: print("Lesson 3: LED Control");
4
ws:: disable_led_task (); // Student LED control
5 }
6 void loop_400Hz(float dt) {
7
if (ws:: is_armed ()) {
8
ws:: led_color (0, 255, 0);
// Green
9
} else {
10
// Battery level as color gradient
11
float v = ws:: battery_voltage ();
12
float level = (v - 3.3f) / (4.2f - 3.3f);
13
if (level < 0) level = 0;
14
if (level > 1) level = 1;
15
uint8_t r = 255 * (1.0f - level );
16
uint8_t g = 255 * level;
17
ws:: led_color(r, g, 0);
18
}
19 }
1
2
user_code.cpp
StampFly Workshop
Lesson 3: LED 制御
6/7
チェックポイント / Checkpoint 確認事項 DISARM 時に赤〜緑のグラデーション表示 ARM すると緑に変わる バッテリー残量に応じて色が変化する 次のレッスン Lesson 4: IMU センサー — 加速度・ジャイロで姿勢を知る StampFly Workshop Lesson 3: LED 制御 7/7
今日のゴール / Today’s Goal 目標 IMU（ジャイロ+加速度）データを読み取り、シリアルモニタと WiFi テレメトリで確認する NED 座標系（北-東-下）の理解 ジャイロスコープで角速度を取得 加速度センサで並進加速度を取得 WiFi テレメトリでデータ取得・可視化 StampFly Workshop Lesson 4: IMU センサ 2/8
NED 座標系 / NED Coordinate System X (Forward) Roll Pitch Y (Right) Yaw Z (Down) BMI270 IMU — 6 軸（3 軸ジャイロ + 3 軸加速度）400 Hz ロ正方向（右手の法則） StampFly Workshop Lesson 4: IMU センサ | 回転矢印 = ジャイ 3/8
IMU API 関数 gyro_x/y/z() accel_x/y/z() 説明 角速度 (Roll/Pitch/Yaw) 加速度 (X/Y/Z) 単位 rad/s m/s2 静止時の値 ジャイロ ≈ 0、加速度 Z ≈ 9.81 m/s2 （重力） StampFly Workshop Lesson 4: IMU センサ 4/8
実習: 6 軸読み取り + シリアル表示 / Hands-on
# include "workshop_api.hpp"
static uint32_t tick = 0;
3 void setup () {
4
ws:: print("Lesson 4: IMU Sensor");
5 }
6 void loop_400Hz(float dt) {
7
tick ++;
8
float gx = ws:: gyro_x ();
9
float gy = ws:: gyro_y ();
10
float gz = ws:: gyro_z ();
11
float ax = ws:: accel_x ();
12
float ay = ws:: accel_y ();
13
float az = ws:: accel_z ();
14
// Print every 100 ms (40 ticks)
15
if (tick % 40 == 0) {
16
ws:: print("G=(%.3f ,%.3f ,%.3f) A=(%.2f ,%.2f ,%.2f)",
17
gx , gy , gz , ax , ay , az);
18
}
19 }
1
2
user_code.cpp
StampFly Workshop
Lesson 4: IMU センサ
5/8
WiFi テレメトリ受信 / Receiving WiFi Telemetry システムが IMU・姿勢・センサデータを自動的に 400 Hz で送信しています 手順 1 2 3 シリアルモニタで SSID を確認（StampFly_XXXX） PC の WiFi で StampFly に接続（IP: 192.168.4.1） sf log wifi -d 30 で 30 秒キャプチャ → CSV 自動保存 出力例 logs/stampfly_wifi_20260303T143000.csv に保存されました -o name.csv でファイル名指定、--no-save で保存なし（統計のみ） StampFly Workshop Lesson 4: IMU センサ 6/8
データの可視化 / Data Visualization 保存した CSV を可視化 sf log viz logs/stampfly_wifi_*.csv モード --mode all --mode sensors --mode attitude 表示内容 全パネル（デフォルト） IMU + 高度センサ 姿勢推定（Roll/Pitch/Yaw） --save plot.png でファイル保存可能 StampFly を手で揺らしながらキャプチャし、ジャイロの変化を グラフで確認しよう StampFly Workshop Lesson 4: IMU センサ 7/8
チェックポイント / Checkpoint 確認事項 シリアルモニタでジャイロ・加速度の値を確認できる 静止時に accel_z ≈ 9.81 を確認 sf log wifi でセンサデータを受信できる sf log viz でデータをグラフ表示できる 次のレッスン Lesson 5: レート P 制御 — ジャイロフィードバックで初フライト StampFly Workshop Lesson 4: IMU センサ 8/8
Lesson 5: レート P 制御 + 初フライト Rate P-Control + First Flight StampFly Workshop StampFly Workshop Lesson 5: レート P 制御 + 初フライト 1/7
今日のゴール / Today’s Goal 目標 比例フィードバック制御を実装し、初の制御飛行を行う 閉ループ制御の考え方 P 制御で角速度を安定化 ARM/DISARM による安全管理 StampFly Workshop Lesson 5: レート P 制御 + 初フライト 2/7
閉ループ制御 / Closed-Loop Control + Target Rate r Σ e P Control Kp × e u Motor Mixer StampFly Drone angular rate response − Gyro Sensor feedback StampFly Workshop Lesson 5: レート P 制御 + 初フライト 3/7
制御 API 関数 gyro_x/y/z() rc_roll() rc_pitch() rc_yaw() rc_throttle() motor_mixer(T,R,P,Y) is_armed() StampFly Workshop 説明 ジャイロ角速度 ロールスティック ピッチスティック ヨースティック スロットル モーターミキサー ARM 状態確認 Lesson 5: レート P 制御 + 初フライト 値域 rad/s −1.0 ∼ +1.0 −1.0 ∼ +1.0 −1.0 ∼ +1.0 0.0 ∼ 1.0 — true/false 4/7
実習: 3 軸 P 制御 / Hands-on
# include "workshop_api.hpp"
void setup () { ws:: print("Lesson 5: Rate P-Control"); }
3 void loop_400Hz(float dt) {
4
if (!ws:: is_armed ()) {
5
ws:: motor_stop_all ();
6
ws:: led_color (50, 0, 0); // Red = DISARM
7
return;
8
}
9
ws:: led_color (0, 50, 0); // Green = ARM
10
float Kp_rp = 0.5f, Kp_yaw = 2.0f;
11
float rate_max = 1.0f, yaw_max = 5.0f;
12
float roll_e = ws:: rc_roll ()* rate_max - ws:: gyro_x ();
13
float pitch_e = ws:: rc_pitch ()* rate_max - ws:: gyro_y ();
14
float yaw_e
= ws:: rc_yaw ()* yaw_max
- ws:: gyro_z ();
15
ws:: motor_mixer(ws:: rc_throttle (),
16
Kp_rp*roll_e , Kp_rp*pitch_e , Kp_yaw*yaw_e );
17 }
1
2
user_code.cpp
StampFly Workshop
Lesson 5: レート P 制御 + 初フライト
5/7
安全注意 / Safety フライト前チェック 保護メガネを着用 低スロットルから徐々に上げる（最初は 30% 以下） プロペラの回転方向を確認（M1=CCW, M2=CW, M3=CCW, M4=CW） 異常時はすぐにスロットルを下げて DISARM StampFly Workshop Lesson 5: レート P 制御 + 初フライト 6/7
チェックポイント / Checkpoint 確認事項 ARM 後スロットルを上げて安定ホバリング スティック操作で機体が応答する DISARM で即座にモーターが停止する 次のレッスン Lesson 6: システムモデリング — 伝達関数からゲインを設計 StampFly Workshop Lesson 5: レート P 制御 + 初フライト 7/7
Lesson 6: システムモデリング System Modeling StampFly Workshop StampFly Workshop Lesson 6: システムモデリング 1 / 13
今日のゴール / Today’s Goal 目標 プラント伝達関数を導出し、モデルに基づいて P ゲインを設計する 1 2 3 モータ+機体のプラント伝達関数 Gp (s) を理解 P 制御の閉ループ特性（ωn , ζ ）を導出 ζ から Kp を設計 — L05 の初フライト経験をモデルで裏付け StampFly Workshop Lesson 6: システムモデリング 2 / 13
物理メカニズム / Physical Mechanism duty Motor Kv τm s + 1 d [rad/s] ωm PWM → motor speed linearize at hover: [N] Propeller 2 F = Ct ωm rotation → thrust [N·m] F Geometry ×4L or ×4κ differential thrust → torque Km , Kf = 2Ct Kv ωm,h , τm s + 1 τ Rigid Body 1 I ·s [rad/s] ω torque → angular rate Km = 4L · Kf duty 信号がどうやって機体の角速度になるか — 4 つのステージ StampFly Workshop Lesson 6: システムモデリング 3 / 13
プラントモデル（簡略化）/ Plant Model (Simplified) control input u Mixer + Motor Km τm s + 1 τ angular rate ω rigid-body rotation actuator dynamics Gp (s) = Body 1 I ·s K , s(τm s + 1) K= Km I ホバー近傍で線形化 → 2 ブロックに集約 Mixer + Motor: duty → トルク— 1 次遅れ Km /(τm s + 1) Body: トルク → 角速度— 積分器 1/(I · s) StampFly Workshop Lesson 6: システムモデリング 4 / 13
τm と Km の導出 / Deriving τm and Km モータ時定数 τm DC モータの電気 · 機械モデルから: Jmp · Rm τm = 2 Ke + Dm Rm Jmp Rm Ke Dm τm 2.01e-8 kg·m2 0.34 Ω 6.13e-4 V/(rad/s) 3.69e-8 0.018 s ≈ 0.02 s 回転子慣性 巻線抵抗 逆起電力定数 粘性摩擦 実効プラントゲイン: StampFly Workshop 実効トルクゲイン Km Kf : ホバー点で duty を ∆d 変えたときの 1 モータ推力変化量 ( dF 4L · Kf Roll/Pitch Kf = , Km = dd hover 4κ · Kf Yaw Kf L κ Km,rp Km,yaw 0.010 N/duty 0.023 m 9.71e-3 m 9.3e-4 N·m 3.9e-4 N·m 同定値 アーム長 トルク推力比 Roll/Pitch Yaw K = Km /I (Kroll = 102, Kpitch = 70, Kyaw = 19 rad/s2 ) Lesson 6: システムモデリング 5 / 13
パラメータ一覧 / Parameter Summary パラメータ 慣性モーメント トルクゲイン モータ時定数 プラントゲイン 記号 Roll Pitch Yaw I 9.16e-6 13.3e-6 20.4e-6 kg · m2 Km 9.3e-4 9.3e-4 3.9e-4 N·m τm 0.02 s（共通） K=K m /I 102 70 19 rad/s2 なぜ軸ごとに K が違う？ Roll/Pitch: 同じ Km だが Iyy > Ixx → Pitch の方が遅い Yaw: κ L なので Km 自体が小さい → さらに遅い StampFly Workshop Lesson 6: システムモデリング 6 / 13
P 制御の閉ループ / P Control Closed Loop + r (t) Σ e(t) Controller Kp u Plant Gp (s) ω(t) − Gcl (s) = Kp K τm s 2 + s + Kp K 閉ループ伝達関数 Gcl (s) = Kp K τm s 2 + s + Kp K StampFly Workshop ←− 2 次系！ Lesson 6: システムモデリング 7 / 13
2 次系の特性 / Second-Order Characteristics 固有振動数と減衰比 r ω 16% ωn = ζ = 0.5 ζ = 0.7 Kp K , τm 1 ζ= p 2 Kp K τm target 設計式: ζ から Kp を逆算 ζ = 1.0 Kp = t 1 4ζ 2 K τm オーバーシュート量 ≈ e −πζ/ 1−ζ (ζ = 0.5 → 16%, ζ = 0.7 → 5%) p StampFly Workshop Lesson 6: システムモデリング 2 8 / 13
開ループボード線図 / Open-Loop Bode Plot |L(jω)| [dB] 60 40 20 1 = 50 τm Kp = 0.5 Kp−= 0.25 2 開ループ伝達関数 0 dB /dec L(s) = Kp · Gp (s) = ωgc = 40 0 −4 0 ωgc = 23 −20 dB /d e c −40 位相余裕 PM −60 ∠L(jω) [deg] Kp K s(τm s + 1) ωgc : |L(jωgc )| = 1 となる周波数 −90 −120 −150 PM=65° PM=51° −180 100 101 102 ω StampFly Workshop [rad/s] PM = 90° − arctan(τm ωgc ) 103 Kp を上げると ωgc ↑ → PM ↓ → 振動的に Lesson 6: システムモデリング 9 / 13
無駄時間の影響 / Dead Time Effect |L| [dB] 制御ループの無駄時間 40 0 −40 −60 ∠L(jω) [deg] −90 τd ≈ 5 ms（センサ処理 + 制御演算 + PWM 更新） |e −jωτd | = 1 gain unchanged No delay τd = 5 ms Ldelay (s) = L(s) · e −τd s −120 51° −150 ∆PM=−11° 40° −180 −210 −240 100 Unstable region 101 ω Kp = 0.5 Kp = 0.25 モデル PM=51° PM=65° 実機 PM≈40° PM≈59° 60° 未達! ギリギリ 102 [rad/s] 実用上 PM ≥ 60° が必要 ζ = 0.7 設計でも遅延込みでボーダーライン→「モデル上は安定」 でも実機で振動する理由 StampFly Workshop Lesson 6: システムモデリング 10 / 13
ゲイン設計表 / Gain Design Table K Kp = 0.5 の ζ ζ = 0.7 の Kp ζ = 1.0 の Kp Roll 102 0.50 0.25 0.12 Pitch 70 0.60 0.36 0.18 Yaw 19 1.15 1.34 0.66 読み取り L05 の Kp = 0.5 は Roll で ζ = 0.50（やや振動的） Yaw は ζ > 1（過減衰 = 応答が遅い）— 軸ごとに Kp を変える べき理由！ StampFly Workshop Lesson 6: システムモデリング 11 / 13
実習: モデルベースゲイン設計 / Hands-on // Model -based Kp design ( target zeta = 0.7) const float tau_m = 0.02f; 3 const float K_roll = 102.0f; 4 const float K_pitch = 70.0f; 5 const float K_yaw = 19.0f; 6 float zeta = 0.7f; 1 2 7 float Kp_roll = 1.0f/(4* zeta*zeta*K_roll *tau_m ); float Kp_pitch = 1.0f/(4* zeta*zeta*K_pitch*tau_m ); 10 float Kp_yaw = 1.0f/(4* zeta*zeta*K_yaw *tau_m ); 8 9 11 // Apply per -axis Kp in control loop float roll_cmd = Kp_roll * (target_roll - ws:: gyro_x ()); 14 float pitch_cmd = Kp_pitch * (target_pitch - ws:: gyro_y ()); 15 float yaw_cmd = Kp_yaw * (target_yaw - ws:: gyro_z ()); 12 13 user_code.cpp (excerpt) StampFly Workshop Lesson 6: システムモデリング 12 / 13
チェックポイント / Checkpoint 確認事項 Gp (s) = K /(s(τm s + 1)) を説明できる モデルベース Kp と L05 の Kp = 0.5 を比較飛行 Roll/Pitch/Yaw の ζ 差を体感で理解 次のレッスン Lesson 7: システム同定 — フライトデータでモデルを検証 StampFly Workshop Lesson 6: システムモデリング 13 / 13
Lesson 7: システム同定 System Identification StampFly Workshop StampFly Workshop Lesson 7: システム同定 1 / 10
今日のゴール / Today’s Goal 目標 フライトデータからプラントモデル Gp (s) = タ K , τm を同定する K のパラメー s (τm s + 1) システム同定（SysID）の考え方 WiFi テレメトリでデータ取得 sf sysid fit でモデルフィッティング 同定値と L6 理論値を比較 StampFly Workshop Lesson 7: システム同定 2 / 10
システム同定とは / What is System Identification? control input u(t) Plant Gp (s) (unknown K , τm ) measured output y(t) Parameter Estimation K̂ , τ̂m compare with L6 theory StampFly Workshop Lesson 7: システム同定 3 / 10
WiFi テレメトリ / WiFi Telemetry WiFi テレメトリ（自動送信） システムが IMU・姿勢・センサデータを 400 Hz で自動送信 sf log wifi で受信 → CSV 保存 → sf log viz で可視化 コマンド 説明 sf log wifi WiFi テレメトリ取得（CSV 保存） sf log wifi -o data.csv ファイル名指定で保存 sf log analyze data.csv フライトデータ分析 sf log viz data.csv 波形可視化 StampFly Workshop Lesson 7: システム同定 4 / 10
プラント入出力の復元 / Reconstructing Plant I/O なぜ開ループ同定が可能? テレメトリの記録内容 ctrl_roll スティック入力 gyro_x 角速度（計測値） 復元手順（Kp 既知） 1 target = ctrl × rate_max 2 u(t) = Kp ×(target − gyro) 3 y(t) = gyro StampFly Workshop Kp が既知なら、プラントへの入力 u(t) を計算できる。閉ループデータでも、開 ループモデルを直接フィッティングで きる。 パラメータ Kp rate_max Lesson 7: システム同定 値 L5 の値（例: 0.5） 1.0 rad/s 5 / 10
フィッティングの仕組み / How Fitting Works 最小二乗法によるパラメータ推定 候補の K , τm でモデル出力 ŷ(t) をシミュレーションし、実測 y(t) との誤差が最小 になる K , τm を探す。 Gp (s) 1 復元した u(t) をモデルに入力: u(t) −−−−→ ŷ(t) 2 P J(K , τm ) = ŷ(t) − y(t) 2 実測 y(t) との二乗誤差を計算: 3 J を最小化する K , τm を数値最適化で求める sf sysid fit が自動で行うこと ホバリング区間の自動検出 データを短いセグメントに分割してフィッティング 結果の統計処理（中央値・不確かさ） StampFly Workshop Lesson 7: システム同定 6 / 10
実習: データ取得 / Hands-on: Data Acquisition L5 の P 制御で飛行し、テレメトリデータを取得する 同定には Kp と rate_max の値を記録しておくこと 1 2 3 4 5 1 sf lesson switch 7 でテンプレートをコピー user_code.cpp に Kp （例: 0.5）をセット ビルド & 書き込み: sf lesson build → sf lesson flash 離陸し、スティック操作でロール・ピッチ入力を入れる PC でデータ受信: sf log wifi -o flight.csv ターミナル ポイント: 2〜3 回のスティック操作で十分。ホバリング中に行うこと。 StampFly Workshop Lesson 7: システム同定 7 / 10
sf sysid fit / Model Fitting Tool コマンド sf sysid fit flight.csv --kp 0.5 --plot 1 2 3 4 5 6 7 === Plant Model Identification === Model: G_p(s) = K / (s * (tau_m * s + 1)) Roll Pitch K = 98.5 (ref: 102.0 , err: 3.4%) tau_m = 0.019 (ref: 0.020 , err: 5.0%) K = 72.1 (ref: 70.0, err: 3.0%) tau_m = 0.021 (ref: 0.020 , err: 5.0%) R2 = 0.94 R2 = 0.91 出力例 --kp 0.5 --axis roll --rate-max 1.0 -o result.yaml StampFly Workshop 飛行時の Kp （必須） 特定軸のみ分析 rate_max（roll/pitch 既定=1.0, yaw=5.0） 結果をファイルに保存 Lesson 7: システム同定 8 / 10
モデル検証 / Model Verification 軸 Roll Pitch Yaw K (同定) ? ? ? K (L6 理論) 102.0 70.0 19.0 設計 Kp の計算（ζ = 0.7） Kp = τm (同定) ? ? ? τm (理論) 0.020 0.020 0.020 R2 ? ? ? 1 4 ζ 2 K τm 考察 同定値と理論値のずれ → モデル誤差・無駄時間が原因 同定 K , τm で設計した Kp は L6 の理論値と近いか? StampFly Workshop Lesson 7: システム同定 9 / 10
チェックポイント / Checkpoint 確認事項 sf log wifi でフライトデータを取得できた sf sysid fit で K , τm を同定した 同定値と L6 理論値を比較した 次のレッスン Lesson 8: PID 制御 — モデルに基づく I/D 項の追加で制御を改善 StampFly Workshop Lesson 7: システム同定 10 / 10
今日のゴール / Today’s Goal 目標 モデルベースの Kp を起点に、I 項・D 項を追加して制御を改善する P 制御の限界: モデル不確かさ・外乱に弱い 工学形式 Kp /Ti /Td とは 不完全微分: ノイズに強い微分項 ログから調整する方法 StampFly Workshop Lesson 8: PID 制御 2 / 10
PID 制御器 / PID Controller P: Kp · e e(t) I: Kp Ti D: Kp · C (s) = Kp StampFly Workshop Z e dt Proportional + Integral Σ + u(t) + Td s ηTd s + 1 Incomplete Derivative 1 Td s 1+ + Ti s ηTd s + 1 Lesson 8: PID 制御 3 / 10
なぜ不完全微分か / Why Incomplete Derivative? 問題 解決策 ジャイロにはノイズがある 理想微分 de dt はノイズを増幅 高周波振動 → モータに悪 影響 不完全微分フィルタで高周 波カット η = 0.1 ∼ 0.2 小 → フィルタ弱い 大 → フィルタ強い トレードオフ 微分の効き（応答速度）←→ ノイズ除去（安定性） StampFly Workshop Lesson 8: PID 制御 4 / 10
工学形式パラメータ / Engineering Form パラメータ Kp Ti 意味 比例ゲイン 積分時間 [s] Td 微分時間 [s] η フィルタ係数 効果 大 → 応答速い、大きすぎ → 振動 小 → 積分強い（偏差早く消える） 、小 さすぎ → ワインドアップ 大 → 微分強い（振動抑制）、大きす ぎ → ノイズ増幅 0.1 ∼ 0.2、大 → ノイズに強い 教科書形式との対応 Ki = Kp /Ti , Kd = Kp · Td StampFly Workshop Lesson 8: PID 制御 5 / 10
ログから調整する / Log-Based Tuning ログで見える症状 振動が収まらない 応答が遅い 定常偏差が残る オーバーシュート大 高周波ノイズ ゆっくり発散 原因 Kp 過大 Kp 過小 I 項不足 D 項不足 η 過小 ワインドアップ 調整 Kp ↓ Kp ↑ Ti ↓（積分強化） Td ↑（微分強化） η ↑（0.1 → 0.2） Ti ↑ ヒント L7 で取得したテレメトリデータを見ながら調整する StampFly Workshop Lesson 8: PID 制御 6 / 10
推奨ゲイン / Recommended Gains 軸 Kp Ti [s] Td [s] η Roll 0.25 1.67 0.01 0.125 Pitch 0.36 1.67 0.01 0.125 Yaw 1.34 4.0 0.005 0.125 Kp は L6 のモデルベース設計値（ζ = 0.7）を使用 チューニング手順 1 2 3 Ti =0, Td =0 にして Kp を調整（振動しない最大値） Ti を大きい値 (5.0) から徐々に下げる Td を小さい値 (0.001) から徐々に上げる StampFly Workshop Lesson 8: PID 制御 7 / 10
実習 Step 1: 理想 PID / Ideal PID float Kp =0.25f, Ti =1.67f, Td =0.01f; float integral =0, prev_err =0; 3 // inside loop_400Hz (dt) 4 float error = target - ws:: gyro_x (); 5 float P = Kp * error; 6 if (Ti > 0) // I term ( trapezoidal ) 7 integral += (dt /(2* Ti)) * (error + prev_err ); 8 float I = Kp * integral; 9 float D = Kp*Td*( error - prev_err )/dt; // ideal D 10 prev_err = error; 11 float roll_out = P + I + D; 1 2 user_code.cpp (roll axis excerpt) やってみよう 飛ばしてみて → 高周波振動に注目 StampFly Workshop Lesson 8: PID 制御 8 / 10
実習 Step 2: 不完全微分 / Incomplete Derivative // Replace ideal D with incomplete derivative filter float eta = 0.125f; 3 float d_filt = 0; // persistent state 4 // inside loop_400Hz (dt) 5 float alpha = 2*eta*Td / dt; 6 float a = (alpha - 1) / (alpha + 1); 7 float b = 2*Td / (( alpha + 1) * dt); 8 d_filt = a * d_filt + b * (error - prev_err ); 9 float D = Kp * d_filt; // Filtered ! 1 2 user_code.cpp (D term replacement) やってみよう 再び飛ばして → 高周波振動が減ったことを確認 StampFly Workshop Lesson 8: PID 制御 9 / 10