電子工作でドローンを飛ばしたい！
子供のおもちゃのドローンしか触ったことのない素人が、自作でドローンを作るまでの物語です。たくさん失敗してます。みてやってください。まだ完成していません！！！
これから始めようと思っている方に見ていただければ幸いです！
電子工作への興味
何年も前から電子工作やってみたい！と画策しており、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】
六軸センサーから姿勢角度算出
上記サイトで9軸センサを使い姿勢推定プログラムを作成する？
ブラシレスモータをESCで動かす
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でサーボモーターを複数同時に動かす、メカを雑に作るならタミヤのユニバーサルアーム＆ユニバーサルプレート【動画解説】
接続図
| 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を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 で動かす方法
ArduinoでPCから、シリアル通信で数値を送る (Serial.parseInt())
姿勢情報でモータをコントロールし安定飛行させる
PID制御に当たっては、こちらのサイト⇛姿勢計測 + PID制御で光量調節（ドローン製作準備，esp32，Arduino IDE）を非常に参考にさせていただきました。ありがとうございます。
躓いたところ
角度偏差（360°に対する百分率）に対して、PID制御量をプロペラ回転数1000μs（スロットル操作幅）にスケーリングした。
高速回転になると、なぜか角度取得値に誤差が発生し、水平なのに傾いてる認識となった、これによりI成分が悪く働きどんどん傾いていく。I成分は使えない。誤差発生要因は？振動による誤差蓄積か？？
⇛ I のゲインを適切に調整して、水平を保つ動作をさせるとドリフト影響なくなる。
これは機体振動がない状態だと、ドリフト影響をうけ、機体がある程度動くとドリフト影響が軽減される？
Madgwickの仕様か？角速度方向が正逆切り替わるとドリフト補正働くのか？
パルスDuty1500μs（速度50％スロットル）程度まで回転数を上げないと機体が浮かないしrollの動きもでないのでゲイン調整する回転数は大切です。50％スロットルで調整しても、50-100％まで安定して制御できるかは確認しないとわからない。
ゲイン調整のとき、ゲインを下げすぎると機体が左右に振れる動きが角度偏差を揺らしてオーバーゲインのように勘違いしたが、実際はゲイン不足により機体が左右に触れいている。
※のちのちに気づいたことですが、モータPWMは50Hz,20msでの制御ループであり、ドローンの制御には厳しい、1ｋHzはあるほうがよい、ようだ。
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にもいれる
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％)
グラフはrollのみ、pitchも同ゲインで制御中
⇛Iを増やすとIの蓄積が不安定をもたらすので、Iを減らし50％で比較的安定。60％でも安定するが、横揺れ発生はする。もう少し、ゲインPIの増加が必要か？ただし、プロペラガード等の物理保護がないと危なくて試験ができない状態。
固定した状態だと、評価継続不可なので、バッテリー購入と無線通信する手段を検討。
ＮｉＣｄバッテリー243ｇ、ドローンテスト機90ｇ弱
7.2ＶのＮｉＣｄバッテリー最大スロットルで約200ｇ浮上。機体重量を200ｇより軽く設定するようにバッテリーを選定。3Ｓ（11Ｖ）、66ｇを購入。
無線化するのに、ＥＰＳ32が格安コスパ最強+arduino開発環境で使用可能なのでこちらに移行することにした。
#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
}
ECS32で無線通信手段を確立する
家に余ってる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を指定しているようなので、変更しアップロード→完了
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を使うことをあきらめる方針に。
4軸のタイムリー制御（更なる高みへ）※未着手
ドローンの自作｜姿勢制御プログラム（ブラシレスモーター制御編）（4軸同時パルス出力：高速応答を志向している）….
ESP32で複数の独立したPWM信号を出力する
複数のサーボを動作させる際の注意点メモ
Arduino – ESP32 の PWM ( LEDC )で 40MHzまでの安定した高周波パルスを思い通りに出せたぞ
BLHeli_Sファームウェアを搭載したESC（8ビットMCU）のサンプリング周期（PWM周波数）は、主に以下の設定とファームウェアのバージョンに依存します。デフォルト: 多くのBLHeli_S ESCは、デフォルトで24kHzまたはそれ以下の可変周波数で動作します。
ドローンを制御するのに必要なサンプリング周波数は？ときくと4kHzを目指して設定するのがよい。しかし、PWMモードでは50Hzが標準のようで（明確な説明書はみつけられなかった）、それより早いサンプリングは意味がないし挙動の予想がつかない。ので、PWM周波数の50Hzに合わせて、50Hzつまり20msより早いPIDサンプリング周期を目指すことにした。
FPVドローンのESCファームウェアとプロトコルの複雑さのデコード：詳細な探索
手に入れたBLHeli_S Ver2.3と記載のESCは額面通りだとoneshot125等以降Dshotにも対応していないようだが、実際にVer情報を読み取って確認してみないと明確なことはいえない。兎に角、ここに踏み込んでいくと沼なので、PWMでドローンを飛ばす！必然的に50Hzで飛ばす！という方針ですすめることとする。
DSHOT、ONESHOT125等 モータの制御方法について
Input signal:
https://bluerobotics.com/wp-content/uploads/2018/10/BLHeli_S-manual-SiLabs-Rev16.x.pdf
Available throttle calibration range is from 1000us to 2000us, and the difference between minimum
and maximum throttle must be more than 140us (70us in bidirectional mode). If a calibration is
done where the difference is less than 140us (70us), the maximum will be shifted so that the
difference is 140us (70us).
Oneshot125 mode works just the same as regular 1-2ms mode, the only difference is that all timing
is divided by 8. And the same for Oneshot42, where all timing is further divided by 3. Multishot
also works similarly, except the input signal range is 5-25us.
The input signal is always sampled with the MCU clock, at 24MHz or 48MHz.
For MCUs running at 24MHz, input signal pulse rates above 8kHz are not recommended.
For MCUs running at 48MHz, input signal pulse rates up to 32kHz are supported.
But please remember that signal rates faster than the gyro or PID loop of the FC does not make
sense, it only results in unnecessary loading of the MCU.
Dshot150 theoretically supports up to 8kHz input rates, Dshot300 supports 16kHz and Dshot600
32kHz. MCUs running at 24MHz do not support Dshot600. Generally it is recommended to run
48MHz MCUs at Dshot300, as the benefit from a higher signalling speed at Dshot600 is outweighed by the increased margins and robustness of Dshot300. Similarly Dshot150 is the
recommended max for 24MHz MCUs.
When the input signal is Dshot, throttle calibration is disabled, and the throttle calibration values are ignored.
Operation manual for BLHeli SiLabs Rev14.x