かなり昔にAVRでドローン用フライトコントローラーのハードウェアを自作しました. その際ファームウェアは既存のMultiWiiを移植して利用しましたが,ファームウェアも自作してみたいと思っていました. そこでSTM32F3マイコンを使って,ドローン用フライトコントローラーのハードウェアとソフトウェアを自作しました. ここで作成するフライトコントローラーは,姿勢推定やモーター制御など機体の制御を主に行い,FPV用のOSD機能やGPS制御などは対応していません. このフライトコントローラーを使って作成したドローンは別のページで紹介しています.
主な仕様は次のとおりです:
マイコンにはFPU内蔵のSTM32F303CCT6を使いました. 8MHz内部クロックをPLLで逓倍して64MHzで駆動します. 加速度センサーとジャイロセンサー(IMU; Inertial Measurement Unit)にはMPU-6000を使います. 電波受信のためにNRF24L01のモジュールを使いますが,サイズを小さくするためにアンテナパターン部分は切り取ってアンテナ用ワイヤーをハンダ付けして使います. その他にフラッシャー用のLED(前後)とバッテリー電圧検出用の分圧回路がついています.
電源レギュレータICは,使用するLiPoバッテリーが1Sの場合は2.85V用を,2Sの場合は3.3V用を使います.
利用目的に応じて使い分けるために,標準サイズ版と小型サイズ版の2つの基板サイズを用意しました. 回路自体は同一ですが,標準サイズ版はESC用の電源端子がついており,余裕のある部品配置でメンテナンス性がよくなっています. 小型サイズ版では,NRF24L01モジュールを立体的に重ねています. 以下に回路図・配線図と完成した基板を示します.
ファームウェアの開発にはArduino IDEのArduino Core STM32を使用しました. このSTM32用Arduino CoreのおかげでSTM32のソフトウェア開発が非常に便利になりましたが,一方でバグも時々存在し,バージョンアップ時に関数の引数が変わったりします. 今回はArduino IDE 1.8.5とSTM32 Cores 1.9.0を使いました. "Generic STM32F3 series"の"RobotDyn BlackPill F303CC"というボードを指定すればそのままビルドして書き込めます.
プログラムはページ末尾にリンクのあるGitHub上に置いてあり,下記のファイルから構成されます. 各モジュールの詳細は以降の各節で説明します.
ファイル | 内容 |
stm32_fc.ino | メイン関数 |
Communication.{h,cpp} | 通信モジュール |
Controller.{h,cpp} | 制御モジュール |
Receiver.{h,cpp} | 受信モジュール |
Sensor.{h,cpp} | センサーモジュール |
System.{h,cpp} | システムモジュール |
Config.h | 設定ファイル |
Peripheral.cpp | マイコン初期化ファイル |
Utility.h | ユーティリティファイル |
このフライトコントローラーでは,座標系の軸の方向を次のように定義しています(右手系). 回転軸(ロール,ピッチ,ヨー)はそれぞれ座標軸の向きに対して右ネジの方向とします.
プログラム中の変数や定数は,基本的に常にSI単位系で値を保存しています.角度はradianです.
フライトコントローラーに関する実践的な情報を得るのに,Cleanflight flight controller: A nose dive into the Frequency Domain (A. Farrukh)が特に参考になりました.
このフライトコントローラーのメイン関数(Arduinoではmain()の代わりにsetup()とloop()があり,前者を一度だけ呼び初期化を行い以後後者を無限に繰り返す)はとても単純なので,下記にコード全体を示します.
void setup() { calib(); com_init(); ctl_init(); rec_init(); sen_init(); sys_init(); timer( TIM2, COM_BAUDRATE / 9, 4, com_process); timer( TIM3, CTL_LOOPFREQ, 2, ctl_process); timer(TIM15, 10000, 3, rec_process); timer(TIM16, SEN_SAMPFREQ, 2, sen_process); timer(TIM17, 8, 3, sys_process); } void loop() { }
calib()は後述しますが,電源投入時にMPUのピンの状態を判断して初期化(キャリブレーション)処理を行います. com, ctl, rec, sen, sysで始まる各関数はそれぞれCommunication, Control, Receiver, Sensor, Systemモジュールで,*_init()がモジュールの初期化を行い,*_process()が一定時間毎に呼び出されて処理を行う関数です. timer(タイマー識別子, 周波数, 優先度, 関数)はUtility.hで定義された関数で,指定された関数をタイマーを使って指定した周期で呼び出します. つまり,このフライトコントローラーはこれらの5つのモジュールがマルチタスクで並列動作する仕組みになっています. IMUからのデータ取得(sen)とPIDコントローラによる姿勢制御(ctl)は優先度がもっとも高く(優先度2),次に受信機(rec)とシステム関係の処理(sys)が優先され(優先度3),通信関係の処理(com)は最も低くなっています(優先度4). 注意しなくてはいけないのは,各モジュールが非同期で動くため,あるモジュールが別のモジュールの状態を直接変更しないようにしています.
なお現在のプログラムでは,受信機とIMUとの通信(どちらもSPI)は割り込みやDMA転送は使用せずにポーリングとCPUによるI/Oアクセスで処理しています. MPUの処理速度に余裕がありコードもシンプルになるのでそのようにしていますが,回路自体は割り込みを使えるように設計してあり,割り込みとDMA転送を使えばこれらの処理にMPUクロックを消費する必要はほとんどなくなります.
このモジュールは,PCとシリアル通信するためのもので飛行には関係ありません. PC上のGUIに表示する情報を出力したり,PC上で更新されたパラメータを入力します. PCとの通信にはMultiwii Serial Protocol (MSP)というものを使います. これはその名のとおり古のMultiWiiのために開発されたプロトコルですが,CleanflightやBetaflightでもそれを拡張したものが生き続けているようです. 今回はPC上でフライトコントローラーの設定を行う際に,新たにPC側のソフトを開発するのは面倒だったため,MSPの必要最小限のセットを実装して既存のMultiWiiConfというソフトを流用できるようにしました. ただしPIDの係数が1バイトでしか保持できないなど不便な点もあります. またフライトコントローラーを経由してESCを直接設定できるESC passthroughモードがあると便利なのですが,今回は対応しませんでした.
このモジュールは,PID制御により機体の姿勢をコントロールします. 送信機のスティックの状態を調べてarmed/disarmed等の状態を更新した後に,機体の制御を行います.
次の2つの操縦モードに対応しています:
2つのモードをより厳密に定義すると,Angleモードでは操縦スティックの傾きと機体の角度が連動するように制御し,Acroモードでは操縦スティックの傾きと機体の回転速度が連動するように制御します. 機体の姿勢を操縦スティックに効率的に追従させるためPID制御を行います. PID制御の詳細はここでは省略しますが,実際のコードはシンプルなので下に示します.
for (int i = 0; i < 3; i++) { float setpoint; if (SELFLEVEL_MODE && i != 2) { // ANGLE mode. setpoint = ((STICK_POSITION[i] - 0.5f) / 0.5f * ANGLE_MAX - sen.angle[i]) * (ROTATION_SPEED_MAX / ANGLE_MAX); } else { // ACRO mode. setpoint = (STICK_POSITION[i] - 0.5f) / 0.5f * ROTATION_SPEED_MAX; } const float error = setpoint - sen.gyro[i]; integ[i] = constrain(integ[i] + error, -integ_limit, integ_limit); deriv[i] = FILTER(error - deriv[i]); const float p_term = pid[i][0] * error; const float i_term = pid[i][1] * integ[i] / CTL_LOOPFREQ; const float d_term = pid[i][2] * deriv[i] * CTL_LOOPFREQ; output[i] = p_term + i_term + d_term; deriv[i] = error; }
一番外側のfor文のループで,3つの回転軸に対する処理をそれぞれ行います. iは0, 1, 2がそれぞれロール,ピッチ,ヨー軸を表します. sen.angle[i]は現在の機体の軸iの角度,sen.gyro[i]は現在の機体の軸iの回転速度,STICK_POSITION[i]は操縦スティックの軸iの傾きを表します(0から1の値で0.5が中立). まず操縦モードに応じてスティックの傾きから目標値(setpoint)を求めて,その目標値と現在値との誤差(error)を計算し,P, I, Dの各項の値を計算します. ANGLEモードの場合はスティックの傾きと現在の機体の姿勢から,目標となる回転速度を決定しています. ヨー軸については操縦モードによらずに,常にスティックの傾きと回転速度が連動するようにします. I項は積分値が大きくなりすぎないようにクリッピングを行い,D項はノイズの影響を減らすためローパスフィルターを適用します. このローパスフィルターには指数移動平均(Exponential Moving Average)を使いましたが,これはPT1フィルターと呼ばれるものとほぼ同じです.
PID制御部で各軸の回転速度をどれだけ変化させるかという出力値が得られた後は,それを4つのモーター(クアッドコプターの場合)に分配するミキシング処理を行うことで各モーターに対する出力値を決定しています. この処理も単純なので,実際のコードを下に示します.
static constexpr float MOTOR_MIX[4][3] = {{-1.0f, -1.0f, -1.0f}, {-1.0f, +1.0f, +1.0f}, {+1.0f, -1.0f, +1.0f}, {+1.0f, +1.0f, -1.0f}}; // Quad X. float motor[4]; for (int i = 0; i < 4; i++) { motor[i] = STICK_POSITION[3] * THROTTLE_GAIN for (int j = 0; j < 3; j++) motor[i] += MOTOR_MIX[i][j] * output[j] * ROTATION_GAIN; } if (rec.value[3] <= STICK_THRESHOLD) { for (int i = 0; i < 4; i++) motor[i] = 0.0f; for (int i = 0; i < 3; i++) integ[i] = deriv[i] = 0.0f; } for (int i = 0; i < 4; i++) motor[i] = constrain(motor[i], 0.0f, 1.0f); if (ARMED) sys_motor(motor);
ここで,STICK_POSITION[3]はスロットルレバーの値(0から1の値)が入っています. スロットルレバーが下げられている時(STICK_THRESHOLDの値より小さい時)は,モーター出力を止めます. MOTOR_MIXという配列には,PID制御の出力(機体の各軸の回転速度)をモーター出力へ変換するパラメータが入っています. ここではX型のクアッドコプター用の値が入っていますが,この値を変えることでH型クアッドコプターやヘキサコプターやオクトコプターなどにも対応できます.
このモジュールは,送信機からの信号の受信処理を行います.送信機とのバインド処理も行います. FHSS方式のHiSKYプロトコルを扱いますが,昔作ったものをベースにしています. タイムアウト処理を工夫して徐々にリトライ時間を延ばすことで,ノイズが多い状況でも信号の取りこぼしが起こりにくくなるようにしました.
一定時間送信機からの信号が受信できない時はオフライン状態であると判断します. その場合は安全のためにコントローラーモジュールですべてのモーターを停止します.
このモジュールは,加速度・ジャイロセンサー(IMU)からの入力を処理します. センサーから入力した生の値からオフセットやノイズを除去し,異なるセンサーの情報を統合して現在の機体の姿勢を推定します. この姿勢の推定結果がControllerモジュールのPID制御で使われます.
姿勢推定を行う場合,加速度センサーとジャイロセンサー(の積分値)のどちらも使うこともできますが,それぞれ異なる特徴があります. 加速度センサーは,短い時間レンジではノイズが多いですが,長い時間レンジでみた値は信頼できます. ジャイロセンサーは,短い時間レンジでは比較的正確ですが,長い時間レンジではドリフトが起こり信頼できません. このように異なる性質を持つセンサーの情報を統合して,より精度の高い推定値を得る処理はセンサーフュージョンと呼ばれます. センサーフュージョンのアルゴリズムについては,このページの解説が詳しいです. 色々な方法がありますが,ここでは少ない計算量で実用的な性能を持つMahonyフィルターを使いました. 姿勢推定を行う場合,姿勢の表現にオイラー角やDCM,クォータニオンを使うことができますが,ここでは計算時にはクォータニオンを使い,最後に扱いやすいオイラー角に変換します. アルゴリズムは下記のようになります.
センサーフュージョンの処理を行う前に,センサーの値からノイズを除去するためにローパスフィルターを適用します. できるだけ遅延を抑えながら不要な帯域をカットする必要がありますが,ここでは2段の双二次フィルターを使いました. なおBetaflightなど既存のフライトコントローラーでは,他にもノッチフィルター(周波数固定のstatic notchや,FFTでモーターノイズに自動追従するdynamic notch)や,RPM filterというESC telemetryにより得たモーターの回転数に基づきモーターノイズ除去を行うフィルターなどが用意されています.
このモジュールは,システムモジュールという名前がついていますが,他のモジュールに入らない雑多な処理を担当します. 具体的には,現在の機体の状態に応じてLEDフラッシャーを点滅させたり,EEPROMからの設定情報の読み込みや書き込み,またESCのPWM信号の制御などを行います. バッテリーの電圧も計測し,その結果を使ってControllerモジュールではバッテリー警告の必要性を判定します. モーターに大きな電流が流れて電圧変動が大きいので,電圧の測定結果はローパスフィルターを通し,警告の判断はシュミットトリガーを使います.
クアッドコプターのモーターの順番は,右後,右前,左後,左前の順番に1, 2, 3, 4として,右後が上から見て時計回りの回転になるようにします.
操作 | 内容 |
左スティックを左上 | センサーキャリブレーション開始(disarmed時) |
左スティックを右下 | armed状態に移行 |
左スティックを左下 | disarmed状態に移行 |
CH5スイッチの状態 | Angle/Acroモードの切替 |
パターン(1秒周期) | 意味 |
10000000 | 電波受信不能 |
11011010 | センサーキャリブレーション中 |
11001000 | disarmed状態(Acroモード) |
11001100 | disarmed状態(Angleモード) |
10101010 | バッテリー無し |
11111100 | バッテリー警告 |
11111111 | armed状態 |
このフライトコントローラーは,起動時にTXD端子がプルダウンされていると以下の初期化シーケンスを実行します. 途中でLEDの点滅中に電池をはずせば,それ以降の初期化シーケンスを中断できます.
フライトコントローラーの設定を行うために,MultiWii用に開発されたGUIであるMultiWiiConfを流用しますが,いくつか表示を読み替えて使う必要があります.
表示 | 内容 |
Cycle Time | Controllerモジュールのサイクルタイム(実際のPIDループタイム) |
I2C error | Sensorモジュールのサイクルタイム(実際のIMUサンプリング周期) |
DEBUG0 | Controllerモジュールの処理時間 |
DEBUG1 | Sensorモジュールの処理時間 |
DEBUG2 | Receiverモジュールの1秒あたりの受信数 |
DEBUG3 | Sensorモジュールのエラー数 |
表示 | デフォルト値 | 倍数 | パラメータ |
min_command | 0.05 | 1000 | stk_thres |
vbatscale | -- | 70 | vbat_scale |
0/1/2:ROLL/PITCH/YAW-P | 0.2 | 100 | pid (p) |
0/1/2:ROLL/PITCH/YAW-I | 0.02 | 1000 | pid (i) |
0/1/2:ROLL/PITCH/YAW-D | 0.002 (YAWは0) | 10000 | pid (d) |
5:PosR-P | 75 | 1 | filter_acc |
6:NavR-P | 75 | 1 | filter_gyro |
7:LEVEL-P | 100 | 1 | filter_dterm |
5:PosR-I | 50 (degree) | 0.1 | angle_max |
6:NavR-I | 180 (degree) | 0.1 | rotation_max |
5:PosR-D | 1.0 | 10 | rotation_gain |
6:NavR-D | 1.0 | 10 | throttle_gain |
この自作フライトコントローラーを使って作成したドローンの詳細は別のページにまとめました. PIDパラメータの微調整を終えるまで何度も墜落させることを覚悟していましたが,はじめにモーターを接続せずにPCに接続して,スティック操作に対して自然なモーター出力になるように調整してから飛ばしたところ,そのままでもかなり安定して飛びました. Pパラメータの許容範囲は比較的広いようですが,IとDのパラメータは増やすと不安定になりやすいので慎重に変更した方がよいです.
今回フライトコントローラーのソフトウェアを自作してみて,ノイズの影響がかなり大きいことを実感しました. センサーの近くにモーターという振動発生源があるので当然かもしれませんが,モーターを止めた場合と動かした場合ではセンサー出力が大きく変わります. またプロペラのバランスや機体の剛性などの問題により異常振動が発生すると動作が不安定になります.
昔AVRでフライトコントローラーのハードウェアを作成した直後にこのSTM32版のハードウェアも完成させていましたが,その後他のことをやっていて長いこと放置していました. 機体を制御するフライトコントローラーのソフトウェア自体にずっと興味はあったので,今回重い腰を上げてようやく動かしました. 数年間ドローンに関する情報から離れていましたが,その間のホビー用ドローンの変化は小さかったようにも大きかったようにも見えます. 部品の性能やコストの進歩は以前と比べて遅くなっていると思います. 数年前にディスコン間近と言われていたMPU-6000をいまだに目にして,ブラシレスモーターやバッテリーも値段はあまり変わらず,むしろ入手性が悪化した部品もあります. 一方で,以前は1S用のブラシレスモーターやESCはマイナーでしたが,TinyWoopなどが流行して小型ドローンも多く目にするようになりました. 数年前はESCプロトコルもどんどん新しい規格が登場して,ループ周波数の高速化も宣伝されていましたが,モーターの機械的な応答速度,送受信機内部の遅延,人間の反応速度などのボトルネックが存在するためかそのような方向の発展は一段落したように感じます. 別の変化として,ドローンの規制対象が200g以上から100g以上に引き下げられるというニュースも出てきました.
フライトコントローラーのハードウェアを自作するのはこれが2回目でしたが,また作る機会があればいくつか改善したいと感じた点がありました: