電子工作でドローンを飛ばしたい！
子供のおもちゃのドローンしか触ったことのない素人が、自作でドローンを作るまでの物語です。たくさん失敗してます。みてやってください。まだ完成していません！！！
これから始めようと思っている方に見ていただければ幸いです！
目次
なぜ自作ドローンか
単なる趣味です！！
何年も前から電子工作やってみたい！と画策しており、PICマイコンやAtmelマイコン等の下調べをやっていた。AtmelAVRでやっていこうと、参考書購入まではやっていたのだが、その後、ライターや実際のマイコン購入までは進まず、ただただ時間が流れていった。
そうこう考えている中で、ドローンを飛ばせたら凄そう！という単なる興味というか、純粋なチャレンジ精神というか、空を飛ぶという見た目の華やかさもあって、是非ドローンを作りたい！という気持ちが育っていったのです。
改めて、2022年より実際に活動開始、再調査。arduino russberypieなどが有名と判明。arduinoのマイコンはAtmelを使用していることが分かり、用途や情報量の多さもありarduinoを購入。実際にはarduinoの模造品（ELEGOO UNO R3）だが、機能は本家と全く一緒で、動作も使っている限り問題なし。
9軸センサとESCとモータの入手まではやったけど、9軸センサの動作確認で躓いて、その後個人的他用で手を付けず3年ほど経過 ～ 改めて製作を決意し、なるべくサクッと飛行させたい。
とにかく飛ばす！と思いつくままに走り出し、走りながら考える。この記録です。
途中Bluetooth機能を使う必要が出てきて、arduino→EPS32に乗り換えました。
ドローン製作のために買った部品
もっと安上がりに済ませる予定というか、皮算用だったけど。まあまあ部品、高いですね。結果的に結構な出費になりました。ドローンを単に飛ばしたい（ラジコンしたい）だけなら、絶対に！既製品を買ったほうが安いので、これからと考えている方は覚悟をお願いします。
ドローンフレーム
ドローン本体（フレーム）のGEP-CX3は新古品を入手（大きさはなるべく小さく、電子部品を搭載可能のサイズで選定）、とりあえずなんでもよかった。オークションで1000円台くらいだったと思う。とりあえず部品がつけばなんでもよかった。
ドローンアクセサリ シグネット GEP-CX3 14フリースタイルのための5mm 3インチのカーボンファイバーフレームラック DIY FPVレーシングドローンクアドコプター
モータとモータコントローラとプロペラはYahooショッピングで安かったものを選んだ。飛行性能についてはわからないのでとにかく組み立てることを優先。
ブラシレスモータ
ブラシレス1-2S
4300KVは1Vで１分間に4300回転（4300rpm）することを表しています。この数値が高いほど高回転になりますが、トルクは低くなります。逆にKV値が低いと最高回転数は低くなりますが、得られるトルクは大きいです。
ブラシレスモータの４桁の数字は下記のようになるらしいです。
例）1104
最初の「１１」・・・モータの中のステーター鉄心の直径(mm)
後ろの「０４」・・・モータの中のステーター鉄心の高さ(mm)←コイルを巻いている各鉄心の高さ
つまり、モータの直径と、内部鉄心の大きさを示す数字なので、実際のモータ外形はステータ直径よりやや大きいものとなり、モータの厚みや軸寸法は全く示していません。つまり、各モータの外形や取り付け寸法、軸寸法はそれぞれのメーカー毎によく確認する必要がありそうです。
＜参考＞ドローン用ブラシレスモータの数字の意味？
ミニ 5.5グラム1104 4300KV 2 ブラシレス モーター 2-4s 2-3インチfpv レーシング rc ドローン quad（ヤフーショッピングで購入、リンク切れ）
マイクロモーター 4300kVミニ1104ブラシレスモーター1S-2S ESC 6A-12A DC高速 ブドーリーマイクロ FPV屋内トラバース飛行機 RCドローンQuadcopter 機械工具
Aliで安いところで400円弱、普通600円～1000円／個程度。この辺が一番安い！
ブラシレス2-4S
バッテリーを誤って3S購入したため、モータも買い替える必要が出て……
高いけど、そのなかでも一番安いものを選んで買いました。いろいろ紆余曲折あって出費がかさんでいます。
Aliセールで4個で4232円、モー最初からこの辺買っときゃよかった。最初の設計時に使用するバッテリー電圧を決めておくことは重要です！
Ysido-レーシングブラシレスfpvモーター,フリースタイルつまようじドローン,元の交換部品,1404,3850kv,4650kv,4個
素性の知れたモータなら、データシートも完備されているんですね！少し感動
プロペラ
kingkong製の2345サイズ（直径2.3インチ、ピッチ4.5インチ）３パドルのプロペラでモータサイズ1104に対応しているようなので購入。
10ペアkingkong 2345 2.3インチ プロペラ 3-Bladed パドル babyhawkためET115 1104 1106（リンクなし）
20 個 Kingkong 2345 58mm PC 3 ブレード プロペラ 1.5mm 取り付け穴 FPV クアッドコプター RC レーサー ドローン用 (10 ペア) (Color : Y)
プロペラガード（追加購入）
PID制御のゲイン設定中、ガードがないとケガします。追加購入Aliexpressで1000円弱。ものの割には一番高くついてる気がする。。。
HGLRC 2.5 Inch Propeller Guard for RC FPV Racing Drone [MA-8033]
ESC（モータコントローラ）
FPV Racing ドローンレース用品 BLHeli_S 4in1 ESC 30A 2-6s (Max40A) Dshot600可（リンク無）
Yahooショッピングで4000円くらいだったと思う。
姿勢制御（6軸／９軸）センサー
モーションセンサーはMPU9250を購入（Amazonで安かったから）←これはだましだった。評価欄で間違いなく９軸センサーであることを確認して購入しましょう。下は、９軸と謳いながら６軸であったと評価欄に記載ある例です。
Ahvqevn MPU-9250-9250 9 軸センサー モジュール/SPI 通信 三軸ジャイロスコープ + 三軸加速度計
バッテリー関連
タミヤバッテリー2S（7.2V）
最初、手持ちのタミヤバッテリーを使っていましたが、240ｇもあって、ドローンが浮かない！なので軽量バッテリーＰＩＰＯへ手をだすことに。
ドローン用LiPoバッテリー3S（11.4V）
Aliで2500円強だった。一応素性のしれたメーカーを選定（ＨＰがある）。もっと安いのもあったけど、ドローン用（軽い？66ｇ）だし、変なの買って火事になったらしゃれにならねー。
パワーを求めて3Sを買ったけど、購入後に気づいた！モータが1-2S対応で、電圧オーバーしてる！痛恨のミス！！
1000円強～2000円弱で2Sのものも買えるんだけど、使わないLIPO電池を持つのは嫌だし、管理できないし。。。。モータを変えるか！？→3Ｓ対応のモータを購入することに。。。
GAONENG GNB LiHV 3S 11.4V 1100mAh 60C XT30 LiPo Battery LongRange
充電器はＹＺ１１４ＳＰ
何年も前に、RCカーバッテリー充電のためにオークションか何かで入手。2-3000円くらいだったか？
タミヤラジコンカーのバッテリーはニッカドで決まり！？の記事を書いたころ。
セルメータ、バランス放電ができるので購入。１１７０円ALI
LIPO保管用、怖いので。３７０円Ali
電子部品関係
ESP32（WROOM-32D 30pin）
ESP32は安くて250円、通常５６０円くらい。
MPU6500
MPU9250として買ったけど、結局MPU6500だった。Aliで160円。
その他
タミヤコネクタオスメス、XT30コネクタメス（買ったバッテリとつなぎたかったけど、バッテリ側がメスだったので間違い）やM1.6ビスセット（モータ固定用に本来はM1.4が必要だった、なぜ？間違い）、各々200-300円程度で他の部品に比べると、割高で結構お金かかってますね。間違いが多い！
はんだはArduinoUNOを入手した初期ごろに秋月で購入してた。はんだ台も購入済み。
はんだじゅっぽん。150円Aliで入手。
うまくはんだ付けできないので。750円くらいAli。磁石で固定する他のクリップ類も何本か+α250円程度。
なんとベースがアルミのようで磁石はくっつかず。少し失敗でした。
ドローン製作のステップ
まずは、aruduinoを使ってみる。使い方についてはググればたくさん情報がでてきます。
ドローン製作のための主なステップは以下となります。
- 姿勢制御センサーのデータをaruduinoへ取り込む
- 姿勢制御センサーのデータを制御可能な単位へ変換
- ブラシレスモータをaruduinoで回転させる（ESCをコントロール）
- 姿勢制御センサーでモータを制御して安定飛行させる
姿勢制御センサーのarduino取り込み
接続図
| Arduino UNO R3 | MPU6500 |
|---|---|
| 3.3V | VCC |
| GND | GND |
| SCL（A5） | SCL |
| SDA（A4） | SDA |
偽9軸センサー(最終的に3軸)の動作確認に超苦戦（3年越し）
購入したものが、MPU9250の9軸センサーと思っていろいろ試す。
6軸を読んでみる→加速度は出るが、角速度がでない。いろいろ試すもNG
翌日、突然角速度がでるようになる。
続いて、地磁気を取得試みるもでない。数日頑張るもでない。
必ずWho am Iでセンサーをチェックしよう
結果的には、Who am Iを試して、アドレス0x75レジスタを読みだして、0x70が返ってきたのでMPU6500（6軸センサ）の偽物でした。このMPU6500もはじめ角速度が出なかったことを考えると、動作怪しいなぁ…。＜ここから３年他用で手をつけず＞
#include <Wire.h>
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
void setup() {
Serial.begin(9600);
Wire.setClock(400000);
Wire.begin();
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
// WHO_AM_I Register
Wire.write(0x75);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 1, true);
if (Wire.available()) {
byte c = Wire.read();
Serial.print("WHO_AM_I = 0x");
Serial.println(c, HEX);
} else {
Serial.println("Failed to read WHO_AM_I");
}
}
void loop() {
}
故障したMPU6500にもてあそばれる
その後、角速度が全軸表示されない（全てゼロ）、加速度はとれているけどよくある不良のようです。ちなみに温度の数値も-32768で飽和？たぶん壊れてる。
みなさん、お安い中華輸入の電子部品はまとめ買いせず、一点一点確かめて購入しないといけないようです。
⇛その後、MPU6500リセット操作で角速度とれるようになりました。多分色々なスケッチを片っ端から試している間に、変な設定を書き込んで、角速度が出てこないような状況になっていたのだと予想してます。
更にその後、角速度が出ているときは、Who am I の値が0x75となっていることが判明。角速度っぽい値がとれている（動きにあわせて数字が反応）ときもあるが、リセットするたびに基準点が変わるし、このとき加速度は飽和したりと、全く信頼性のないデータがとれる状況。
MPUリセット安定待ち100msを入れれば、元の加速度3軸のみとれる故障MPU6500の挙動に戻ります。（加速度数値は安定）現象が整理できるまでに数週間かかってしまいました。うまく数値が取得できないときは故障と思っていいかもしれません。
リセット安定を待たず起動すると、センサデータが不安定表示される原因は不明ですが、状況は把握でき、このパチものMPU6500は結局のところ加速度3軸しかとれないことがわかりました。
MPU6500のデータ取得スケッチ
その後、再購入でMPU9250を入手。しかし、Who Am Iしてみると、0x70でMPU6500だと判明。もちろん地磁気も読み取れず。一応角速度はきちんと動いているのでMPU6500を手に入れたと思ってあきらめる。安いベンダーから購入する際は大量購入しないでください。偽物の可能性もあります。でも１６０円だったので仕方ない、あきらめます。
#include <Wire.h>
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
void setup() {
Serial.begin(9600);
Wire.setClock(400000);
Wire.begin();
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
}
void loop() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
float temp = (temperature / 333.87f) + 21.0f; // データシートより
Serial.print(axRaw); Serial.print(",");
Serial.print(ayRaw); Serial.print(",");
Serial.print(azRaw); Serial.print(",");
Serial.print(temp); Serial.print(",");
Serial.print(gx_dps); Serial.print(",");
Serial.print(gy_dps); Serial.print(",");
Serial.println(gz_dps);
delay(100);
}
モーションセンサ参考サイト
6軸センサーの設定参考サイト
Arduinoから MPU6050の値を取得してみる (生値の取得方法)
【Arduino / MPU-6500】 I²C を一通り学びつつ、さっさと実装する
Arduino 入門 Lesson 32 【６軸モーションセンサ編 その２】 （内容が詳しい、単位変換まであり）
地磁気センサーの設定参考サイト
Arduinoで9軸センサー（加速度・ジャイロ・地磁気）を使ってみる
第5回 Arduino入門 I2C通信編（Wireシリアル通信方法）MPU9250.h？？
ArduinoでMPU9250（加速度センサ、磁気センサ）を使う方法（ライブラリでサクッと地磁気まで取得する方法）
Madgwickフィルタでyaw,pitch,rollを取得
躓いた点
加速度は単位は関係ないが、角速度は単位換算を正確にしないといけないのだけど、角速度の重みが間違っていて変な表示になった。
MPU6500を逆向き（おそらくZ軸が逆さ）にして配線しており、yaw,pitch,rollが変な値に収束した。キャリブレーションできていなかったのも原因かもしれない。
シリアルモニタが停止してしまった。サンプリング時間がオーバーフローしてloop実行されなくなったのが原因。サンプリング時間オーバーするようなら、サンプリング時間を変更しないといけない。とりあえず、シリアルで速度入力する部分が1000ms必要のようで、サンプリング時間外に出した。数値入力のタイミングではサンプリング時間で計算できないが、テストなので良しとする。
Madgwickフィルター設定の参考サイト
#include <MadgwickAHRS.h>
#include <Wire.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
// Timers
unsigned long timer = 0;
float timeStep = 0.01;//MadgwickFilter SAMPLING time(if100Hz,=1/100)
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
void setup() {
Serial.begin(115200);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
MadgwickFilter.begin(100); //if100,100Hz
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(200000);
Wire.begin();
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
}
void loop() {
timer = millis();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
float ay_g = ayRaw / 16384.0;
float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
float temp = (temperature / 333.87f) + 21.0f; // データシートより
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, ax_g, ay_g, az_g);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw();
Serial.print(roll); Serial.print(",");
Serial.print(pitch); Serial.print(",");
Serial.print(yaw); Serial.print(",");
Serial.print(axRaw); Serial.print(",");
Serial.print(ayRaw); Serial.print(",");
Serial.print(azRaw); Serial.print(",");
Serial.print(temp); Serial.print(",");
Serial.print(gx_dps); Serial.print(",");
Serial.print(gy_dps); Serial.print(",");
Serial.println(gz_dps);
// Wait to full timeStep period
delay((timeStep * 1000) - (millis() - timer));
}
Arduinoを用いてIMUで姿勢推定 on ROS（詳しく総括）
MPU9250の磁気センサー（AK8963）をI2Cで使う・Magwickフィルタと連携
ArduinoのMadgwickライブラリの使い方
Madgwickライブラリの収束を速くする 【Arduino/MPU6050】
六軸センサーから姿勢角度算出（メインプログラムの参考にさせてもらった、メインループが定周期で実行されるようになってる）
姿勢推定アルゴリズム実装：STM32単体で検証する「Madgwickフィルタ」の性能サンプリング周波数をいじって改造してるのと、挙動について細かく解説
上記サイトで9軸センサを使い姿勢推定プログラムを作成する？
ブラシレスモータをESCで動かす
作り始めのとき、ESCはPWMで動く！という認識で（間違ってはいない）あったので、PWMモードでプログラムを組んでいます。
※PWMモードとは、基本的には、50Hz周波数のPWMでDuty ON時間を1000μs～2000μsで制御して、0-100％スロットルとして認識してくれるものです。
後述しますが、他にもいろいろなモードがあるのですが、兎に角、まずはモータを回してみるということでPWMモードでプログラムを組みました。正確にはPWMモードしか知りませんでした！
PWMモードでESCの試運転をする
MPU6500で大分手こずったので、ESCとモータが確実に動くのか確認します。
1番右前が左回転(A赤B黒C黄)、プロペラは逆回転で付け替え。2番左前を右回転、3番左後ろを右回転(A赤B青C黒)、4番右後ろを左回転に設定配線します。
| ピン | 接続先 | 備考 |
|---|---|---|
| D6 | ESC 信号線（白） | |
| GND | ESC GND（黒） | |
| GND | 可変抵抗 GND | ※実際には抵抗使用せず |
| A7 | 可変抵抗 信号 | ※実際には抵抗使用せず |
| 5V | 可変抵抗 電源側 | ※実際には抵抗使用せず |
可変抵抗でモータを動かすサンプルプログラムを改変して、シリアル入力の数値でモータ回転数を与えるプログラムを作成。
#include <Servo.h>
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN 6 //ESCへの出力ピン
int volume = 0; //可変抵抗の値を入れる変数
int val = 0;
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
Servo esc; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc`と命名している．
void setup() {
///////////////////////////////////////////////////////
//setup
///////////////////////////////////////////////////////////
Serial.begin(9600); //シリアル通信初期化
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
Serial.println("Turn on power source, then wait 2 seconds and press any key.");
while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
Serial.read();
esc.attach(ESC_PIN); //ESCへの出力ピンをアタッチします
Serial.println("Writing maximum output.");
esc.writeMicroseconds(MAX_SIGNAL); //ESCへ最大のパルス幅を指示します
Serial.println("Wait 2 seconds.");
delay(2000);
Serial.println("Writing minimum output");
esc.writeMicroseconds(MIN_SIGNAL); //ESCへ最小のパルス幅を指示します
Serial.println("Wait 2 seconds. Then motor starts");
delay(2000);
}
///////////////////////////////////////////////////////
//loop
///////////////////////////////////////////////////////////
void loop() {
//受信データがある場合if内を処理
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
Serial.println(val); //出力
}
esc.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
//可変抵抗の接続がある場合の処理
// volume = analogRead(A7) * 3.0; //可変抵抗の値(1024cnt/5v)を * x.x で掛けて変数volumeに入れる．値を調整したい場合は倍率を変更する．
// sprintf(message, "Pulse Width: %d micro sec", volume); //シリアルモニタに表示するメッセージを作成
// Serial.println(message); //可変抵抗の値をシリアルモニタに表示
// esc.writeMicroseconds(volume); // パルス幅 `volume` のPWM信号を送信する
}
D6ピンに4軸それぞれのESC信号線を一括して接続すると、4軸同時動作が可能なことを確認。
続いて、4軸別々にモータを回転させるプログラムをつくる。
接続図
| Arduino UNO R3 | ESC |
|---|---|
| D6 | ESC1 信号（右前） |
| D9 | ESC2 信号（左前） |
| D10 | ESC3 信号（左後） |
| D11 | ESC4 信号（右後） |
| GND | ESC GND（共通接続） |
色々試行錯誤するうちにESCのキャリブレーションがおかしくなったみたいで、回転しない軸がありました。結果的に下のサンプルプログラムではESCキャリブレーションをかけているプログラムとなったみたいです。
#include <Servo.h>
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN6 6 //ESCへの出力ピン
#define ESC_PIN9 9 //ESCへの出力ピン
#define ESC_PIN10 10 //ESCへの出力ピン
#define ESC_PIN11 11 //ESCへの出力ピン
int val = 0;
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
Servo esc1; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc1`と命名している．
Servo esc2; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc2`と命名している．
Servo esc3; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc3`と命名している．
Servo esc4; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc4`と命名している．
void setup() {
///////////////////////////////////////////////////////
//setup
///////////////////////////////////////////////////////////
Serial.begin(9600); //シリアル通信初期化
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
Serial.println("Turn on power source, then wait 2 seconds and press any key.");
while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
Serial.read();
esc1.attach(ESC_PIN6); //ESCへの出力ピンをアタッチします
esc2.attach(ESC_PIN9); //ESCへの出力ピンをアタッチします
esc3.attach(ESC_PIN10); //ESCへの出力ピンをアタッチします
esc4.attach(ESC_PIN11); //ESCへの出力ピンをアタッチします
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESCへ最大のパルス幅を指示します
Serial.println("Wait 2 seconds.esc1");
delay(2000);
esc2.writeMicroseconds(MAX_SIGNAL); //ESCへ最大のパルス幅を指示します
Serial.println("Wait 2 seconds.esc2");
delay(2000);
esc3.writeMicroseconds(MAX_SIGNAL); //ESCへ最大のパルス幅を指示します
Serial.println("Wait 2 seconds.esc3");
delay(2000);
esc4.writeMicroseconds(MAX_SIGNAL); //ESCへ最大のパルス幅を指示します
Serial.println("Wait 2 seconds.esc4");
delay(2000);
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESCへ最小のパルス幅を指示します
Serial.println("Wait 2 seconds.esc1");
delay(2000);
esc2.writeMicroseconds(MIN_SIGNAL); //ESCへ最小のパルス幅を指示します
Serial.println("Wait 2 seconds.esc2");
delay(2000);
esc3.writeMicroseconds(MIN_SIGNAL); //ESCへ最小のパルス幅を指示します
Serial.println("Wait 2 seconds.esc3");
delay(2000);
esc4.writeMicroseconds(MIN_SIGNAL); //ESCへ最小のパルス幅を指示します
Serial.println("Wait 2 seconds.esc4. Then motor starts");
delay(2000);
}
///////////////////////////////////////////////////////
//loop
///////////////////////////////////////////////////////////
void loop() {
//受信データがある場合if内を処理
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
Serial.println(val); //出力
}
esc1.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc2.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc3.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc4.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
}
ESCのファームウェア「BLHeli_S」について（入手したものバージョン不明、基板にはVer2.3と記載）
PWMモードのキャリブレーション挙動（試行錯誤で確認）
ノーマルオペレーション（キャリブレーション実施済み前提）
●起動時に♪タララ↑と上がり調子の３ビープ
⇛PWMを2000（Max値）で流す（２秒間）
※このとき５秒以上より長く？するとキャリブレーションモードに入るので注意
最低スロットルPWMが1000以上検知されると鳴る。（2秒2000で設定）
続けて1000の最低スロットル（６秒以上）
●♪タララ↑と上がり調子の３ビープ
多分最低スロットル認識で鳴る。（1000で4秒程度で鳴る）
●低音♪ビーと高音♪ピーの短い２ビープ
モータ運転準備完了？（ここまで最低スロットルから６秒）
これでモータは動く。
スロットルキャリブレーション
●起動時に♪タララ↑と上がり調子の３ビープ
⇛PWMを2000（Max値）で流す
※このとき５秒程度より長くするとキャリブレーションモードに入る
●再度♪タララ↑と上がり調子の３ビープ
恐らくキャリブレーションモードに入ったことを示す
●低音♪ビーと長めの１ビープ
●高音♪ピピと短めの２ビープ
MAXスロットル測定中
●♪タラララ↑と上がり調子の４ビープを３回繰り返す
MAXスロットルがESCに記憶されたら鳴る。
（ここまでフルスロットル2000で１２秒を流し続ける）
続けて、最低スロットル1000を流す。
●高音♪ピピ、ピピ、…と短めの２ビープを１０回程度繰り返す
MINスロットル測定中
●♪タラララ↓と下がり調子の４ビープを３回繰り返す
MINスロットルがESCに記憶されたら鳴る。キャリブレーション完了。
●低音♪ビーと高音♪ピーの短い２ビープ
モータ運転準備完了？
（ここまで１０秒最低スロットル１000を流し続ける）
これでモータは動く。電源を切ってもキャリブレーション結果は記憶されている。
ちなみに
●低音♪ビーと高音♪ピーの長い２ビープ
スロットルがMIN以下の場合に鳴る。
ESCのバージョンによって色々違うみたいです。特にプログラム開発中は、いろんな操作でキャリブレーションが狂ったりすることがある（あった）ので、毎回スロットルキャリブレーションを実施したほうがよいです。
///////////////////////////////////////////////////////
//ESCスロットルキャリブレーション時は setup()に以下をいれる
///////////////////////////////////////////////////////////
esc1.attach(ESC_PIN6); //ESCへの出力ピンをアタッチします
esc2.attach(ESC_PIN9); //ESCへの出力ピンをアタッチします
esc3.attach(ESC_PIN10); //ESCへの出力ピンをアタッチします
esc4.attach(ESC_PIN11); //ESCへの出力ピンをアタッチします
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESC1へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc1");
esc2.writeMicroseconds(MAX_SIGNAL); //ESC2へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc2");
esc3.writeMicroseconds(MAX_SIGNAL); //ESC3へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc3");
esc4.writeMicroseconds(MAX_SIGNAL); //ESC4へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc4");
delay(12000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESC1へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc1");
esc2.writeMicroseconds(MIN_SIGNAL); //ESC2へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc2");
esc3.writeMicroseconds(MIN_SIGNAL); //ESC3へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc3");
esc4.writeMicroseconds(MIN_SIGNAL); //ESC4へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc4. Then motor starts");
delay(10000);//ESC MINキャリブレーション処理完了のために必要
///////////////////////////////////////////////////////
//ESCスロットルキャリブレーション ショートVer
///////////////////////////////////////////////////////////
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESC1へ最大のパルス幅を指示します
esc2.writeMicroseconds(MAX_SIGNAL); //ESC2へ最大のパルス幅を指示します
esc3.writeMicroseconds(MAX_SIGNAL); //ESC3へ最大のパルス幅を指示します
esc4.writeMicroseconds(MAX_SIGNAL); //ESC4へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc1-4");
delay(12000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESC1へ最小のパルス幅を指示します
esc2.writeMicroseconds(MIN_SIGNAL); //ESC2へ最小のパルス幅を指示します
esc3.writeMicroseconds(MIN_SIGNAL); //ESC3へ最小のパルス幅を指示します
esc4.writeMicroseconds(MIN_SIGNAL); //ESC4へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc1-4. Then motor starts");
delay(10000);//ESC MINキャリブレーション処理完了のために必要
///////////////////////////////////////////////////////
//ESCノーマルオペレーション時（キャリブレーションを一度でも実施している）は setup()に以下をいれる
///////////////////////////////////////////////////////////
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESC1へ最大のパルス幅を指示します
Serial.println("Wait 2 seconds to wakeup esc1");
esc2.writeMicroseconds(MAX_SIGNAL); //ESC2へ最大のパルス幅を指示します
Serial.println("Wait 2 seconds to wakeup esc2");
esc3.writeMicroseconds(MAX_SIGNAL); //ESC3へ最大のパルス幅を指示します
Serial.println("Wait 2 seconds to wakeup esc3");
esc4.writeMicroseconds(MAX_SIGNAL); //ESC4へ最大のパルス幅を指示します
Serial.println("Wait 2 seconds to wakeup esc4");
delay(2000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESC1へ最小のパルス幅を指示します
Serial.println("Wait 6 seconds to wakeup esc1");
esc2.writeMicroseconds(MIN_SIGNAL); //ESC2へ最小のパルス幅を指示します
Serial.println("Wait 6 seconds to wakeup esc2");
esc3.writeMicroseconds(MIN_SIGNAL); //ESC3へ最小のパルス幅を指示します
Serial.println("Wait 6 seconds to wakeup esc3");
esc4.writeMicroseconds(MIN_SIGNAL); //ESC4へ最小のパルス幅を指示します
Serial.println("Wait 6 seconds to wakeup esc4. Then motor starts");
delay(6000);//ESC MINキャリブレーション処理完了のために必要
///////////////////////////////////////////////////////
//ESCノーマルオペレーション ショートVer
///////////////////////////////////////////////////////////
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESC1へ最大のパルス幅を指示します
esc2.writeMicroseconds(MAX_SIGNAL); //ESC2へ最大のパルス幅を指示します
esc3.writeMicroseconds(MAX_SIGNAL); //ESC3へ最大のパルス幅を指示します
esc4.writeMicroseconds(MAX_SIGNAL); //ESC4へ最大のパルス幅を指示します
Serial.println("Wait 2 seconds to wakeup esc1-4");
delay(2000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESC1へ最小のパルス幅を指示します
esc2.writeMicroseconds(MIN_SIGNAL); //ESC2へ最小のパルス幅を指示します
esc3.writeMicroseconds(MIN_SIGNAL); //ESC3へ最小のパルス幅を指示します
esc4.writeMicroseconds(MIN_SIGNAL); //ESC4へ最小のパルス幅を指示します
Serial.println("Wait 6 seconds to wakeup esc1-4. Then motor starts");
delay(6000);//ESC MINキャリブレーション処理完了のために必要
ブラシレスモータ参考サイト
ブラシレスモーターを Arduino と ESC で動かす方法（ESC動作の基本プログラムとして参考にした）
ArduinoでPCから、シリアル通信で数値を送る (Serial.parseInt())（ESCスロットル値を抵抗器ではなくシリアル入力にするために参考にした）
機体の構成
右前からモータ１、左回りに続き、左前モータ２、左後ろモータ３、右後ろモータ４とした。
ESC１-4はモータ番号と同一とした。
モータ１とモータ３が左回転、モータ2とモータ4が右回転とした。
ROLL左旋回を+、PITCH機首上昇を+、YAW左旋回を+とした。
姿勢情報でモータをコントロールし安定させる
PID制御に当たっては、こちらのサイト⇛姿勢計測 + PID制御で光量調節（ドローン製作準備，esp32，Arduino IDE）を非常に参考にさせていただきました。ありがとうございます。
メインループはMadgwickAHRSフィルタのプログラムの流れを引き継いで、特に思慮はなく、100Hz(10ms)で実行されるようにしてます。
※あとあとで分かったことですが、Servo.hライブラリで出力されるPWM周波数は50Hz固定なので、50Hz→20msの実行周期で、モータが制御されていることになります。
躓いたところ
角度偏差（360°に対する百分率）に対して、PID制御量をプロペラ回転数1000μs（スロットル操作幅）にスケーリングした。
高速回転になると、なぜか角度取得値に誤差が発生し、水平なのに傾いてる認識となった、これによりI成分が悪く働きどんどん傾いていく。I成分は使えない。誤差発生要因は？振動による誤差蓄積か？？
→I のゲインを適切に調整して、水平を保つ動作をさせるとドリフト影響なくなる。
これは機体振動がない状態だと、ドリフト影響をうけ、機体がある程度動くとドリフト影響が軽減される？
Madgwickの仕様か？角速度方向が正逆切り替わるとドリフト補正働くのか？
パルスDuty1500μs（速度50％スロットル）程度まで回転数を上げないと機体が浮かないしrollの動きもでないのでゲイン調整する回転数は大切です。50％スロットルで調整しても、50-100％まで安定して制御できるかは確認しないとわからない。
ゲイン調整のとき、ゲインを下げすぎると機体が左右に振れる動きが角度偏差を揺らしてオーバーゲインのように勘違いしたが、実際はゲイン不足により機体が左右に触れいている。
※さらにこの後のちに気づいたことですが、モータPWMは50Hz,20msでの制御ループであり、ドローンの制御には厳しい、1ｋHzはあるほうがよい、ようだ。
#include <MadgwickAHRS.h>
#include <Servo.h>
#include <Wire.h>
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
// Timers
unsigned long timer = 0;
float timeStep = 0.01;//MadgwickFilter SAMPLING time(if100Hz,=1/100)
float SAMPLING_TIME_MS = 10;//PIDsamplingTime
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
//zero set value
float pitch_z = 0;
float roll_z = 0;
float yaw_z = 0;
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN6 6 //ESCへの出力ピン
#define ESC_PIN9 9 //ESCへの出力ピン
#define ESC_PIN10 10 //ESCへの出力ピン
#define ESC_PIN11 11 //ESCへの出力ピン
int val = MIN_SIGNAL;
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
float cmd[3] = {0.0f, 0.0f, 0.0f};//つかってる？
Servo esc1; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc1`と命名している．
Servo esc2; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc2`と命名している．
Servo esc3; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc3`と命名している．
Servo esc4; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc4`と命名している．
// Gain: roll, pitch, yaw
float Kp[3] = {0.5f, 0.5f, 0.0f};//100.0f
float Ki[3] = {0.0005f, 0.0005f, 0.0f};//0.1f
float Kd[3] = {0.02f, 0.02f, 0.0f};//1.0f
// Measured value: roll, pitch, yaw
float rpy_p[3] = {0.0f, 0.0f, 0.0f};
float rpy_i[3] = {0.0f, 0.0f, 0.0f};
float rpy_d[3] = {0.0f, 0.0f, 0.0f};
float rpy_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float pid_rpy[3] = {0.0f, 0.0f, 0.0f};
float data[3] = {0.0f, 0.0f, 0.0f};
void setup() {
Serial.begin(115200);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
MadgwickFilter.begin(100); //if100,100Hz
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(200000);
Wire.begin();
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
//Serial.println("Turn on power source, then wait 2 seconds and press any key.");
//while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
//Serial.read();
Serial.println("Turn on ESC power source, before this message then wait 5 seconds. Auto start calibration the ESC");
delay(5000);
esc1.attach(ESC_PIN6); //ESCへの出力ピンをアタッチします
esc2.attach(ESC_PIN9); //ESCへの出力ピンをアタッチします
esc3.attach(ESC_PIN10); //ESCへの出力ピンをアタッチします
esc4.attach(ESC_PIN11); //ESCへの出力ピンをアタッチします
Serial.println("Writing maximum output.");
esc1.writeMicroseconds(MAX_SIGNAL); //ESC1へ最大のパルス幅を指示します
esc2.writeMicroseconds(MAX_SIGNAL); //ESC2へ最大のパルス幅を指示します
esc3.writeMicroseconds(MAX_SIGNAL); //ESC3へ最大のパルス幅を指示します
esc4.writeMicroseconds(MAX_SIGNAL); //ESC4へ最大のパルス幅を指示します
Serial.println("Wait 12 seconds to caliblate MAX for esc1-4");
delay(12000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
esc1.writeMicroseconds(MIN_SIGNAL); //ESC1へ最小のパルス幅を指示します
esc2.writeMicroseconds(MIN_SIGNAL); //ESC2へ最小のパルス幅を指示します
esc3.writeMicroseconds(MIN_SIGNAL); //ESC3へ最小のパルス幅を指示します
esc4.writeMicroseconds(MIN_SIGNAL); //ESC4へ最小のパルス幅を指示します
Serial.println("Wait 10 seconds to caliblate MIN for esc1-4. Then motor starts");
delay(10000);//ESC MINキャリブレーション処理完了のために必要
//RPYゼロ角度取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll_z = MadgwickFilter.getRoll();
pitch_z = MadgwickFilter.getPitch();
yaw_z = MadgwickFilter.getYaw();
}
void loop() {
timer = millis();
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
// 加速度はMadgwickFilter内で正規化されるため単位変換不要
//float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
//float ay_g = ayRaw / 16384.0;
float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
//float temp = (temperature / 333.87f) + 21.0f; // データシートより
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw() - yaw_z;
Serial.print(roll); Serial.print(",");
//Serial.print(pitch); Serial.print(",");
//Serial.print(az_g); Serial.print(",");
//Serial.print(yaw); Serial.print(",");
//Serial.print(val); Serial.print(","); //出力
data[0]=roll/360; //角度を百分率換算（100％/360°）
data[1]=pitch/360;
data[2]=yaw/360;
for (int i=0; i<3; i++) {
rpy_p[i] = data[i];
rpy_i[i] += rpy_p[i];
rpy_d[i] = (rpy_p[i] - rpy_pre[i]) / ((float)SAMPLING_TIME_MS/1000);
rpy_pre[i] = rpy_p[i];
if (val <= MIN_SIGNAL) { //出力が最低以下でPID出力ゼロとする
rpy_i[i] = 0;
pid_rpy[i] = 0;
}
else if (val >= MAX_SIGNAL){ //出力が最高以上のリミット
val = MAX_SIGNAL;
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
pid_rpy[i] = pid_rpy[i]*1000; //制御量（角度百分率）をプロペラ回転数1000dgit換算
}
else {
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
pid_rpy[i] = pid_rpy[i]*1000; //制御量（角度百分率）をプロペラ回転数1000dgit換算
}
}
esc1.writeMicroseconds(val-pid_rpy[0]-pid_rpy[1]+pid_rpy[2]); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc2.writeMicroseconds(val+pid_rpy[0]-pid_rpy[1]-pid_rpy[2]); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc3.writeMicroseconds(val+pid_rpy[0]+pid_rpy[1]+pid_rpy[2]); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc4.writeMicroseconds(val-pid_rpy[0]+pid_rpy[1]-pid_rpy[2]); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
Serial.print(Kp[0]*rpy_p[0]*1000); Serial.print(",");
Serial.print(Ki[0]*rpy_i[0]*1000); Serial.print(",");
Serial.print(Kd[0]*rpy_d[0]*1000); Serial.print(",");
Serial.print(pid_rpy[0]); Serial.println(",");
//Serial.print(val-pid_rpy[0]-pid_rpy[1]+pid_rpy[2]); Serial.print(",");
//Serial.print(val+pid_rpy[0]-pid_rpy[1]-pid_rpy[2]); Serial.print(",");
//Serial.print(val+pid_rpy[0]+pid_rpy[1]+pid_rpy[2]); Serial.print(",");
//Serial.print(val-pid_rpy[0]+pid_rpy[1]-pid_rpy[2]); Serial.print(",");
// Wait to full timeStep period
if ( ((timeStep * 1000) - (millis() - timer)) >= 0 ){
delay((timeStep * 1000) - (millis() - timer));//ここまで約5ms
}
else{
Serial.print((timeStep * 1000) - (millis() - timer));
Serial.println("msタイムオーバーです！");//main loopタイムオーバー表示
}
//受信データがある場合if内を処理
//約1000ms処理必要のためdelayの後に置く
//if処理なしで約5ms
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
}
//Serial.println((timeStep * 1000) - (millis() - timer));//MadgwickFilter実行周波数の残り時間ms
}
Roll軸のゲイン調整をする
ゲインを調整する過程（10ms／サンプリング）
メインループ10msで実行しているので1サンプリングごとに10msの時間軸です。つまり120ms/div、画面いっぱいで480msのグラフ。(多分10msくらいで動いたはず)
モータ制御周期は10ms（100Hz）なのか、20ms（50Hz）なのかは不明です。
※arduinoのPWMが50Hzより早く変更されたときの挙動とESCが50Hzより早いPWMを受けっとったらどうなるかの挙動がわからない
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.8,Ki=0.001,Kd=0.04 発散する val=1500(回転数50％)
グラフはrollのみ調整中でpitchは無制御
⇛Kpゲインが少し高いか
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.001,Kd=0.04 発散する val=1500(回転数50％)
グラフはrollのみ調整中でpitchは無制御
⇛Kdゲインが効きすぎている？少し高いか
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.001,Kd=0.02 発散する val=1500(回転数50％)
グラフはrollのみ調整中でpitchは無制御
⇛Kpゲインが効きすぎている？少し高いか
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.4,Ki=0.001,Kd=0.02 発散する val=1500(回転数50％)
グラフはrollのみ調整中でpitchは無制御
⇛不安定な動きは見られなくなったので、同じ設定をpichにもいれる
Roll軸+Pich軸でゲイン調整
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.4,Ki=0.001,Kd=0.02 発散っぽい不安定 val=1500(回転数50％)
グラフはrollのみ、pitchも同ゲインで制御中
⇛Kdさげてみる
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.4,Ki=0.001,Kd=0.01 横揺れしてる？発散気味 val=1500(回転数50％)
グラフはrollのみ、pitchも同ゲインで制御中
⇛Kpをすこしさげる
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.3,Ki=0.001,Kd=0.01 発散微妙 val=1500(回転数50％)
グラフはrollのみ、pitchも同ゲインで制御中
⇛横揺れ発生、ゲインが低すぎる
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.6,Ki=0.005,Kd=0.01 発散する val=1500(回転数50％)
グラフはrollのみ、pitchも同ゲインで制御中
⇛その後試行錯誤も横揺れ収まらず、I要素を増やすと横揺れに反応して制動動作になることを発見。
これで横揺れ抑制収束するので、もう少しIを効かしてみる
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.0005,Kd=0.02 発散する val=1600(回転数60％) PID単位はdigit(1000digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中
⇛Iを増やすとIの蓄積が不安定をもたらすので、Iを減らし50％で比較的安定。60％でも安定するが、横揺れ発生はする。もう少し、ゲインPIの増加が必要か？ただし、プロペラガード等の物理保護がないと危なくて試験ができない状態。
固定した状態だと、評価継続不可なので、バッテリー購入と無線通信する手段を検討。
ＮｉＣｄバッテリー243ｇ、ドローンテスト機90ｇ弱
7.2ＶのＮｉＣｄバッテリー最大スロットルで約200ｇ浮上。機体重量を200ｇより軽く設定するようにバッテリーを選定。3Ｓ（11Ｖ）、66ｇを購入。
無線化するのに、ＥＰＳ32が格安コスパ最強+arduino開発環境で使用可能なのでこちらに移行することにした。
ESP32をarduinoIDEでプログラミング
arduinoでの無線通信をしようとしたときに、通信基板を追加等していくと数千円以上のコストがかかることが判明。そこで最近の電子工作で主流のESP32（数百円）に乗り換えれば、そのまま無線通信もできるらしい。
家に余ってるWiiリモコンとの通信もできるとのHPもあったので、こちらに移行してみた。
家に余ってるWiiリモコンを比較的手軽に使えそう、かつ、安いESC32を導入。
画像元はhttps://circuits4you.com/2018/12/31/esp32-devkit-esp32-wroom-gpio-pinout/
ESP32をArduino IDEで使用できるようにする
やり方をネットで検索すると、下のような手順で環境設定メニューにURLを追加して…などの操作ができてきましたが、2026年現在はそのままインストールできました。Win11 Arduino IDE2.3.7
次に、Arduino IDEのPreferences…メニューを選びます。すると設定ウィンドウが開きますが、その中に「追加のボードマネージャのURL 」という項目があります。ここに安定版配布URL（https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json）をペーストします。
ボードマネージャーを開きます。
espで検索すると「esp32」が表示されるのでINSTALLをクリック。
ダウンロードとインストールが始まります。しばらく数分待つとインストール成功のメッセージがでます。
さきほどなかったesp32が追加されています。購入したESP32-WROOMを選択します。
早速これまでのプログラムをコンパイル実施、エラー発生。Servoライブラリが非対応のようなメッセージなので、先人とAIの知恵を借り、「ESP32Servo」をライブラリに追加すればよいとのこと。
早速インストールし、Servo.h→ESP32Servo.hに変更する。
//#include <Servo.h> #include <ESP32Servo.h>//上をこのように書き換えるだけ。
その後、コンパイルは実施できたが、シリアルポートが認識されない。AIに尋ねるとドライバー問題が多いそうだが、デバイスマネージャ確認してもそもそも認識されていない。USBケーブルが充電用、通信用あるらしく、違うケーブルに変えると無事認識してくれた。
アップロードするも完了しないようで、プログラムも走らない。→ピンアサインが古いarduinoのままで存在しないPINを指定しているようなので、変更しアップロード→完了
ESP32でのPWM周期確認とPID制御周期検討
ライブラリESP32Servo.hの実行が遅い問題
Servo.h→ESP32Servo.hに変更したところ、100HzのMadgwickフィルタvoid loop()がタイムオーバーする事象が発生。30ｍsタイムオーバーしているので、100Hzの10ｍsからは3倍遅くなってる。
arduinoUNOR3ではPWM出力まで含めて約5msで動いていたので、困った。
PWM出力する部分（esc1.writeMicroseconds(xx);）手前までは2ms、そこからPWM出力×4軸部分で38msかかってる。。。
想像でしかないが、Servoライブラリ標準では50Hz,20ｍsでＰＷＭパルスを出力しているようなのだが、もしかして20ｍsの1周期完了を待っているのではないか？
ESP32Servo.hではPWMの周波数を変更できるので、試しに変更して確認。
※ちなみにArduinoのServo.hではPWM周波数は50Hz固定で変更できないらしい。
周波数を短くすると、loop実行時間は短くなった。つまり、Servo出力時にPWM周期完了を待っている可能性が高い。
ここを追及して調べていくとは沼りそうなので、さらっと納得してESP32Servo.hを使うことをあきらめる方針に。
（注：このあとしっかり沼にはまってPWM高速志向しました。）
PWM出力回路の実行周期比較（ESP32）
ESP32Servo.hを使用して4軸出力確認。実際のパルスは確認していない。
下が、ESP32Servo.hを使用した4軸出力確認プログラム。xxx.setPeriodHertz(50);の部分でPWM周波数を50Hzと設定しています。Esp32loop実行周期は40msだった。50Hzの20ms周期に収まっていない。
50HzのPWMこの部分を変更して確認。
500Hzとすると、3msとなった。これも周期の2msに収まっていない。
// 15,2,4,16 PIN にPWM信号を出力させる例
#include <Wire.h>
#include<ESP32Servo.h> //esp32でサーボを使うためのライブラリをインクルード
Servo esc1; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc1`と命名している．
Servo esc2; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc2`と命名している．
Servo esc3; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc3`と命名している．
Servo esc4; //Servoオブジェクトを作成する．今回はESCにPWM信号を送るので，`esc4`と命名している．
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESCへの出力ピン
#define ESC_PIN2motor 2 //ESCへの出力ピン
#define ESC_PIN3motor 4 //ESCへの出力ピン
#define ESC_PIN4motor 16 //ESCへの出力ピン
unsigned long timer = 0;
int val = MIN_SIGNAL;
void setup() {
Serial.begin(115200);// 115200bpsでポートを開く
esc1.attach(ESC_PIN1motor, MIN_SIGNAL, MAX_SIGNAL); //ESCへの出力ピンをアタッチ,パルス幅1000-2000
esc2.attach(ESC_PIN2motor, MIN_SIGNAL, MAX_SIGNAL); //ESCへの出力ピンをアタッチします,パルス幅1000-2000
esc3.attach(ESC_PIN3motor, MIN_SIGNAL, MAX_SIGNAL); //ESCへの出力ピンをアタッチします,パルス幅1000-2000
esc4.attach(ESC_PIN4motor, MIN_SIGNAL, MAX_SIGNAL); //ESCへの出力ピンをアタッチします,パルス幅1000-2000
esc1.setPeriodHertz(50); //PWM周波数50Hz
esc2.setPeriodHertz(50); //PWM周波数50Hz
esc3.setPeriodHertz(50); //PWM周波数50Hz
esc4.setPeriodHertz(50); //PWM周波数50Hz
}
void loop() {
timer = millis();
esc1.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc2.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc3.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
esc4.writeMicroseconds(val); // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
Serial.print("loop実行周期ms=");
Serial.println((millis() - timer));//MadgwickFilter実行周波数の残り時間ms
}
AIによるとESP32Servoライブラリの仕様は40〜900Hzの範囲のようなので、1kHzとかを目指すことはできない。
そこでLEDCライブラリを使用したPWMプログラムでも確認。ServoライブラリはLEDCをベースにしているようなので、当然同じ結果になるだろう。
ledcWriteを使ったとき。error: ‘ledcSetup’ was not declared in this scopeが発生、Arduino ESP32 coreの2系から3系になるときに破壊的な変更があったようだ。リンク先を参考に修正。
ESP32で複数の独立したPWM信号を出力する（参考にさせてもらったけど、LEDCの記述が古かった、エラー発生）
Arduino – ESP32 の PWM ( LEDC )で 40MHzまでの安定した高周波パルスを思い通りに出せたぞ（LEDCについて詳しく解説してある）
// 15,2,4,16 PIN にPWM信号を出力させる例
// loop()の実行時間確認用
#include <Wire.h>
int vref;
const int freq_dc = 1000;
const int resolution = 8;
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESCへの出力ピン
#define ESC_PIN2motor 2 //ESCへの出力ピン
#define ESC_PIN3motor 4 //ESCへの出力ピン
#define ESC_PIN4motor 16 //ESCへの出力ピン
unsigned long timer = 0;
void setup() {
Serial.begin(115200);// 115200bpsでポートを開く
vref = 0;
ledcWrite(ESC_PIN1motor, vref);// 出力値（デューティ比）分解能で指定の書き込み
ledcWrite(ESC_PIN2motor, vref);
ledcWrite(ESC_PIN3motor, vref);
ledcWrite(ESC_PIN4motor, vref);
// ピンの初期設定 (ピン番号, 周波数, 分解能)
ledcAttach(ESC_PIN1motor, freq_dc, resolution); // ピン, Hz, 8bit(0-255)
ledcAttach(ESC_PIN2motor, freq_dc, resolution);
ledcAttach(ESC_PIN3motor, freq_dc, resolution);
ledcAttach(ESC_PIN4motor, freq_dc, resolution);
}
void loop() {
timer = micros();
vref = 128 ;
ledcWrite(ESC_PIN1motor, vref);// 出力値（デューティ比）分解能で指定の書き込み
ledcWrite(ESC_PIN2motor, vref);
ledcWrite(ESC_PIN3motor, vref);
ledcWrite(ESC_PIN4motor, vref);
timer = micros()- timer;
Serial.print("loop実行周期μsは");
Serial.println(timer);//MadgwickFilter実行周波数の残り時間μs
}
「const int freq_dc = 50;」で50Hzを指定している。このときのloop実行周期は40ms。
500Hzで3ms。1000Hzで1ms。millis()をmicros()に変更して実行周期をms→μsで表示するように変更したところ、1000Hzで1947μs。なので1000Hzの周期は1ms（1000μs）なのでloop実行がPWM周期に収まっていない。
つまり、Esp32Servoと同様の結果となった。また、PWM周期終了を待つのか？PWM周期+αのloop実行周期となるようだ。
関数（digitalWrite/Read）使って直接PWMを生成する方式にチャレンジ。
実行周期確認用のためPWM周期は未設定、PWMのON時間を1μsに設定して確認。
loop実行周期は10μsとなった。
これならPWM周期内でloopの中にPID周期を収めることができる。
ドローンに理想といわれる4kHz→250μs周期に十分収まるようだ。
// 15,2,4,16 PIN にPWM信号を出力させる例
// loop()の実行時間確認用
#include <Wire.h>
const int freq_dc = 1000;
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESCへの出力ピン
#define ESC_PIN2motor 2 //ESCへの出力ピン
#define ESC_PIN3motor 4 //ESCへの出力ピン
#define ESC_PIN4motor 16 //ESCへの出力ピン
unsigned long timer = 0;
unsigned long ESCLoopTimer = 0;
unsigned long ESC1_timer = 0;
unsigned long ESC2_timer = 0;
unsigned long ESC3_timer = 0;
unsigned long ESC4_timer = 0;
void setup() {
Serial.begin(115200);// 115200bpsでポートを開く
pinMode(ESC_PIN1motor, OUTPUT);
pinMode(ESC_PIN2motor, OUTPUT);
pinMode(ESC_PIN3motor, OUTPUT);
pinMode(ESC_PIN4motor, OUTPUT);
}
void loop() {
timer = micros();
//PWM 15,2,4,16pin => HIGH
digitalWrite(ESC_PIN1motor, HIGH);
digitalWrite(ESC_PIN2motor, HIGH);
digitalWrite(ESC_PIN3motor, HIGH);
digitalWrite(ESC_PIN4motor, HIGH);
//PWMパルス幅カウンターセット
ESCLoopTimer = micros();
ESC1_timer = 1 + ESCLoopTimer;
ESC2_timer = 1 + ESCLoopTimer;
ESC3_timer = 1 + ESCLoopTimer;
ESC4_timer = 1 + ESCLoopTimer;
//PWM 3,5,6,9pin => LOW
while (digitalRead(ESC_PIN1motor) + digitalRead(ESC_PIN2motor) + digitalRead(ESC_PIN3motor) + digitalRead(ESC_PIN4motor) > 0) {
ESCLoopTimer = micros();
if (ESC1_timer <= ESCLoopTimer)digitalWrite(ESC_PIN1motor, LOW);
if (ESC2_timer <= ESCLoopTimer)digitalWrite(ESC_PIN2motor, LOW);
if (ESC3_timer <= ESCLoopTimer)digitalWrite(ESC_PIN3motor, LOW);
if (ESC4_timer <= ESCLoopTimer)digitalWrite(ESC_PIN4motor, LOW);
}
timer = micros()- timer;
Serial.print("loop実行周期μsは");
Serial.println(timer);//MadgwickFilter実行周波数の残り時間μs
}
参考にしたのはドローンの自作｜姿勢制御プログラム（ブラシレスモーター制御編）（4軸同時パルス出力：高速応答を志向している）….のプログラム。関数（digitalWrite/Read）使ってますが、処理に時間が掛かるのと、一度にpin操作できないため、ポートを直接制御すると、さらに高速になるようです。
▼例えば▼
PORTD |= B11110000;
ただ、これはarduinoでの処理で、ESP32ではGPIOを使うようです。
上記プログラムでパルス幅カウンターを全て”1″(1μs)にして確認してみると、実行周期は9-10μsとなった。
【ESP32】主要な命令の実行速度を計測してみたによると、digitalWriteは1.15μs、digitalReadは0.5μsなので、上記プログラムでは総合的に10μsなのかもしれません。でもこのままで十分早いようなのでこのままいきます。
→Oneshot125や、その先Dshotを見越してさらに高速化を検討しました。
こんな感じで記述すれば4、5、6、7pinを同時にHIGHにできますし、digitalWrite()使用するより圧倒的に処理速度が速いです。
50HzのPWMでPID制御するのは、制御ループ的には遅いらしい。100Hzが基本で、1kHz程度が望ましい。4kHzがドローン制御の基本らしい（AI談）。このあたり、自作ドローンのサイトをいろいろみても制御周期について詳しく記載されている情報がみつからない（日本語HPのみ確認）。
PID制御周期の高速化検討
十分に早いPWM周期を作れそうな雰囲気になりましたが、
標準50HzのPWMを使用して、50Hzの周期20ms以内にloop()を実行する方針で進めようと思いましたが。。。
50Hzより早いPWMをESCに渡しても、ESCの挙動がどうなるか不明なため（BLheli_sバージョン不明）です。
→その後手持ちESCのBLheli_sがVer16であることが判明、50Hz以上のPWMで動作可。
50HzのPWM周期でプログラムを進めると、これがボトルネックとなり、必然的にPID制御周期も50Hzとなる。多分、この先安定飛行を目指すならPID周期は早いに越したことはないし、50Hzは遅い！
とAIも言っているので、PID周期を高速化するために、PWMプログラムの高速化を志向することにした。
前提として、ESC側が読み取れるPWM信号を出さないといけないので、BLHeli_Sファームについて確認することとした。→ESCのBLheli_sがVer16であることが判明。OneshotやDshotにも対応している！
GPIO直接制御によるPWM高速化
[結果] さまざまなGPIO読み書き方法の速度比較によると、GPIOを直接制御できれば更に50％以上の高速化ができそうなので、やってみる。
ここに詳しくGPIOについて解説してありました。ESP32の低レベルGPIOアクセス その1 デジタル入力・出力
半分も理解できませんが、サンプルプログラムがあるのでこれを参考にやってみる。
ESP-IDFを使ってみる 高速GPIO
いろいろ試すが、GPIO.out_w1tsをコンパイルする際に「GPIOが宣言されていない」というエラーとなる。AIに聞いて、ESP32-WROOM-32D（または32U）などのモデルにおいて GPIO.out_w1ts が使えない場合、「ライブラリのバージョンや環境によっては、構造体への直接アクセスが制限されていることがあります。その場合は、より確実なマクロを使用してください。」とのことでREG_WRITE()を使えとのこと。REG_READ()も使って以下のプログラムを実行。
// 15,2,4,16 PIN にPWM信号を出力させる例
// loop()の実行時間確認用
//PIN0-31用のプログラム
#include <Wire.h>
const int freq_dc = 1000;
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESCへの出力ピン
#define ESC_PIN2motor 2 //ESCへの出力ピン
#define ESC_PIN3motor 4 //ESCへの出力ピン
#define ESC_PIN4motor 16 //ESCへの出力ピン
unsigned long timer = 0;
unsigned long ESCLoopTimer = 0;
unsigned long ESC1_timer = 0;
unsigned long ESC2_timer = 0;
unsigned long ESC3_timer = 0;
unsigned long ESC4_timer = 0;
void setup() {
Serial.begin(115200);// 115200bpsでポートを開く
pinMode(ESC_PIN1motor, OUTPUT);
pinMode(ESC_PIN2motor, OUTPUT);
pinMode(ESC_PIN3motor, OUTPUT);
pinMode(ESC_PIN4motor, OUTPUT);
digitalWrite(ESC_PIN1motor, LOW);
digitalWrite(ESC_PIN2motor, LOW);
digitalWrite(ESC_PIN3motor, LOW);
digitalWrite(ESC_PIN4motor, LOW);
}
void loop() {
timer = micros();
//PWM 15,2,4,16pin => HIGH
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN1motor));
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN2motor));
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN3motor));
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN4motor));
//PWMパルス幅カウンターセット
ESCLoopTimer = micros();
ESC1_timer = 0 + ESCLoopTimer;
ESC2_timer = 10 + ESCLoopTimer;
ESC3_timer = 0 + ESCLoopTimer;
ESC4_timer = 0 + ESCLoopTimer;
//PWM 3,5,6,9pin => LOW
while ((REG_READ(GPIO_IN_REG)&(1 << ESC_PIN1motor)) + (REG_READ(GPIO_IN_REG)&(1 << ESC_PIN2motor)) + (REG_READ(GPIO_IN_REG)&(1 << ESC_PIN3motor)) + (REG_READ(GPIO_IN_REG)&(1 << ESC_PIN4motor)) > 0) {
ESCLoopTimer = micros();
if (ESC1_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN1motor));
if (ESC2_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN2motor));
if (ESC3_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN3motor));
if (ESC4_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN4motor));
}
timer = micros()- timer;
Serial.print("loop実行周期μsは");
Serial.println(timer);//MadgwickFilter実行周波数の残り時間μs
}
１ループ実行最短で2-3μsとなった。
遅くない？ネットで調べる限りもっと早いはず。
AIに聞く→ESP32におけるmicros()関数の実行時間は、クロック周波数240MHzの場合で約0.6〜1.5μs程度（CPUサイクル換算で約150〜300サイクル前後）です。
PWMでは十分に早い応答ですがDshotに挑戦するにあたっては大きすぎる遅れ時間なのでさらに高速化を志向します。
loop()内を以下のようにmicros()のみ残して検証。
void loop() {
timer = micros();
ESCLoopTimer = micros();
timer = micros()- timer;
Serial.print("loop実行周期μsは");
Serial.println(timer);//MadgwickFilter実行周波数の残り時間μs
}
純粋にループ内にmicros();を３回分残して実行→1-2μsとなった。お前か！
AIの提案は続く、
1μs以下の精度で処理時間を計測したい場合は、Xtensaプロセッサ内蔵のサイクルカウンタ (CCOUNT) を直接利用するのが最適です。ESP.getCycleCount()（Arduino環境）またはアセンブリ命令 rsr.ccount を使用します。分解能: 240MHz時、約4.17ns単位での計測が可能です。ESP32において、ESP.getCycleCount() の実行時間は約 10〜20ナノ秒 (ns) 程度です。
下のプログラムに書き換えたところ、タイマー値ゼロでの実行周期は135サイクル→135×4.17=563nsに改善。
タイマー値を240（240MHzでは1μsあたり240サイクル）に変更すると…361サイクル→1505ns。つまり、最大で約500ns(0.5μs）のパルス幅誤差が発生している可能性があります。
この程度の時間幅ならDshotでも十分許容できそうなのでこれでOKとします！
実は、下のプログラムがうまく動かなくて数日足止め食らいました。タイマーが動作せず、While()条件も実行されていないようだった。結果的には…
REG_WRITE(GPIO_OUT_W1TC_REG, ESC_PIN4motor);この構文はダメで、正しくは
REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN4motor));
これが分からず、かなり遊ばせてもらいました。みなさん気を付けて。
実行周期は伸びない？？？
delayMicroseconds(0);をいれただけで、タイマー値ゼロでも297*4.17=1239ns なので600nsほどサイクルが必要なようです。
これでDshotにも挑戦できそうです。
// 指定PIN にPWM信号を出力させる例
// loop()の実行時間確認用
#include <Wire.h>
uint32_t cpuFreq = 0;
const int freq_dc = 1000;
#define MAX_SIGNAL 2000 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 1000 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESC1への出力ピン指定
#define ESC_PIN2motor 2 //ESC2への出力ピン指定
#define ESC_PIN3motor 4 //ESC3への出力ピン指定
#define ESC_PIN4motor 16 //ESC4への出力ピン指定
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t input = 0;
uint32_t ESCLoopTimer =0;
uint32_t ESC1_timer =0;
uint32_t ESC2_timer =0;
uint32_t ESC3_timer =0;
uint32_t ESC4_timer =0;
void setup() {
Serial.begin(115200);// 115200bpsでポートを開く
pinMode(ESC_PIN1motor, OUTPUT);
pinMode(ESC_PIN2motor, OUTPUT);
pinMode(ESC_PIN3motor, OUTPUT);
pinMode(ESC_PIN4motor, OUTPUT);
digitalWrite(ESC_PIN1motor, LOW);
digitalWrite(ESC_PIN2motor, LOW);
digitalWrite(ESC_PIN3motor, LOW);
digitalWrite(ESC_PIN4motor, LOW);
// CPU周波数をMHzで取得
cpuFreq = ESP.getCpuFreqMHz();
}
void loop() {
start = ESP.getCycleCount();//開始時のサイクル数を取得
//PWM 指定pin => HIGH
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN1motor)|BIT(ESC_PIN2motor)|BIT(ESC_PIN3motor)|BIT(ESC_PIN4motor));
//PWMパルス幅カウンターセット
ESCLoopTimer = ESP.getCycleCount();
ESC1_timer = 240 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
ESC2_timer = 0 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
ESC3_timer = 0 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
ESC4_timer = 0 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
input = REG_READ(GPIO_IN_REG);
//PWM 指定pin => LOW
while (input & (BIT(ESC_PIN1motor)|BIT(ESC_PIN2motor)|BIT(ESC_PIN3motor)|BIT(ESC_PIN4motor))) {
ESCLoopTimer = ESP.getCycleCount();
if (ESC1_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN1motor));
if ((ESC2_timer <= ESCLoopTimer)) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN2motor));
if (ESC3_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN3motor));
if (ESC4_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN4motor));
input = REG_READ(GPIO_IN_REG);
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
Serial.print("CPU実行サイクル周波数MHzは");
Serial.print(cpuFreq);
Serial.print(" loop実行サイクル数は");
Serial.println(duration);//MadgwickFilter実行サイクル数240MHzでは4.17ns/cycle
}
PWMをESP32に実装する
GPIOで1μs分解能のPWMができたので、ESP32のPIDプログラムへ実装してみます。
＜躓いたところ＞
Loop実行時間をシリアル表示させる際に、LooP内にシリアル出力を置いてしまい、シリアル自体の実行が非常に長い（μsのオーダーでは）ので全体実行時間が長くなった。→途中変数にカウント値を格納しLoop最後でシリアル表示させた。
Wire.setClock(200000); 200kHzでセンサMPU6500と通信では417000cycle*4.17ns→1.739msかかってしまう。
AIによると、記述のタイミング: 必ず Wire.begin() を呼び出した後に記述する必要があります。Wire.begin() を実行すると設定がデフォルトの 100kHz にリセットされてしまうためです。やってました！
つまり記述ミスで、上100Hzで1.739ms。
記述を直して、
200kHzでセンサMPU6500と通信では232000cycle*4.17ns→0.967ms
400kHzでセンサMPU6500と通信では138400cycle*4.17ns→0.577ms
MPU6500が400kHzまでしか対応していないようなので、ここまでが最高速度です。
Onshot125を想定して最大パルス幅250μsで実行すると
838.3μsで1loop完了、1msまで待ち、PID及びMdwickgフィルタ周期（1kHz）でなんとかいけそうです。
しかし！
PIDゲイン設定のためにシリアル表示をさせるためにさらに高速化。
シリアルは一括表示に変更、これでかなり高速化された。
センサー値は構造体に一括転送に変更したが、構造体のほうが20μsほど遅くなったので元に戻す。
250μs最大パルスのとき、１ループ29μsオーバーする。
DualCpuでセンサー通信を別で実行して高速化志向という手段もあったが、プログラムが複雑になるし、Dshotを考えた時には2バイト送信に100μs必要で、こちらには十分対応できそうなのでこのままいく。
Oneshot125はあきらめて、Oneshot42でPWMプログラムを組みました。
シリアルプロットは1ms周期だと早すぎて評価できないので、10回に一度プロットさせ10msサンプリングにした。
Kiはスキャン周期が10倍になったので1/10とした。
//-------------------------------
#include <MadgwickAHRS.h>
#include <Wire.h>
Madgwick MadgwickFilter;
char buf[64]; // シリアル高速表示用 送信用バッファ（十分なサイズを確保）
float A ;
float B ;
float C ;
float D ;
int counter=0;//定周期シリアル表示用カウンタ
uint32_t cpuFreq = 0;
const int freq_dc = 1000;//ledc周波数指定（スロットルキャリブレーション用）
const int resolution = 10;//ledc分解能bit（スロットルキャリブレーション用）
// Timers
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t t1 =0 ;//サイクル実行時間測定用
uint32_t t1a =0 ;//サイクル実行時間測定用
uint32_t t1b =0 ;//サイクル実行時間測定用
uint32_t t2 =0;//サイクル実行時間測定用
uint32_t t3 =0;//サイクル実行時間測定用
uint32_t t4 =0;//サイクル実行時間測定用
uint32_t t5 =0;//サイクル実行時間測定用
uint32_t input = 0;
uint32_t ESCLoopTimer =0;
uint32_t ESC1_timer =0;
uint32_t ESC2_timer =0;
uint32_t ESC3_timer =0;
uint32_t ESC4_timer =0;
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
//構造体形式のほうが実行遅くなったので不採用
//struct __attribute__((packed)) SensorData {
// int16_t axRaw, ayRaw, azRaw;
// int16_t temperature;
// int16_t gxRaw, gyRaw, gzRaw;
//} data; // ちょうど14バイト
uint32_t timeStep = 240000;//1μs=240 ,240000=1ms, MadgwickFilter SAMPLING time(if100Hz,=1/100)
float SAMPLING_TIME_MS = 1;//PIDsamplingTime
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
//zero set value
float pitch_z = 0;
float roll_z = 0;
float yaw_z = 0;
#define MAX_SIGNAL 85 //PWM信号における最大のパルス幅[マイクロ秒]
#define MIN_SIGNAL 42 //PWM信号における最小のパルス幅[マイクロ秒]
#define ESC_PIN1motor 15 //ESC1への出力ピン指定
#define ESC_PIN2motor 2 //ESC2への出力ピン指定
#define ESC_PIN3motor 4 //ESC3への出力ピン指定
#define ESC_PIN4motor 16 //ESC4への出力ピン指定
int val = MIN_SIGNAL;
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
float cmd[4] = {0.0f, 0.0f, 0.0f};//ESCへの指令（％換算）
// Gain: roll, pitch, yaw
float Kp[3] = {0.5f, 0.5f, 0.0f};//100.0f
float Ki[3] = {0.0003f, 0.0003f, 0.0f};//0.1f
float Kd[3] = {0.02f, 0.02f, 0.0f};//1.0f
// Measured value: roll, pitch, yaw
float rpy_p[3] = {0.0f, 0.0f, 0.0f};
float rpy_i[3] = {0.0f, 0.0f, 0.0f};
float rpy_d[3] = {0.0f, 0.0f, 0.0f};
float rpy_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float pid_rpy[3] = {0.0f, 0.0f, 0.0f};
float degree[3] = {0.0f, 0.0f, 0.0f};
void setup() {
Serial.begin(921600);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
//サイクル時間の経過表示は変数使用して最後一括でシリアル表示させる。
//途中でのシリアル表示は実行周期を致命的に引き延ばす
MadgwickFilter.begin(1000); //if100,100Hz
Wire.begin();
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//Wire.begin();の後に記述しないと100kHzにリセットされる。400kHzが最高 対MPU6500
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(400000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
//Serial.println("Turn on power source, then wait 2 seconds and press any key.");
//while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
//Serial.read();
Serial.println("Turn on ESC power source, before this message then wait 5 seconds. Auto start calibration the ESC");
delay(5000);
// ledc ピンの初期設定 (ピン番号, 周波数, 分解能)
ledcAttach(ESC_PIN1motor, freq_dc, resolution); // ピン, Hz, 10bit(0-1023)
ledcAttach(ESC_PIN2motor, freq_dc, resolution);
ledcAttach(ESC_PIN3motor, freq_dc, resolution);
ledcAttach(ESC_PIN4motor, freq_dc, resolution);
Serial.println("Writing maximum output.");
ledcWrite(ESC_PIN1motor, MAX_SIGNAL*1000/1024);// 出力値（デューティ比）分解能で指定の書き込み
ledcWrite(ESC_PIN2motor, MAX_SIGNAL*1000/1024);//1周期1000μsにつき分解能1024
ledcWrite(ESC_PIN3motor, MAX_SIGNAL*1000/1024);
ledcWrite(ESC_PIN4motor, MAX_SIGNAL*1000/1024);
Serial.println("Wait 12 seconds to caliblate MAX for esc1-4");
delay(12000);//ESC MAXキャリブレーション処理完了のために必要
Serial.println("Writing minimum output");
ledcWrite(ESC_PIN1motor, MIN_SIGNAL*1000/1024);
ledcWrite(ESC_PIN2motor, MIN_SIGNAL*1000/1024);
ledcWrite(ESC_PIN3motor, MIN_SIGNAL*1000/1024);
ledcWrite(ESC_PIN4motor, MIN_SIGNAL*1000/1024);
Serial.println("Wait 10 seconds to caliblate MIN for esc1-4. Then motor starts");
delay(10000);//ESC MINキャリブレーション処理完了のために必要
pinMode(ESC_PIN1motor, OUTPUT);
pinMode(ESC_PIN2motor, OUTPUT);
pinMode(ESC_PIN3motor, OUTPUT);
pinMode(ESC_PIN4motor, OUTPUT);
digitalWrite(ESC_PIN1motor, LOW);
digitalWrite(ESC_PIN2motor, LOW);
digitalWrite(ESC_PIN3motor, LOW);
digitalWrite(ESC_PIN4motor, LOW);
// CPU周波数をMHzで取得
cpuFreq = ESP.getCpuFreqMHz();
//RPYゼロ角度取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
//構造体形式のほうが実行遅くなったので不採用
//if (Wire.available() >= 14) {
// // 14バイトを構造体のメモリ空間に直接コピー
// Wire.readBytes((char*)&data, 14);
//}
// この時点で data.axRaw などで即座に値にアクセス可能
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll_z = MadgwickFilter.getRoll();
pitch_z = MadgwickFilter.getPitch();
yaw_z = MadgwickFilter.getYaw();
}
void loop() {
start = ESP.getCycleCount();//開始時のサイクル数を取得
//PWM 指定pin => HIGH
REG_WRITE(GPIO_OUT_W1TS_REG, BIT(ESC_PIN1motor)|BIT(ESC_PIN2motor)|BIT(ESC_PIN3motor)|BIT(ESC_PIN4motor));
//PWMパルス幅カウンターセット
ESCLoopTimer = ESP.getCycleCount();
ESC1_timer = cmd[0]*240 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなるcmd[0]
ESC2_timer = cmd[1]*240 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
ESC3_timer = cmd[2]*240 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
ESC4_timer = cmd[3]*240 + ESCLoopTimer;//サイクル数でパルス幅指定240MHzの場合は240で1μsとなる
input = REG_READ(GPIO_IN_REG);
//PWM 指定pin => LOW
while (input & (BIT(ESC_PIN1motor)|BIT(ESC_PIN2motor)|BIT(ESC_PIN3motor)|BIT(ESC_PIN4motor))) {
ESCLoopTimer = ESP.getCycleCount();
if (ESC1_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN1motor));
if (ESC2_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN2motor));
if (ESC3_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN3motor));
if (ESC4_timer <= ESCLoopTimer) REG_WRITE(GPIO_OUT_W1TC_REG, BIT(ESC_PIN4motor));
input = REG_READ(GPIO_IN_REG);
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1 = duration;//サイクル実行時間測定用
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
//構造体形式のほうが実行遅くなったので不採用
//if (Wire.available() >= 14) {
// // 14バイトを構造体のメモリ空間に直接コピー
// Wire.readBytes((char*)&data, 14);
//}
// この時点で data.axRaw などで即座に値にアクセス可能
// 加速度値を分解能で割って加速度(G)に変換する
// 加速度はMadgwickFilter内で正規化されるため単位変換不要
//float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
//float ay_g = ayRaw / 16384.0;
//float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
//float temp = (temperature / 333.87f) + 21.0f; // データシートより
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1a = duration;//サイクル実行時間測定用
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw() - yaw_z;
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1b = duration;//サイクル実行時間測定用
degree[0]=roll/360; //角度を百分率換算（100％/360°）
degree[1]=pitch/360;
degree[2]=yaw/360;
for (int i=0; i<3; i++) {
rpy_p[i] = degree[i];
rpy_i[i] += rpy_p[i];
rpy_d[i] = (rpy_p[i] - rpy_pre[i]) / ((float)SAMPLING_TIME_MS/1000);
rpy_pre[i] = rpy_p[i];
if (val <= MIN_SIGNAL) { //出力が最低以下でPID出力ゼロとする
rpy_i[i] = 0;
pid_rpy[i] = 0;
val = MIN_SIGNAL;
}
else if (val >= MAX_SIGNAL){ //出力が最高以上のリミット
val = MAX_SIGNAL;
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
pid_rpy[i] = pid_rpy[i]*42; //制御量（角度百分率）をプロペラ回転数42dgit換算(42μs)
}
else {
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
pid_rpy[i] = pid_rpy[i]*42; //制御量（角度百分率）をプロペラ回転数42dgit換算(42μs)
}
}
cmd[0] = val-pid_rpy[0]-pid_rpy[1]+pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[1] = val+pid_rpy[0]-pid_rpy[1]-pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[2] = val+pid_rpy[0]+pid_rpy[1]+pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[3] = val-pid_rpy[0]+pid_rpy[1]-pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
// Wait to full timeStep period
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t2 = duration;//サイクル実行時間測定用
//受信データがある場合if内を処理
//if処理実行時約1000ms？今後処置必要：注意
//if処理なしで約10μs
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t3 = duration;//サイクル実行時間測定用
//Serial.print(roll); Serial.print(F(","));
//Serial.print(pitch); Serial.print(",");
//Serial.print(az_g); Serial.print(",");
//Serial.print(yaw); Serial.print(",");
//Serial.print(val); Serial.print(","); //出力
counter++;//10回に一回シリアル表示10ms周期プロット
if (counter>=10){
A = Kp[0]*rpy_p[0]*42;
B = Ki[0]*rpy_i[0]*42;
C = Kd[0]*rpy_d[0]*42;
D = pid_rpy[0];
// データを整形してバッファに格納 高速化
sprintf(buf, "%.2f,%.2f,%.2f,%.2f,%.2f", roll,A,B,C,D);
// まとめて送信（1回の呼び出しで済む）
Serial.println(buf);
counter=0;
}
//Serial.print(A); Serial.print(F(","));
//Serial.print(B); Serial.print(F(","));
//Serial.print(C); Serial.print(F(","));
//Serial.print(D); Serial.print(F(","));
//timeStepを超えるまで何もしない
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t4 = duration;//サイクル実行時間測定用
while ( timeStep > duration ){
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
}
t5 = duration;//サイクル実行時間測定用
//Serial.print(" PWM実行サイクル数");
//Serial.print(t1);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(" MPU6500サイクル数");
//Serial.print(t1a);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(" MdWickサイクル数");
//Serial.print(t1b);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(" PID実行サイクル数");
//Serial.print(t2);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(" ifSerial後サイクル数");
//Serial.print(t3);//WAIT完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(" timeStep後サイクル数");
//Serial.print(t4);//WAIT完了サイクル数240MHzでは4.17ns/cycle
//Serial.print(roll); Serial.print(",");
//Serial.print(pitch); Serial.print(",");
//Serial.print(az_g); Serial.print(",");
//Serial.print(yaw); Serial.print(",");
//Serial.print(val); Serial.print(","); //出力
//Serial.print(Kp[0]*rpy_p[0]*1000); Serial.print(",");
//Serial.print(Ki[0]*rpy_i[0]*1000); Serial.print(",");
//Serial.print(Kd[0]*rpy_d[0]*1000); Serial.print(",");
//Serial.print(pid_rpy[0]); Serial.print(",");
//Serial.print(val-pid_rpy[0]-pid_rpy[1]+pid_rpy[2]); Serial.print(",");
//Serial.print(val+pid_rpy[0]-pid_rpy[1]-pid_rpy[2]); Serial.print(",");
//Serial.print(val+pid_rpy[0]+pid_rpy[1]+pid_rpy[2]); Serial.print(",");
//Serial.print(val-pid_rpy[0]+pid_rpy[1]-pid_rpy[2]); Serial.print(",");
//end = ESP.getCycleCount();// 終了時のサイクル数を取得
//duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
//t5 = duration;//サイクル実行時間測定用
//Serial.print(" Serial表示後全loop終了サイクル数");
//Serial.print(t4);Serial.print(",");
//Serial.println(t5);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
}
PIDサイクル1kHz(1ms周期)で実行。シリアルプロットは1サンプリング／10msごとに設定の時間軸。つまり120ms/div、画面いっぱいで480msのグラフ。
モータ制御周期は1ms（1kHz）。
PIDサンプリング10msの時と比べて角度の振れ幅がかなりおさえられている、見た目にも揺れない。
角度の振れ周期も約2/3の時間で収束している。
制御周期が10倍になったので当然といえば、当然の結果か。。
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.0002,Kd=0.02 収束する val=60(回転数50％弱) PID単位はdigit(42digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中
⇛IはPIDサイクル10倍になったので過敏になりすぎたため0.0005→0.0002に調整。安定しているようにみえる。
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.0003,Kd=0.02 収束する val=60(回転数50％弱) PID単位はdigit(42digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中
⇛Iがどうも調子悪そうで0.0002→0.0003に増やす。安定するが、BESTかどうかわからない。
最初から１軸づつ調整したほうがよさそうだ。
いずれにしてもケーブルの拘束から解放しないときちんと評価はできない。
Dshotに移行する
将来的な制御性を向上させるためにデジタル化しておいたほうがよいだろうということで、Dshotを使うことにしました。自分でプログラムを組むことも考えましたが？？？頭がこんがらがりそう。
そんなときAIに尋ねると、
ESP32でDShot（ドローン用ESCのデジタル通信プロトコル）を実装する場合、CPU負荷を抑えつつ正確なタイミングを維持するために、RMT（Remote Control Peripheral）周辺機能を利用するのが一般的。
DShotは極めて精密なパルス幅制御（1bitあたり数マイクロ秒）を必要とするため、通常のGPIO操作（Bit-banging）ではOSの割り込みなどにより信号が不安定になります。RMTを使用すれば、ハードウェアレベルで正確な信号を生成可能。
とのこと。DshotRMTライブラリなるものを推奨された。
知らなかった！こんな便利なライブラリがあるなら、最初から使えばPWMプログラムに何十日も時間を費やす必要なかった。（ハードウェア知識は向上したのでその辺はトレードオフですが）
早速使ってみる。
Arduino IDEのライブラリマネージャーで「DShotRMT」を検索してインストール。
AIに聞きながらプログラミング。。
オブジェクトDshotRMTのピン番号の宣言指定が厳しくてコンパイルエラーを食らったが、AIに聞いて解決。
begin()の引数を認識してくれず（コンパイルエラー）手こずったが、オブジェクト作成時に宣言することで解決。
全部解決しました。
#include <DShotRMT.h>
// モーター接続ピンの定義
// モーターピン指定（GPIO）
gpio_num_t M1_PIN = GPIO_NUM_15;
gpio_num_t M2_PIN = GPIO_NUM_2;
gpio_num_t M3_PIN = GPIO_NUM_4;
gpio_num_t M4_PIN = GPIO_NUM_16;
//定義の指定が厳しく上記、GPIO_NUM_xxをかえるとピン変更できる
// DShotオブジェクトの作成
// モーター4つのインスタンスを静的に確保（ヒープ確保の遅延を防止）
// ここで DSHOT150 を指定します→150はうまくいかず300でESCが認識してくれた
// 引数: (GPIOピン, DShotモード, Bidirectional有効化, モーター極数)
static DShotRMT motors[4] = {
DShotRMT(M1_PIN, DSHOT300 ,false),
DShotRMT(M2_PIN, DSHOT300 ,false),
DShotRMT(M3_PIN, DSHOT300 ,false),
DShotRMT(M4_PIN, DSHOT300 ,false)
};
uint16_t throttle = 0;
void setup() {
Serial.begin(115200);
// DShotの初期化（例：DShot150を使用）
// 第1引数に DSHOT_MODE_150 を指定します
// 第2引数は Bidirectional DShot を使うかどうか（通常は false）
// 引数に指定をおこなうとエラーとなるため
// begin は引数なしで呼び出し、初期化のみ行います
for (int i = 0; i < 4; i++) {
motors[i].begin();
}
Serial.println("Arming All 4 ESCs...");
// アーム操作：全モーターに「0」を送り続ける
// ESCが起動音を出し終わるまで（約2〜3秒）継続
// ESCが「ピッ、ピピッ」と鳴るまで繰り返すのが一般的
for (int i = 0; i < 300; i++) {
motors[0].sendThrottle(0);
motors[1].sendThrottle(0);
motors[2].sendThrottle(0);
motors[3].sendThrottle(0);
delay(10);
}
Serial.println("ESC Armed!");
}
void loop() {
// スロットル値を0〜2000（DShot標準）で制御
// 48〜2047が有効な回転値、0は停止、1〜47は特殊コマンド
// 1. センサー読み取りやPID計算（ここでは省略）
// 2. スロットル値の一括更新
// sendThrottleはRMTハードウェアにデータを投げるだけなので非常に高速（非ブロッキング）
motors[0].sendThrottle(throttle);
motors[1].sendThrottle(throttle);
motors[2].sendThrottle(throttle);
motors[3].sendThrottle(throttle);
// 3. 次の制御サイクルまで待機
// DShot600なら1kHz〜8kHz程度のループも現実的です
delayMicroseconds(500);
//受信データがある場合if内を処理
//if処理実行時約1000ms？今後処置必要：注意
//if処理なしで約10μs
if (Serial.available() > 0) {
delay(10);
throttle = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
}
}
実装のアドバイスまで頂きました。
ピン選定: 使用するピンは、ESP32の「Output Only」ピン（GPIO 34, 35, 36, 39など）を避け、通常のGPIO（12〜33の間など）を使用してください。
電源供給: 4つのモーターを回すと大電流が流れます。ESP32の3.3VピンからESCに電源を供給せず、必ずLiPoバッテリーから直接ESCへ給電し、GNDだけを共通（共通GND）にしてください。
DShotの速度: もし通信エラーが出る場合は、DSHOT600からDSHOT300に下げると安定しやすくなります。
フェイルセーフ: 実装時は、シリアル通信が途切れたりエラーが起きた場合に、全モーターに「0」を送る処理をループ内に入れることをお勧めします。
DSHOT150ではESCがうまく動かず、Arming認識してくれない。AIによるとDSHOT600で試してみるといいとのこと。他記事でも150がうまく動かず300で動いたというものも見ていたので、DSHOT300にするとうまく動いた！
ここまで、数時間でDSHOTパルスが打てるようになった。
AIによると、ELheli_sのそもそもの設定でDSHOT150が使用できない設定になっている可能性もあるとのこと、真相は？？
。。。PWM高速化をGPIO高速でたたいて苦労して作った数十日の苦労は何だったのだろう？便利なライブラリーは皆さん是非使いましょう！
DSHOT300にPIDを組み込んで動作確認
DShotRMT.hライブラリーを使用したPIDプログラムに書き換えました。大分すっきりしました！
実行した結果、motors.sendThrottleで4モータ分書き出しに要したのは約100μsでした。
プログラム実行時間は下のとおり。
6軸センサ読み取りまで ～606μs
MadgwickFilter読み取りとPID計算まで ～632μs
dshot信号処理（sendThrottle）まで ～730μs
シリアル出力除いたloop終了時間まで ～744μs
全て1000μs（1kHz）に収まるので、このまま使えそうです。
更に高速処理（4kHzとか）を目指すには、実行ネックの6軸センサ読み取り（約600μs）を別core CPUで実行するとかする必要がありそう。。ここは、今後必要ならやっていくかもしれません。
//-------------------------------
#include <MadgwickAHRS.h>
#include <Wire.h>
#include <DShotRMT.h>
Madgwick MadgwickFilter;
char buf[64]; // シリアル高速表示用 送信用バッファ（十分なサイズを確保）
float A ;
float B ;
float C ;
float D ;
int counter=0;//定周期シリアル表示用カウンタ
uint32_t cpuFreq = 0;
// Timers
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t t1 =0 ;//サイクル実行時間測定用
uint32_t t2 =0;//サイクル実行時間測定用
uint32_t t3 =0;//サイクル実行時間測定用
uint32_t t4 =0;//サイクル実行時間測定用
uint32_t t5 =0;//サイクル実行時間測定用
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
uint32_t timeStep = 240000;//1μs=240 ,240000=1ms, MadgwickFilter SAMPLING time(if100Hz,=1/100)
float SAMPLING_TIME_MS = 1;//PIDsamplingTime
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
//zero set value
float pitch_z = 0;
float roll_z = 0;
float yaw_z = 0;
// モーターピン指定（GPIO）
gpio_num_t M1_PIN = GPIO_NUM_15;
gpio_num_t M2_PIN = GPIO_NUM_2;
gpio_num_t M3_PIN = GPIO_NUM_4;
gpio_num_t M4_PIN = GPIO_NUM_16;
// DShotオブジェクトの作成
// モーター4つのインスタンスを静的に確保（ヒープ確保の遅延を防止）
// ここで DSHOT150 を指定します→150はうまくいかず300でESCが認識してくれた
// 引数: (GPIOピン, DShotモード, Bidirectional有効化, モーター極数)
static DShotRMT motors[4] = {
DShotRMT(M1_PIN, DSHOT300 ,false),
DShotRMT(M2_PIN, DSHOT300 ,false),
DShotRMT(M3_PIN, DSHOT300 ,false),
DShotRMT(M4_PIN, DSHOT300 ,false)
};
uint16_t throttle[4] = {0, 0, 0, 0};
#define MAX_SIGNAL 100 //dshot 2027digitに相当[%]
#define MIN_SIGNAL 0 //dshot 48digitに相当[%]
int val = MIN_SIGNAL;//valは％で入力（例55は55％）
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
float cmd[4] = {0.0f, 0.0f, 0.0f};//ESCへの指令（％換算）
// Gain: roll, pitch, yaw
float Kp[3] = {0.5f, 0.5f, 0.0f};//100.0f
float Ki[3] = {0.0003f, 0.0003f, 0.0f};//0.1f
float Kd[3] = {0.02f, 0.02f, 0.0f};//1.0f
// Measured value: roll, pitch, yaw
float rpy_p[3] = {0.0f, 0.0f, 0.0f};
float rpy_i[3] = {0.0f, 0.0f, 0.0f};
float rpy_d[3] = {0.0f, 0.0f, 0.0f};
float rpy_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float pid_rpy[3] = {0.0f, 0.0f, 0.0f};
float degree[3] = {0.0f, 0.0f, 0.0f};
void setup() {
Serial.begin(115200);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
//サイクル時間の経過表示は変数使用して最後一括でシリアル表示させる。
//途中でのシリアル表示は実行周期を致命的に引き延ばす
MadgwickFilter.begin(1000); //if100,100Hz
Wire.begin();
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//Wire.begin();の後に記述しないと100kHzにリセットされる。400kHzが最高 対MPU6500
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(400000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
//Serial.println("Turn on power source, then wait 2 seconds and press any key.");
//while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
//Serial.read();
Serial.println("Turn on ESC power source, before this message then wait 5 seconds. Auto start calibration the ESC");
delay(5000);
// DShotの初期化
// begin は引数なしで呼び出し、初期化のみ行います
for (int i = 0; i < 4; i++) {
motors[i].begin();
}
Serial.println("Arming All 4 ESCs...");
// アーム操作：全モーターに「0」を送り続ける
// ESCが起動音を出し終わるまで（約2〜3秒）継続
// ESCが「ピッ、ピピッ」と鳴るまで繰り返すのが一般的
for (int i = 0; i < 5000; i++) {
motors[0].sendThrottle(0);
motors[1].sendThrottle(0);
motors[2].sendThrottle(0);
motors[3].sendThrottle(0);
delay(1);
}
Serial.println("ESC Armed!");
// CPU周波数をMHzで取得
cpuFreq = ESP.getCpuFreqMHz();
//RPYゼロ角度取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll_z = MadgwickFilter.getRoll();
pitch_z = MadgwickFilter.getPitch();
yaw_z = MadgwickFilter.getYaw();
}
void loop() {
start = ESP.getCycleCount();//開始時のサイクル数を取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
// 加速度はMadgwickFilter内で正規化されるため単位変換不要
//float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
//float ay_g = ayRaw / 16384.0;
//float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
//float temp = (temperature / 333.87f) + 21.0f; // データシートより
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1 = duration;//サイクル実行時間測定用
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw() - yaw_z;
degree[0]=roll/360; //角度を百分率換算（100％/360°）
degree[1]=pitch/360;
degree[2]=yaw/360;
for (int i=0; i<3; i++) {
rpy_p[i] = degree[i];
rpy_i[i] += rpy_p[i];
rpy_d[i] = (rpy_p[i] - rpy_pre[i]) / ((float)SAMPLING_TIME_MS/1000);
rpy_pre[i] = rpy_p[i];
if (val <= MIN_SIGNAL) { //出力が最低以下でPID出力ゼロとする
rpy_i[i] = 0;
pid_rpy[i] = 0;
val = MIN_SIGNAL;
}
else if (val >= MAX_SIGNAL){ //出力が最高以上のリミット
val = MAX_SIGNAL;
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
//PID制御量（百分率）0.0〜1.0（パーセント）
}
else {
pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
//PID制御量（百分率）0.0〜1.0（パーセント）
}
}
cmd[0] = val*0.01-pid_rpy[0]-pid_rpy[1]+pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[1] = val*0.01+pid_rpy[0]-pid_rpy[1]-pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[2] = val*0.01+pid_rpy[0]+pid_rpy[1]+pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[3] = val*0.01-pid_rpy[0]+pid_rpy[1]-pid_rpy[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[0] = 48 + cmd[0] * (2047 - 48);//0.0〜1.0（パーセント） を 48〜2047 にスケーリング
cmd[1] = 48 + cmd[1] * (2047 - 48);
cmd[2] = 48 + cmd[2] * (2047 - 48);
cmd[3] = 48 + cmd[3] * (2047 - 48);
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t2 = duration;//サイクル実行時間測定用
// 1. 範囲を48〜2047に制限し、2. uint16_tに変換する
throttle[0] = (uint16_t)constrain(cmd[0], 48, 2047);
throttle[1] = (uint16_t)constrain(cmd[1], 48, 2047);
throttle[2] = (uint16_t)constrain(cmd[2], 48, 2047);
throttle[3] = (uint16_t)constrain(cmd[3], 48, 2047);
// スロットル値の一括更新
// sendThrottleはRMTハードウェアにデータを投げるだけなので非常に高速（非ブロッキング）
motors[0].sendThrottle(throttle[0]);
motors[1].sendThrottle(throttle[1]);
motors[2].sendThrottle(throttle[2]);
motors[3].sendThrottle(throttle[3]);
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t3 = duration;//サイクル実行時間測定用
//受信データがある場合if内を処理
//if処理実行時約1000ms？今後処置必要：注意
//if処理なしで約10μs
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
}
counter++;//10回に一回シリアル表示10ms周期プロット
if (counter>=10){
A = Kp[0]*rpy_p[0]*(2047 - 48);
B = Ki[0]*rpy_i[0]*(2047 - 48);
C = Kd[0]*rpy_d[0]*(2047 - 48);
D = pid_rpy[0]*(2047 - 48);
// データを整形してバッファに格納 高速化
sprintf(buf, "%.2f,%.2f,%.2f,%.2f,%.2f", roll,A,B,C,D);
// まとめて送信（1回の呼び出しで済む）
Serial.println(buf);
counter=0;
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t4 = duration;//サイクル実行時間測定用
//timeStepを超えるまで何もしない
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
while ( timeStep > duration ){
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t5 = duration;//サイクル実行時間測定用
//Serial.print(t1);Serial.print(",");
//Serial.print(t2);Serial.print(",");
//Serial.print(t3);Serial.print(",");
//Serial.print(t4);Serial.print(",");
//Serial.println(t5);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
}
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
Kp=0.5,Ki=0.0003,Kd=0.02 収束する val=45(回転数45％) PID単位はdigit(2000digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中（ケーブルによる拘束あり）
1kHzのonshot42と変わらない動きにみえますが、より細かく制御できているはず。
いよいよドローンを空中に浮かべる下準備が整いました。ただ、いろいろ調べているうちに（AI提案）、PID制御ループに手直しの余地があるようなのでそこらへんを詰めていきます。
ドローン制御にはカスケードPIDが一般的のようです。
2重カスケードPIDに変更する
ドローンの制御構成は、外側の角度制御（アウター・ループ）が目標の「角速度」を計算し、内側の角速度制御（インナー・ループ）が実際の「モーター出力」を決定するカスケード（多重）PID構成にするのが標準的で最も安定します。
ステップ1：角度PID（アウター・ループ）
目標角度と現在の角度の差分から、機体をどれくらいの速さで回すべきか（目標角速度）を決めます。
- 入力:
目標角度–現在角度 - 演算: 主にP制御（比例）のみ、または低めのI項。
- 出力:
目標角速度(deg/s)
ステップ2：角速度PID（インナー・ループ）
角度PIDが出した「目標の速さ」と、実際のジャイロ値を比較して、モーターの強さを決めます。
入力: 目標角速度 – 現在角速度（ジャイロ生値）
演算: PID制御（P, I, Dすべて使用）。ここが機体の安定性の要です。
出力: PID操作量 (PWM等の補正値)
4. 構成のポイント
- サンプリング周期の固定:
ESP32のmicros()を使い、ループの間隔（dt）を一定に保つか、計算式に正確なdtを含めてください。 - 変位（D項）の扱い:
D項（微分）は「角速度のさらなる変化（角加速度）」を抑えるブレーキ役です。ジャイロの生データはノイズが多いため、ローパスフィルタを通してから計算に使うのが一般的です。 - I項の制限（アンチワインドアップ）:
機体が地面に引っかかっている時などにI項が溜まりすぎないよう、iTermには必ず上限（constrain関数等）を設けてください。
実装のポイント
センサー値の取得: 上記コードの current_angle と current_rate には、MPU6050などのIMUから取得した値を代入してください。
サンプリング周期 (dt): ドローンでは制御周期の一定さが命です。delay() ではなく Ticker ライブラリや vTaskDelay を使い、200Hz〜1kHz程度で回すのが理想的です。
アンチワインドアップ: error_sum が増えすぎないよう、constrain 関数などで累積値を制限（積分保護）する処理を実戦では追加します。
変更した点
外側ループの制御単位は角度（°）とした。内側ループの制御単位は角速度(deg/s)とした。
モータ制御量は、48-2047digit/100%にPIDモータ制御量をそのまま加算する。
dtでPIDループ周期変更による単位時間影響をなくした。
アンチワインドアップ: error_sum が増えすぎないよう、constrain 関数などで累積値を制限（積分保護）する処理を実戦では追加します。
//-------------------------------
#include <MadgwickAHRS.h>
#include <Wire.h>
#include <DShotRMT.h>
Madgwick MadgwickFilter;
char buf[64]; // シリアル高速表示用 送信用バッファ（十分なサイズを確保）
float A ;
float B ;
float C ;
float D ;
int counter=0;//定周期シリアル表示用カウンタ
uint32_t cpuFreq = 0;
// Timers
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t t1 =0 ;//サイクル実行時間測定用
uint32_t t2 =0;//サイクル実行時間測定用
uint32_t t3 =0;//サイクル実行時間測定用
uint32_t t4 =0;//サイクル実行時間測定用
uint32_t t5 =0;//サイクル実行時間測定用
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
uint32_t timeStep = 240000;//1μs=240 ,240000=1ms, MadgwickFilter SAMPLING time(if100Hz,=1/100)
float SAMPLING_TIME_MS = 1;//PIDsamplingTime
float dt = (float)SAMPLING_TIME_MS/1000;//PID単位時間
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
//zero set value
float pitch_z = 0;
float roll_z = 0;
float yaw_z = 0;
// モーターピン指定（GPIO）
gpio_num_t M1_PIN = GPIO_NUM_15;
gpio_num_t M2_PIN = GPIO_NUM_2;
gpio_num_t M3_PIN = GPIO_NUM_4;
gpio_num_t M4_PIN = GPIO_NUM_16;
// DShotオブジェクトの作成
// モーター4つのインスタンスを静的に確保（ヒープ確保の遅延を防止）
// ここで DSHOT150 を指定します→150はうまくいかず300でESCが認識してくれた
// 引数: (GPIOピン, DShotモード, Bidirectional有効化, モーター極数)
static DShotRMT motors[4] = {
DShotRMT(M1_PIN, DSHOT300 ,false),
DShotRMT(M2_PIN, DSHOT300 ,false),
DShotRMT(M3_PIN, DSHOT300 ,false),
DShotRMT(M4_PIN, DSHOT300 ,false)
};
uint16_t throttle[4] = {0, 0, 0, 0};
#define MAX_SIGNAL 100 //dshot 2027digitに相当[%]
#define MIN_SIGNAL 0 //dshot 48digitに相当[%]
int val = MIN_SIGNAL;//valは％で入力（例55は55％）
char message[50]; //シリアルモニタへ表示する文字列を入れる変数
float cmd[4] = {0.0f, 0.0f, 0.0f};//ESCへの指令（％換算）
// --- PIDゲインの設定 (機体に合わせて調整が必要) ---
// 外側ループ (角度制御)
// Gain: roll, pitch, yaw
float Kp_sp[3] = {1.0f, 1.0f, 0.0f};//100.0f
float Ki_sp[3] = {0.0f, 0.0f, 0.0f};//0.1f
float Kd_sp[3] = {0.0f, 0.0f, 0.0f};//1.0f
// 内側ループ (角速度制御)
float Kp_out[3] = {0.1f, 0.1f, 0.0f};//100.0f
float Ki_out[3] = {0.0f, 0.0f, 0.0f};//0.1f
float Kd_out[3] = {0.0f, 0.0f, 0.0f};//1.0f
// Measured value: roll, pitch, yaw
float sp_ref_p[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_i[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_d[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_p_pre[3] = {0.0f, 0.0f, 0.0f};
float output_ref_p[3] = {0.0f, 0.0f, 0.0f};
float output_ref_i[3] = {0.0f, 0.0f, 0.0f};
float output_ref_d[3] = {0.0f, 0.0f, 0.0f};
float output_ref_p_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float sp_ref[3] = {0.0f, 0.0f, 0.0f};//角速度指令
float output_ref[3] = {0.0f, 0.0f, 0.0f};//モータ出力
float sp_fb[3] = {0.0f, 0.0f, 0.0f};//角速度実績
float angle_ref[3] = {0.0f, 0.0f, 0.0f};//角度指令
float angle_fb[3] = {0.0f, 0.0f, 0.0f};//角度実績
void setup() {
Serial.begin(115200);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
//サイクル時間の経過表示は変数使用して最後一括でシリアル表示させる。
//途中でのシリアル表示は実行周期を致命的に引き延ばす
MadgwickFilter.begin(1000); //if100,100Hz
Wire.begin();
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//Wire.begin();の後に記述しないと100kHzにリセットされる。400kHzが最高 対MPU6500
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(400000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
//Serial.println("Turn on power source, then wait 2 seconds and press any key.");
//while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
//Serial.read();
Serial.println("Turn on ESC power source, before this message then wait 5 seconds. Auto start calibration the ESC");
delay(5000);
// DShotの初期化
// begin は引数なしで呼び出し、初期化のみ行います
for (int i = 0; i < 4; i++) {
motors[i].begin();
}
Serial.println("Arming All 4 ESCs...");
// アーム操作：全モーターに「0」を送り続ける
// ESCが起動音を出し終わるまで（約2〜3秒）継続
// ESCが「ピッ、ピピッ」と鳴るまで繰り返すのが一般的
for (int i = 0; i < 5000; i++) {
motors[0].sendThrottle(0);
motors[1].sendThrottle(0);
motors[2].sendThrottle(0);
motors[3].sendThrottle(0);
delay(1);
}
Serial.println("ESC Armed!");
// CPU周波数をMHzで取得
cpuFreq = ESP.getCpuFreqMHz();
//RPYゼロ角度取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll_z = MadgwickFilter.getRoll();
pitch_z = MadgwickFilter.getPitch();
yaw_z = MadgwickFilter.getYaw();
}
void loop() {
start = ESP.getCycleCount();//開始時のサイクル数を取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
// 加速度はMadgwickFilter内で正規化されるため単位変換不要
//float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
//float ay_g = ayRaw / 16384.0;
//float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
sp_fb[0]=gx_dps; //角速度（deg/s）
sp_fb[1]=gy_dps;
sp_fb[2]=gz_dps;
//float temp = (temperature / 333.87f) + 21.0f; // データシートより
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1 = duration;//サイクル実行時間測定用
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw() - yaw_z;
angle_fb[0]=roll; //角度（°）
angle_fb[1]=pitch;
angle_fb[2]=yaw;
for (int i=0; i<3; i++) {
// 1. 【外側ループ】目標角度から「目標角速度」を計算
sp_ref_p[i] = angle_fb[i] - angle_ref[i];
sp_ref_i[i] += sp_ref_p[i] * dt;
sp_ref_d[i] = (sp_ref_p[i] - sp_ref_p_pre[i]) / (dt);
sp_ref_p_pre[i] = sp_ref_p[i];
sp_ref[i] = Kp_sp[i]*sp_ref_p[i] + Ki_sp[i]*sp_ref_i[i] + Kd_sp[i]*sp_ref_d[i];
// 2. 【内側ループ】目標角速度から「モーター出力(操作量)」を計算
output_ref_p[i] = sp_ref[i] - sp_fb[i];
output_ref_i[i] += output_ref_p[i] * dt;
output_ref_d[i] = (output_ref_p[i] - output_ref_p_pre[i]) / (dt);
output_ref_p_pre[i] = output_ref_p[i];
if (val <= 48) { //出力が最低以下でPID出力ゼロとする
sp_ref_i[i] = 0;
sp_ref[i] = 0;
output_ref_i[i] = 0;
output_ref[i] = 0;
val = MIN_SIGNAL;
}
else {
val = (uint16_t)constrain(val, 48, 2047);
output_ref[i] = Kp_out[i]*output_ref_p[i] + Ki_out[i]*output_ref_i[i] + Kd_out[i]*output_ref_d[i];
}
}
//0〜100（パーセント） を 48〜2047 digitで出力
cmd[0] = val-output_ref[0]-output_ref[1]+output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[1] = val+output_ref[0]-output_ref[1]-output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[2] = val+output_ref[0]+output_ref[1]+output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[3] = val-output_ref[0]+output_ref[1]-output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t2 = duration;//サイクル実行時間測定用
// 1. 範囲を48〜2047に制限し、2. uint16_tに変換する
throttle[0] = (uint16_t)constrain(cmd[0], 48, 2047);
throttle[1] = (uint16_t)constrain(cmd[1], 48, 2047);
throttle[2] = (uint16_t)constrain(cmd[2], 48, 2047);
throttle[3] = (uint16_t)constrain(cmd[3], 48, 2047);
// スロットル値の一括更新
// sendThrottleはRMTハードウェアにデータを投げるだけなので非常に高速（非ブロッキング）
motors[0].sendThrottle(throttle[0]);
motors[1].sendThrottle(throttle[1]);
motors[2].sendThrottle(throttle[2]);
motors[3].sendThrottle(throttle[3]);
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t3 = duration;//サイクル実行時間測定用
//受信データがある場合if内を処理
//if処理実行時約1000ms？今後処置必要：注意
//if処理なしで約10μs
if (Serial.available() > 0) {
delay(10);
val = Serial.parseInt(); //文字列データを数値に変換
while (Serial.available() > 0) {//受信バッファクリア
char t = Serial.read();
}
}
counter++;//10回に一回シリアル表示10ms周期プロット
if (counter>=10){
//A = Kp_sp[0]*sp_ref_p[0];
//B = Ki_sp[0]*sp_ref_i[0];
//C = Kd_sp[0]*sp_ref_d[0];
//D = sp_ref[0]*(2047 - 48);
A = Kp_out[0]*output_ref_p[0];
B = Ki_out[0]*output_ref_i[0];
C = Kd_out[0]*output_ref_d[0];
D = output_ref[0];
// データを整形してバッファに格納 高速化
sprintf(buf, "%.2f,%.2f,%.2f,%.2f,%.2f", roll,A,B,C,D);
// まとめて送信（1回の呼び出しで済む）
Serial.println(buf);
counter=0;
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t4 = duration;//サイクル実行時間測定用
//timeStepを超えるまで何もしない
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
while ( timeStep > duration ){
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t5 = duration;//サイクル実行時間測定用
//Serial.print(t1);Serial.print(",");
//Serial.print(t2);Serial.print(",");
//Serial.print(t3);Serial.print(",");
//Serial.print(t4);Serial.print(",");
//Serial.println(t5);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
}
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：D成分（黄）、５：PID出力（桃）
角度ループ Kp=1.0,Ki=0.0,Kd=0.0
角速度ループ Kp=0.1,Ki=0.0,Kd=0.0 非常に安定
val=900(900/1999*100=回転数45％) PID単位はdigit(2000digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中（ケーブルによる拘束あり）
モータ出力PID補正値を表示
全表示480ms（10msサンプリング）
下は角度のみ抜粋
PIDゲインは全く詰めていませんが、Kpのみ適当に設定して非常に安定。角度振幅が3°強に収まっており見た目にも今までで一番安定しています。
PIDループも1msに収まっており問題なし。
PIDゲイン調整は、ケーブル拘束から解放されないと意味がないので、二重カスケードPIDプログラムはいったんこれで完成とします。
まずは「角度を維持するだけのシンプルなP制御」から作り始め、徐々にD項やI項を追加していくのがデバッグの近道です。
出力: PID操作量 (PWM等の補正値)
フェイルセーフ: 実装時は、シリアル通信が途切れたりエラーが起きた場合に、全モーターに「0」を送る処理をループ内に入れることをお勧めします。
ESCのPWMプロトコルについて
これは micros() （約1,000ns = 1μs）と比較して、約50〜100倍高速であることを意味します。
分解能: 240MHz時、約4.17ns単位での計測が可能です。
ESCへのPWM信号を作る際に、ESCのサンプリング周期や、BLHeli_Sファームウェアを搭載したESC（8ビットMCU）のプロトコルについて知る必要がでてきたので、調べてまとめてみた。
AIによるとBLHeli_Sのサンプリング周期は、設定とファームウェアのバージョンに依存するが、
デフォルト: 多くのBLHeli_S ESCは、デフォルトで24MHzクロック周期（41.7ns周期）、モータPWMは24kHzまたはそれ以下の可変周波数で動作。十分早くESC側の読み取りを気にする必要はなさそうだ。
ドローンを制御するのに必要なサンプリング周波数は？→4kHzを目指して設定するのがよい。とのこと。
しかし、PWMモードでは50Hzが標準のようで（明確な説明書はみつけられなかった）、それより早いサンプリングは意味がないし挙動の予想がつかない。
FPVドローンのESCファームウェアとプロトコルの複雑さのデコード：詳細な探索によると、ESCには複数のプロトコルがあるということが分かり、
手に入れたESCがBLHeli_S Ver2.3と記載のものだったので、ESC表記の額面通り（Ver2）だとoneshot125等以降Dshotにも対応していないようだった。
ので、当初は、PWM周波数の50Hzに合わせて、50Hzつまり20msでのPIDサンプリング周期を目指すことにしていた。
その後、ESCのBLheli_sがVer16であることを確認でき、oneshot125やDshotにも対応していることが分かり、PID高速化を志向して1kHz以上のPWMで動作を目指すことにした。
当然、OneshotやDshotのプロトコルを理解できていないとPWM信号が作れないので調べたが、以下備忘録。まとめ。
PWMモード
使用可能なスロットル校正範囲は1000μsから2000μsで、最小スロットルと最大スロットルの差は140μs（双方向モードでは70μs）以上である必要があります。差が140μs（70μs）未満で校正が行われた場合、差が140μs（70μs）になるように最大値がシフトされます。
Oneshot125
Oneshot125モードは通常の1～2msモードと全く同じように動作しますが、唯一の違いはすべてのタイミングが8で割られることです。
つまりスロットル校正範囲は125μsから250μs（1000/8=125μs～2000/8=250μs）となる。
Oneshot42
Oneshot42も同様で、すべてのタイミングがOnshot125からさらに3で割られます。
つまりスロットル校正範囲は42μsから84μs（125/3=41.66μs～250/3=83.33μs）となる。
Multishot
マルチショットもOnshotと同様に動作しますが、入力信号の範囲は5～25μsです。
Dshot
入力信号は常にMCUクロック（24MHzまたは48MHz）でサンプリングされます。24MHzで動作するMCUの場合、8kHzを超える入力信号パルスレートは推奨されません。48MHzで動作するMCUの場合、最大32kHzの入力信号パルスレートがサポートされます。ただし、FCのジャイロまたはPIDループよりも速い信号レートは意味がなく、MCUに不要な負荷をかけるだけであることを覚えておいてください。
Dshot150は理論上最大8kHzの入力レートをサポートし、DShot150: 1フレーム約106μsかかるため、理論上は約9.3kHzが限界。
Dshot300は16kHz、Dshot600は32kHzをサポートします。24MHzで動作するMCUはDshot600をサポートしません。一般的に、48MHzのMCUはDshot300で動作させることが推奨されます。これは、Dshot600での高速信号伝送によるメリットよりも、Dshot300の余裕と堅牢性の向上の方が大きいためです。同様に、24MHzのMCUではDshot150が推奨される最大レートです。
入力信号がDshotの場合、スロットルキャリブレーションは無効になり、スロットルキャリブレーション値は無視されます。
DSHOT、ONESHOT125等 モータの制御方法について（ESCプロトコルについて詳しく解説してくれてます）
https://bluerobotics.com/wp-content/uploads/2018/10/BLHeli_S-manual-SiLabs-Rev16.x.pdf：BLHeli_s Ver16の説明書
https://bluerobotics.com/wp-content/uploads/2018/10/BLHeli_S-manual-SiLabs-Rev16.x.pdf
Operation manual for BLHeli SiLabs Rev14.x
BLHeli_sのVer.設定を確認する
AIにBLheli_sの設定読み出し方法を質問。Arduino Uno/Nanoを「1-wireインターフェース」として使用し、BLHeliSuite（またはBLHeliConfigurator）経由でBLHeli_S ESCの設定・フラッシュが可能です。Arduinoに専用スケッチを書き込み、D11ピンをESCの信号線に、GNDを共有して接続します。
ここも少し参考にした。ESCのプログラミング ～Eachine XM10A読出し編～
ちなみにBLHeliSuite32のやつは、試したけどBLheli_sはダメだよ的なメッセージがでてモニターできませんでした。 BLHeli Configuratorは接続方法がなく（フライトコントローラ経由で接続するらしい）、arduino経由での接続できませんでした。
BLHeli_sのモニターをする
接続は4軸あるESCそれぞれの信号GNDと信号線に、ArduinoのGNDとD11を接続した。それぞれの軸を個別につなぎなおして確認しなければいけません。
BLHeliSuiteは、2026年2月現在、Releases · 4712/BLHeliSuiteからDLできました。
BLHeliSuite16714903.zipをダウンロード（最新が2022年、もうオワタシステムです）して、展開→実行！
Win11セキュリティアラート発生！「不明な発行元」らしいです。
いろいろWeb上うろうろ探したけど、結局ここでしかDLできず。
念のためWindowsDefenderでウィルスチェック→問題なし。念には念をいれて
VirusTotalでチェック→「no security vendors flagged this url as malicious」は、セキュリティ分析サイト（主にVirusTotalなど）において、「どのセキュリティベンダーも、このURLを悪意のある（危険な）サイトとしてフラグ（検知）しなかった」→問題なしと判断。
ということで、BLHeliSuite.exeを実行！
https://github.com/bitdump/BLHeli
「Make interfaces」タブでMake Arduino Interface Boardsから、「Uno/ATmega328」を選択（ArduinoUNOなので）する。Portを適切に選択（この時はCOM3）し「Arduino BLHeli Bootloader」をクリック。
sucsessfullyとでれば成功。
Selict ATMEL／SILABS InterfaceタブでSILABS BLHeli Bootloader(USB/Com)を選択。
「Connect」をクリックするとESCと接続されるはずですが、上記エラーでつながらず。Port選択を間違えてCOM1にしてました。
Port:3 としてconnectを押す。Connectが一瞬、Disconnectになるけど、すぐConnectに戻り「Read Setup」を押しても接続されていない的なエラーがでる。なんだか勝手に接続キレてるようなので配線を確認→配線の接触不良のようで、その後Connectを押すと、Disconnectに表示が変わり接続OK。
ReadSetupでESC読み出し成功！
君は！Ver16.7ではないですか！！（多分最新、最終Ver）
見た目にだまされてはいけません。一安心です。
ESP32で無線通信手段を確立する
ドローン製作の過程で、ドローンを無線化する必要があり、簡単にできそうなのでArduinoからESP32に乗り換えました。結果的にはドローン制御にはESP32の能力(高Clock,DualCore)が必要だったとPID開発の過程で分かり、よかったです。
というわけで、WiiリモコンとESP32をつなぐプログラムをサクッとつくります。
ESP32でWiiリモコンを使う方法を参考にしました。
まずArduinoIDEのライブラリから”ESP32Wiimote”を検索しINSTALLします。2026.3現在しっかり更新がはいっているようでした。環境Win11。
ButtonStateの宣言型でいろいろコンパイルエラーが発生し、GitHubの配布元のサンプルプログラムを参考に下のプログラムでコンパイルOK。
Wiiリモコンの電池カバー内のSYNCボタンを押すと接続できるようです。
#include "ESP32Wiimote.h"
// Timers
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t t1 =0 ;//サイクル実行時間測定用
ESP32Wiimote wiimote;
void setup()
{
Serial.begin(115200);
wiimote.init();
}
void loop()
{
start = ESP.getCycleCount();//開始時のサイクル数を取得
wiimote.task();
if (wiimote.available() > 0)
{
ButtonState button = wiimote.getButtonState();
if (buttonStateHas(button, kButtonA)) {
Serial.println("A pressed");
}
if (buttonStateHas(button, kButtonB)) {
Serial.println("B pressed");
}
if (buttonStateHas(button, kButtonLeft)) {
Serial.println("← pressed");
}
if (buttonStateHas(button, kButtonRight)) {
Serial.println("→ pressed");
}
if (buttonStateHas(button, kButtonUp)) {
Serial.println("↑ pressed");
}
if (buttonStateHas(button, kButtonDown)) {
Serial.println("↓ pressed");
}
if (buttonStateHas(button, kButtonHome)) {
Serial.println("HOME pressed");
}
if (buttonStateHas(button, kButtonPlus)) {
Serial.println("+ pressed");
}
if (buttonStateHas(button, kButtonMinus)) {
Serial.println("- pressed");
}
if (buttonStateHas(button, kButtonOne)) {
Serial.println("1 pressed");
}
if (buttonStateHas(button, kButtonTwo)) {
Serial.println("2 pressed");
}
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1 = duration;//サイクル実行時間測定用
Serial.println(t1);
delay(10);
}
Wiiリモコンとの接続、全ボタンの取り込み確認できました！
ループ実行周期はボタン押さずに666*4.17=2777ns(2.7μs)で十分早いです。
ボタン全押し8か所シリアル表示で70867*4.17=295515ns(296μs)でした。実装時にはボタン押すたびにシリアル表示はさせないので全く問題ありません。
ドローン操縦での主要な2つの操作モード
AIによるとモード２が主流の操作方法で、直感的に操作できるらしい。
現在、世界的には「モード2」が標準的ですが、日本ではラジコン時代の名残で「モード1」も広く普及しています。
モード1のメリット: 日本のベテラン操縦士や登録講習機関（ドローンスクール）で多く採用されているため、教わりやすい環境があります
モード2のメリット: 右スティックだけで「前後左右」の移動が完結するため、ゲーム機のコントローラーに近く直感的に操作できます。
モード1のメリット: 日本のベテラン操縦士や登録講習機関（ドローンスクール）で多く採用されているため、教わりやすい環境があります
| 操作箇所 | スティックの動き | モード1（日本で主流） | モード2（世界標準） |
|---|---|---|---|
| 左スティック | 上下 | エレベーター (前進・後退) |
スロットル (上昇・下降) |
| 左右 | ラダー (左旋回・右旋回) |
ラダー (左旋回・右旋回) |
|
| 右スティック | 上下 | スロットル (上昇・下降) |
エレベーター (前進・後退) |
| 左右 | エルロン (左移動・右移動) |
エルロン (左移動・右移動) |
モード2ボタンを割り当て
Wiiリモコンにドローン操作を割り当てます。
縦持ちでつかうとして、直感的な操作となるように
十字キーにモード２の右スティック機能を割り当て、
ABボタンに上昇下降、+-ボタンに旋回機能を割り当てます。
今のところ用途のないHOME、1，2ボタンは非常停止用とします。
#include "ESP32Wiimote.h"
ESP32Wiimote wiimote;
void setup()
{
Serial.begin(115200);
wiimote.init();
}
void loop()
{
wiimote.task();
if (wiimote.available() > 0)
{
ButtonState button = wiimote.getButtonState();
if (buttonStateHas(button, kButtonA)) {
Serial.println("A:上昇");
}
if (buttonStateHas(button, kButtonB)) {
Serial.println("B:下降");
}
if (buttonStateHas(button, kButtonLeft)) {
Serial.println("←:左移動");
}
if (buttonStateHas(button, kButtonRight)) {
Serial.println("→:右移動");
}
if (buttonStateHas(button, kButtonUp)) {
Serial.println("↑:前進");
}
if (buttonStateHas(button, kButtonDown)) {
Serial.println("↓:後退");
}
if (buttonStateHas(button, kButtonHome)) {
Serial.println("HOME:非常停止");
}
if (buttonStateHas(button, kButtonPlus)) {
Serial.println("+:右旋回");
}
if (buttonStateHas(button, kButtonMinus)) {
Serial.println("-:左旋回");
}
if (buttonStateHas(button, kButtonOne)) {
Serial.println("1:非常停止");
}
if (buttonStateHas(button, kButtonTwo)) {
Serial.println("2:非常停止");
}
}
delay(10);
}
これをドローン制御PIDに組み込んでみます。
| 操作内容 | モード2 (世界標準・直感的) | モード1 (日本で普及) |
|---|---|---|
| 左スティック (上下) | スロットル (上昇・下降) | エレベーター (前進・後退) |
| 左スティック (左右) | ラダー (左右旋回) | ラダー (左右旋回) |
| 右スティック (上下) | エレベーター (前進・後退) | スロットル (上昇・下降) |
| 右スティック (左右) | エルロン (左右移動) | エルロン (左右移動) |
PIDプログラムにWiiリモコンを組み込む（ESP32）
Wiiリモコン取り込みはおそらく1ms通りにサンプリングされておらず、ゲームレート60fpsに合わせているようで、実際に取り込み加算処理の数値を多めにとる必要があります。
角度指令（前進後進、右移動左移動）もリモコンから取り込めるようにしましたが、ケーブルに拘束されて動作確認がうまくできません。
PIDゲインも簡単に調整しましたが、やはりケーブル拘束されて動作がしっかり確認できないので、ドローン飛行可能状態に組みなおしをします。
#include <MadgwickAHRS.h>
#include <Wire.h>
#include <DShotRMT.h>
#include "ESP32Wiimote.h"
ESP32Wiimote wiimote;//Wiiリモコン用
Madgwick MadgwickFilter;//MadgwickFilter
char buf[64]; // シリアル高速表示用 送信用バッファ（十分なサイズを確保）
float A ;
float B ;
float C ;
float D ;
int counter=0;//定周期シリアル表示用カウンタ
uint32_t cpuFreq = 0;
// Timers
uint32_t start;
uint32_t end;
uint32_t duration = 0;
uint32_t t1 =0 ;//サイクル実行時間測定用
uint32_t t2 =0;//サイクル実行時間測定用
uint32_t t3 =0;//サイクル実行時間測定用
uint32_t t4 =0;//サイクル実行時間測定用
uint32_t t5 =0;//サイクル実行時間測定用
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;
uint32_t timeStep = 240000;//1μs=240 ,240000=1ms, MadgwickFilter SAMPLING time(if100Hz,=1/100)
float SAMPLING_TIME_MS = 1;//PIDsamplingTime
float dt = (float)SAMPLING_TIME_MS/1000;//PID単位時間
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
//zero set value
float pitch_z = 0;
float roll_z = 0;
float yaw_z = 0;
// モーターピン指定（GPIO）
gpio_num_t M1_PIN = GPIO_NUM_15;
gpio_num_t M2_PIN = GPIO_NUM_2;
gpio_num_t M3_PIN = GPIO_NUM_4;
gpio_num_t M4_PIN = GPIO_NUM_16;
// DShotオブジェクトの作成
// モーター4つのインスタンスを静的に確保（ヒープ確保の遅延を防止）
// ここで DSHOT150 を指定します→150はうまくいかず300でESCが認識してくれた
// 引数: (GPIOピン, DShotモード, Bidirectional有効化, モーター極数)
static DShotRMT motors[4] = {
DShotRMT(M1_PIN, DSHOT300 ,false),
DShotRMT(M2_PIN, DSHOT300 ,false),
DShotRMT(M3_PIN, DSHOT300 ,false),
DShotRMT(M4_PIN, DSHOT300 ,false)
};
uint16_t throttle[4] = {0, 0, 0, 0};//ESCへの指令（48-2027 uint）
#define MAX_SIGNAL 2027 //dshot 2027digitに相当[%]
#define MIN_SIGNAL 48 //dshot 48digitに相当[%]
float throttle_ref = MIN_SIGNAL;//スロットル
float cmd[4] = {0.0f, 0.0f, 0.0f};//ESCへの指令（float）
// --- PIDゲインの設定 (機体に合わせて調整が必要) ---
// 外側ループ (角度制御)
// Gain: roll, pitch, yaw
float Kp_sp[3] = {1.0f, 1.0f, 0.0f};//100.0f
float Ki_sp[3] = {0.5f, 0.5f, 0.0f};//0.1f
float Kd_sp[3] = {0.0f, 0.0f, 0.0f};//1.0f
// 内側ループ (角速度制御)
float Kp_out[3] = {0.1f, 0.1f, 0.0f};//100.0f
float Ki_out[3] = {0.0f, 0.0f, 0.0f};//0.1f
float Kd_out[3] = {0.001f, 0.001f, 0.0f};//1.0f
// Measured value: roll, pitch, yaw
float sp_ref_p[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_i[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_d[3] = {0.0f, 0.0f, 0.0f};
float sp_ref_p_pre[3] = {0.0f, 0.0f, 0.0f};
float output_ref_p[3] = {0.0f, 0.0f, 0.0f};
float output_ref_i[3] = {0.0f, 0.0f, 0.0f};
float output_ref_d[3] = {0.0f, 0.0f, 0.0f};
float output_ref_p_pre[3] = {0.0f, 0.0f, 0.0f};
// Calculated value: roll, pitch, yaw
float sp_ref[3] = {0.0f, 0.0f, 0.0f};//角速度指令
float output_ref[3] = {0.0f, 0.0f, 0.0f};//モータ出力
float sp_fb[3] = {0.0f, 0.0f, 0.0f};//角速度実績
float angle_ref[3] = {0.0f, 0.0f, 0.0f};//角度指令
float angle_fb[3] = {0.0f, 0.0f, 0.0f};//角度実績
void setup() {
Serial.begin(115200);// if9600,9600bpsでポートを開く
//9600メインループタイムオーバとなりシリアルモニタ表示できない
//サイクル時間の経過表示は変数使用して最後一括でシリアル表示させる。
//途中でのシリアル表示は実行周期を致命的に引き延ばす
wiimote.init();//wiimoteイニシャル
MadgwickFilter.begin(1000); //if100,100Hz
Wire.begin();
// if"400000",400kHz I2C clock. Comment this line if having compilation difficulties
//Wire.begin();の後に記述しないと100kHzにリセットされる。400kHzが最高 対MPU6500
//MadgwickFilterサンプリングより早い必要があるが不要に早くしない
Wire.setClock(400000);
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x80); //0x80でオールレジスタリセット（初期化処理）
Wire.endTransmission();
delay(100);//安定待ち時間がないとバグる（めちゃめちゃ大事です！）
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0x00); //0x00で起動処理
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1C);
Wire.write(0x00); //加速度設定FS_SEL_0 16,384 LSB / g
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1B);
Wire.write(0x00); //角速度設定131LSB/(degree/s)
Wire.endTransmission();
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x05);
Wire.endTransmission();
Serial.println("Program begin...");
Serial.println("This program will calibrate the ESC.");
//Serial.println("Turn on power source, then wait 2 seconds and press any key.");
//while (!Serial.available()); //シリアルポートで何か入力されるまで待ちます
//Serial.read();
Serial.println("Turn on ESC power source, before this message then wait 5 seconds. Auto start calibration the ESC");
delay(5000);
// DShotの初期化
// begin は引数なしで呼び出し、初期化のみ行います
for (int i = 0; i < 4; i++) {
motors[i].begin();
}
Serial.println("Arming All 4 ESCs...");
// アーム操作：全モーターに「0」を送り続ける
// ESCが起動音を出し終わるまで（約2〜3秒）継続
// ESCが「ピッ、ピピッ」と鳴るまで繰り返すのが一般的
for (int i = 0; i < 5000; i++) {
motors[0].sendThrottle(0);
motors[1].sendThrottle(0);
motors[2].sendThrottle(0);
motors[3].sendThrottle(0);
delay(1);
}
Serial.println("ESC Armed!");
// CPU周波数をMHzで取得
cpuFreq = ESP.getCpuFreqMHz();
//RPYゼロ角度取得
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll_z = MadgwickFilter.getRoll();
pitch_z = MadgwickFilter.getPitch();
yaw_z = MadgwickFilter.getYaw();
}
void loop() {
start = ESP.getCycleCount();//開始時のサイクル数を取得
wiimote.task();
if (wiimote.available() > 0){
ButtonState button = wiimote.getButtonState();
if (buttonStateHas(button, kButtonA)) {
throttle_ref = throttle_ref + 5 ;//1s毎に60回加算されるdigit(2000dig/100%)60fps
//Serial.println("A:上昇");
}
if (buttonStateHas(button, kButtonB)) {
throttle_ref = throttle_ref - 5 ;//1s毎に60回減算するdigit(2000dig/100%)
//Serial.println("B:下降");
}
if (buttonStateHas(button, kButtonLeft)) {
angle_ref[0] = angle_ref[0] - 0.05;//1s毎に60回加算されるdigit(2000dig/100%)60fps
//Serial.println("←:左移動");
}
if (buttonStateHas(button, kButtonRight)) {
angle_ref[0] = angle_ref[0] + 0.05;
//Serial.println("→:右移動");
}
if (buttonStateHas(button, kButtonUp)) {
angle_ref[1] = angle_ref[1] - 0.05;
//Serial.println("↑:前進");
}
if (buttonStateHas(button, kButtonDown)) {
angle_ref[1] = angle_ref[1] + 0.05;
//Serial.println("↓:後退");
}
if (buttonStateHas(button, kButtonHome)) {
throttle_ref = throttle_ref - 100 ;//非常停止は下方stepへ（後どり優先）
//Serial.println("HOME:非常停止");
}
if (buttonStateHas(button, kButtonPlus)) {
angle_ref[2] = angle_ref[2] + 0.2;
//Serial.println("+:右旋回");
}
if (buttonStateHas(button, kButtonMinus)) {
angle_ref[2] = angle_ref[2] - 0.2;
//Serial.println("-:左旋回");
}
if (buttonStateHas(button, kButtonOne)) {
throttle_ref = throttle_ref - 100 ;//1ms毎に減算するdigit(2000dig/100%)
//Serial.println("1:非常停止");
}
if (buttonStateHas(button, kButtonTwo)) {
throttle_ref = throttle_ref - 100 ;//1ms毎に減算するdigit(2000dig/100%)
//Serial.println("2:非常停止");
}
}
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0x68, 14);
while (Wire.available() < 14);
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
// 加速度はMadgwickFilter内で正規化されるため単位変換不要
//float ax_g = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
//float ay_g = ayRaw / 16384.0;
//float az_g = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gx_dps = gxRaw / 131.0; //degree / s へ変換
float gy_dps = gyRaw / 131.0;
float gz_dps = gzRaw / 131.0;
sp_fb[0]=gx_dps; //角速度（deg/s）
sp_fb[1]=gy_dps;
sp_fb[2]=gz_dps;
//float temp = (temperature / 333.87f) + 21.0f; // データシートより
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t1 = duration;//サイクル実行時間測定用
MadgwickFilter.updateIMU(gx_dps, gy_dps, gz_dps, axRaw, ayRaw, azRaw);
roll = MadgwickFilter.getRoll();
pitch = MadgwickFilter.getPitch();
yaw = MadgwickFilter.getYaw() - yaw_z;
angle_fb[0]=roll; //角度（°）
angle_fb[1]=pitch;
angle_fb[2]=yaw;
for (int i=0; i<3; i++) {
// 1. 【外側ループ】目標角度から「目標角速度」を計算
sp_ref_p[i] = angle_fb[i] - angle_ref[i];
sp_ref_i[i] += sp_ref_p[i] * dt;
sp_ref_d[i] = (sp_ref_p[i] - sp_ref_p_pre[i]) / (dt);
sp_ref_p_pre[i] = sp_ref_p[i];
sp_ref[i] = Kp_sp[i]*sp_ref_p[i] + Ki_sp[i]*sp_ref_i[i] + Kd_sp[i]*sp_ref_d[i];
// 2. 【内側ループ】目標角速度から「モーター出力(操作量)」を計算
output_ref_p[i] = sp_ref[i] - sp_fb[i];
output_ref_i[i] += output_ref_p[i] * dt;
output_ref_d[i] = (output_ref_p[i] - output_ref_p_pre[i]) / (dt);
output_ref_p_pre[i] = output_ref_p[i];
if (throttle_ref <= 48) { //出力が最低以下でPID出力ゼロとする
sp_ref_i[i] = 0;
sp_ref[i] = 0;
angle_ref[i] = 0;
output_ref_i[i] = 0;
output_ref[i] = 0;
throttle_ref = MIN_SIGNAL;
}
else {
throttle_ref = (float)constrain(throttle_ref, 48, 2047);
output_ref[i] = Kp_out[i]*output_ref_p[i] + Ki_out[i]*output_ref_i[i] + Kd_out[i]*output_ref_d[i];
}
}
//0〜100（パーセント） を 48〜2047 digitで出力
cmd[0] = throttle_ref-output_ref[0]-output_ref[1]+output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[1] = throttle_ref+output_ref[0]-output_ref[1]-output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[2] = throttle_ref+output_ref[0]+output_ref[1]+output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
cmd[3] = throttle_ref-output_ref[0]+output_ref[1]-output_ref[2]; // パルス幅 `val` のPWM信号を送信する[マイクロ秒]
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t2 = duration;//サイクル実行時間測定用
// 1. 範囲を48〜2047に制限し、2. uint16_tに変換する
throttle[0] = (uint16_t)constrain(cmd[0], 48, 2047);
throttle[1] = (uint16_t)constrain(cmd[1], 48, 2047);
throttle[2] = (uint16_t)constrain(cmd[2], 48, 2047);
throttle[3] = (uint16_t)constrain(cmd[3], 48, 2047);
// スロットル値の一括更新
// sendThrottleはRMTハードウェアにデータを投げるだけなので非常に高速（非ブロッキング）
motors[0].sendThrottle(throttle[0]);
motors[1].sendThrottle(throttle[1]);
motors[2].sendThrottle(throttle[2]);
motors[3].sendThrottle(throttle[3]);
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t3 = duration;//サイクル実行時間測定用
counter++;//10回に一回シリアル表示10ms周期プロット
if (counter>=10){
A = Kp_sp[0]*sp_ref_p[0];
B = Ki_sp[0]*sp_ref_i[0];
//C = Kd_sp[0]*sp_ref_d[0];
C = sp_ref[0];
//A = Kp_out[0]*output_ref_p[0];
//B = Ki_out[0]*output_ref_i[0];
//C = Kd_out[0]*output_ref_d[0];
D = output_ref[0];
//A = angle_ref[0];
//C = angle_ref[2];
// データを整形してバッファに格納 高速化
sprintf(buf, "%.2f,%.2f,%.2f,%.2f,%.2f", roll,A,B,C,D);
// まとめて送信（1回の呼び出しで済む）
Serial.println(buf);
counter=0;
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t4 = duration;//サイクル実行時間測定用
//timeStepを超えるまで何もしない
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
while ( timeStep > duration ){
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
}
end = ESP.getCycleCount();// 終了時のサイクル数を取得
duration = end - start;// 差分を計算（32ビットのオーバーフローも自動で考慮される）
t5 = duration;//サイクル実行時間測定用
//Serial.print(t1);Serial.print(",");
//Serial.print(t2);Serial.print(",");
//Serial.print(t3);Serial.print(",");
//Serial.print(t4);Serial.print(",");
//Serial.println(t5);//待ち時間完了サイクル数240MHzでは4.17ns/cycle
}
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：P+D成分（黄）、５：PIDモータ出力（桃）
角度ループ Kp=1.0,Ki=0.5,Kd=0.0
角速度ループ Kp=0.1,Ki=0.0,Kd=0.001 安定だが発散する
スロットル不明 PID単位はdigit(2000digit/100%)
グラフはrollのみ、pitchも同ゲインで制御中（ケーブルによる拘束あり）
モータ出力PID補正値を表示
全表示480ms（10msサンプリング）
完全に離陸しないとこれ以上評価できません。
ここまでのPINアサイン
| 接続先デバイス | ESP32 ピン (GPIO) | 役割 |
|---|---|---|
| 電源 (入力) | VIN (5V) / GND | 外部(ESC等)から5Vを供給 |
| IMU (MPU6500センサー) | D21 (SDA) / D22 (SCL) | 姿勢制御用のデータを取得 |
| モーター1 (PWM) | D15 | 右前モーターの速度制御 |
| モーター2 (PWM) | D2 | 左前モーターの速度制御 |
| モーター3 (PWM) | D4 | 左後モーターの速度制御 |
| モーター4 (PWM) | D16 | 右後モーターの速度制御 |
| Bluetooth (Wiiリモコン) | (内臓) | Wiiリモコン（送信機）からの信号受信用 |
モータ1とモータ3をリバースに変更し、回転方向を合わせる。
双方向モード？
初号機完成
飛ばしてみたが、くるくるYaw左方向に回る。ロール左傾き安定しない。
PID調整が必要だ。
さらに、ESCとESP32を電源でつないだせいか、バッテリーをつながないとESP32が起動しなくなった。
USBのみではESP32起動しない。これは起動時に大電流をとられている可能性があってPC側の電源がシャットアウトしているかもしれない。試しに、シリアルとESP32単体に繋いだら、正常に電源が入った。
ESC側からESP32電源をもらう構成が、バッテリーなしだとESCに電流取られて起動できないようだ。電源のつながりにSW等を入れれば解決するかもしれない。
PIDのKpから調整。
適当に角度Kp＝10、速度Kp＝10でやったら、吹っ飛んできてプロペラで指が深く切れた、調整方法十分注意してください）
仕切り直して、内ループから調整。外ループのゲインは全てゼロにしておく。
内ループRollのKpを調整。Roll軸のみ動くように固定し、調整実施。
シリアルモニタは4ｍｓ、これより早い（3ｍｓより早い）とループが致命的に伸びる。なぜか10ループごとに10ms実行に要すloopが発生する。原因は不明だが、何かの割り込み処理（シリアル出力に関して）が入るのだとおもう。
1：roll角度deg（青）、２：P成分（橙）、３：I成分（緑）、４：P+D成分（黄）、５：PIDモータ出力（桃）
角度ループ Kp=5,Ki=0,Kd=0.0
角速度ループ Kp=0.32,Ki=0.0,Kd=0.002 安定だが発散する
スロットル 948digit(2000digit/100%)
グラフはrollのみ、roll軸で固定
モータ出力PID補正値を表示
全表示480ms（10msサンプリング）
角速度ループ：注意として確実に機体本体が浮上し不要な拘束力から解き放たれないと正確な挙動は分からない。ゲイン調整するスロットル出力に注意する。
Kp0.32付近が安定する。ゲインが確実に効いているが偏差が減るとゲイン効きが弱まって結果的に制動力が弱いようなので、微分成分を追加。0.003より0.002で安定、傾きが大きくなると発散動作になるが水平付近では安定している。
角度ループの調整。Kp＝1で悪影響を与えない程度に効くが、浮上すると発散気味。比例分はあまり効かずに、積分成分に効いてもらうほうがよさそう。