STM32を使用したドローン用フライトコントローラーの自作

はじめに

かなり昔にAVRでドローン用フライトコントローラーのハードウェアを自作しました．その際ファームウェアは既存のMultiWiiを移植して利用しましたが，ファームウェアも自作してみたいと思っていました．そこでSTM32F3マイコンを使って，ドローン用フライトコントローラーのハードウェアとソフトウェアを自作しました．ここで作成するフライトコントローラーは，姿勢推定やモーター制御など機体の制御を主に行い，FPV用のOSD機能やGPS制御などは対応していません．このフライトコントローラーを使って作成したドローンは別のページで紹介しています．

主な仕様は次のとおりです:

MPU: STM32F303CCT6 (64MHz, 256KB Flash, 48KB RAM)
IMU: MPU-6000
ループ周波数: 8kHz以下
ESCプロトコル: PWM, Oneshot125, Oneshot42
内蔵受信機: HiSKYプロトコル(nRF24L01)
基板サイズ(ネジ穴サイズ): 36x36mm (30.5x30.5mm)または20x20mm (16x16mm)
バッテリー: 1Sまたは2S
動作電圧: 2.85V (1S)または3.3V (2S)

ハードウェア

マイコンには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	ユーティリティファイル

このフライトコントローラーでは，座標系の軸の方向を次のように定義しています(右手系)．回転軸(ロール，ピッチ，ヨー)はそれぞれ座標軸の向きに対して右ネジの方向とします．

X: 前方向
Y: 右方向
Z: 下方向

プログラム中の変数や定数は，基本的に常に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クロックを消費する必要はほとんどなくなります．

Communication

このモジュールは，PCとシリアル通信するためのもので飛行には関係ありません． PC上のGUIに表示する情報を出力したり，PC上で更新されたパラメータを入力します． PCとの通信にはMultiwii Serial Protocol (MSP)というものを使います．これはその名のとおり古のMultiWiiのために開発されたプロトコルですが，CleanflightやBetaflightでもそれを拡張したものが生き続けているようです．今回はPC上でフライトコントローラーの設定を行う際に，新たにPC側のソフトを開発するのは面倒だったため，MSPの必要最小限のセットを実装して既存のMultiWiiConfというソフトを流用できるようにしました．ただしPIDの係数が1バイトでしか保持できないなど不便な点もあります．またフライトコントローラーを経由してESCを直接設定できるESC passthroughモードがあると便利なのですが，今回は対応しませんでした．

Controller

このモジュールは，PID制御により機体の姿勢をコントロールします．送信機のスティックの状態を調べてarmed/disarmed等の状態を更新した後に，機体の制御を行います．

次の2つの操縦モードに対応しています:

Angleモード
初心者向きの操縦モードです．操縦スティックを傾けるとその方向にドローンが移動し，スティックを戻すと空中に静止します．安定した操作ができます．Self-levelモードとも呼ばれます．
Acroモード
上級者向きの操縦モードです．操縦スティックを傾けるとその方向にドローンが回転し，スティックを戻すと現在の姿勢を保ちます．アクロバティックな操作ができます．RateモードやManualモードとも呼ばれます．

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型クアッドコプターやヘキサコプターやオクトコプターなどにも対応できます．

Receiver

このモジュールは，送信機からの信号の受信処理を行います．送信機とのバインド処理も行います． FHSS方式のHiSKYプロトコルを扱いますが，昔作ったものをベースにしています．タイムアウト処理を工夫して徐々にリトライ時間を延ばすことで，ノイズが多い状況でも信号の取りこぼしが起こりにくくなるようにしました．

一定時間送信機からの信号が受信できない時はオフライン状態であると判断します．その場合は安全のためにコントローラーモジュールですべてのモーターを停止します．

Sensor

このモジュールは，加速度・ジャイロセンサー(IMU)からの入力を処理します．センサーから入力した生の値からオフセットやノイズを除去し，異なるセンサーの情報を統合して現在の機体の姿勢を推定します．この姿勢の推定結果がControllerモジュールのPID制御で使われます．

姿勢推定を行う場合，加速度センサーとジャイロセンサー(の積分値)のどちらも使うこともできますが，それぞれ異なる特徴があります．加速度センサーは，短い時間レンジではノイズが多いですが，長い時間レンジでみた値は信頼できます．ジャイロセンサーは，短い時間レンジでは比較的正確ですが，長い時間レンジではドリフトが起こり信頼できません．このように異なる性質を持つセンサーの情報を統合して，より精度の高い推定値を得る処理はセンサーフュージョンと呼ばれます．センサーフュージョンのアルゴリズムについては，このページの解説が詳しいです．色々な方法がありますが，ここでは少ない計算量で実用的な性能を持つMahonyフィルターを使いました．姿勢推定を行う場合，姿勢の表現にオイラー角やDCM，クォータニオンを使うことができますが，ここでは計算時にはクォータニオンを使い，最後に扱いやすいオイラー角に変換します．アルゴリズムは下記のようになります．

ジャイロセンサーと加速度センサーから，現在の回転速度ω=(ω_x, ω_y, ω_z)と加速度a=(a_x, a_y, a_z)を入力します．大きさを1に正規化して向きを逆にした加速度a'を計算します(センサーで測定した重力の単位ベクトル)．
a' = －a / ‖a‖
クォータニオンで表現された現在の姿勢推定値r=(r₀, r₁, r₂, r₃)から，重力のベクトルg=(g_x, g_y, g_z)を推定します(rによるZ軸単位ベクトルの回転)．
g = Imag(r^* ⊗ (0, 0, 1) ⊗ r) = (2 (q₁q₃+q₀q₂), 2 (q₂q₃−q₀q₁), q₀²−q₁²−q₂²+q₃²)
センサーで測定された重力ベクトルa'と推定された重力ベクトルgとの誤差eを外積として計算します．
e = a' × g
計算された誤差を使いP制御により回転速度を修正します(PI制御が使われる場合もあります)．
ω' = ω + K_p e
修正された回転速度を使って現在の姿勢を更新します．クォータニオンの時間微分に経過時間を掛けます．
δr = 1/2 r ⊗ ω' = 1/2 (−r₁ω'_x−r₂ω'_y−r₃ω'_z, r₀ω'_x−r₃ω'_y+r₂ω'_z, r₃ω'_x+r₀ω'_y−r₁ω'_z, −r₂ω'_x+r₁ω'_y+r₀ω'_z)
r' = (r + δr Δt) / ‖r + δr Δt‖
クォータニオンで表現されている現在の姿勢r'をオイラー角sに変換します．
s = (arctan(2 (r'₂r'₃+r'₀r'₁) / (r'₀²−r'₁²−r'₂²+r'₃²)), arcsin(2 (r'₀r'₂−r'₁r'₃)), arctan(2 (r'₁r'₂+r'₀r'₃) / (r'₀²+r'₁²−r'₂²−r'₃²)))

センサーフュージョンの処理を行う前に，センサーの値からノイズを除去するためにローパスフィルターを適用します．できるだけ遅延を抑えながら不要な帯域をカットする必要がありますが，ここでは2段の双二次フィルターを使いました．なおBetaflightなど既存のフライトコントローラーでは，他にもノッチフィルター(周波数固定のstatic notchや，FFTでモーターノイズに自動追従するdynamic notch)や，RPM filterというESC telemetryにより得たモーターの回転数に基づきモーターノイズ除去を行うフィルターなどが用意されています．

System

このモジュールは，システムモジュールという名前がついていますが，他のモジュールに入らない雑多な処理を担当します．具体的には，現在の機体の状態に応じてLEDフラッシャーを点滅させたり，EEPROMからの設定情報の読み込みや書き込み，またESCのPWM信号の制御などを行います．バッテリーの電圧も計測し，その結果を使ってControllerモジュールではバッテリー警告の必要性を判定します．モーターに大きな電流が流れて電圧変動が大きいので，電圧の測定結果はローパスフィルターを通し，警告の判断はシュミットトリガーを使います．

使用方法

モーターの配置

クアッドコプターのモーターの順番は，右後，右前，左後，左前の順番に1, 2, 3, 4として，右後が上から見て時計回りの回転になるようにします．

プロポの操作(Mode 2の場合)

操作	内容
左スティックを左上	センサーキャリブレーション開始(disarmed時)
左スティックを右下	armed状態に移行
左スティックを左下	disarmed状態に移行
CH5スイッチの状態	Angle/Acroモードの切替

LEDの表示パターン(Config.hで変更可能)

パターン(1秒周期)	意味
10000000	電波受信不能
11011010	センサーキャリブレーション中
11001000	disarmed状態(Acroモード)
11001100	disarmed状態(Angleモード)
10101010	バッテリー無し
11111100	バッテリー警告
11111111	armed状態

フライトコントローラーの初期化

このフライトコントローラーは，起動時にTXD端子がプルダウンされていると以下の初期化シーケンスを実行します．途中でLEDの点滅中に電池をはずせば，それ以降の初期化シーケンスを中断できます．

前後のLEDを2Hzで交互に5秒間点滅．
送信機のバインドをリセット．
前後のLEDを1Hzで交互に5秒間点滅．
フライトコントローラーのパラメータをリセット．
前後のLEDを0.5Hzで交互に5秒間点滅．
ESCキャリブレーションを実行．
前後のLEDを点灯．

インストール方法

Config.hのBAT_CELL(1または2)とBAT_VREF(2.85または3.3)を使用するLiPoバッテリーのセル数により書き換える．フライトコントローラー基板が傾いて配置されている場合はIMU_ROTも書き換える．
BOOTとVCC端子を短絡させて，TXD/RXD端子を使いファームウェアを書き込む．
TXDとGNDを短絡させて電源を入れて初期化を行い，ESCキャリブレーションが終わるのを待つ．
送信機をbindさせる．

MultiWiiConfでの設定

フライトコントローラーの設定を行うために，MultiWii用に開発されたGUIであるMultiWiiConfを流用しますが，いくつか表示を読み替えて使う必要があります．

ACCのROLL/PITCH/ZはX/Y/Zを表す．またACCの値はMultiWiiと極性が逆になる．
ACCは512倍した値を，GYROはradianをdegreeに変換して4.1を掛けた値を表示する．
モーター出力は[0, 1]を[1000, 2000]に変換した値を表示する．
受信機出力は[0, 1]を[1000, 2000]に変換した値を表示する．
姿勢については，radianをdegreeにして，pitchはマイナスを掛けている．
情報の表示

表示	内容
Cycle Time	Controllerモジュールのサイクルタイム(実際のPIDループタイム)
I2C error	Sensorモジュールのサイクルタイム(実際のIMUサンプリング周期)
DEBUG0	Controllerモジュールの処理時間
DEBUG1	Sensorモジュールの処理時間
DEBUG2	Receiverモジュールの1秒あたりの受信数
DEBUG3	Sensorモジュールのエラー数

変更可能なパラメータ(PC送信時には0〜255の値にするために倍数を掛けている)

表示	デフォルト値	倍数	パラメータ
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回目でしたが，また作る機会があればいくつか改善したいと感じた点がありました:

36x36mmの基板サイズはやや大きいが，20x20mmで2層構造にすると不便なので，30x30mm程度がよかったかもしれない．
20x20mmの基板サイズの場合，プロペラに干渉しにくいように45度傾けた配置にすればよかった(今回ソフトウェアでIMUの向きを補正しました)．
RXDやTXDなど基板上の端子のシルク印刷をすべきだった．
モーターに大電流が流れるので電源のコンデンサを強化すべきだった．
ESCの電源の穴が小さすぎた(AWG20のケーブルだと入りません)．
IMU出力にはLPFを適用するために結局遅延が入って低周波成分しか使わないので，安いMPU-6050でも十分だったかもしれない．
STM32F3にはSPIが2系統あるが，今は遅い方のSPIをIMUに割り当てているので使い方を逆にした方が柔軟性がある．

ダウンロード

ファームウェア

[Back]

2021-02 製作
2021-04-11 ページ作成
2021-11-14 ページ更新
T. Nakagawa