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


はじめに

かなり昔にAVRでドローン用フライトコントローラーのハードウェアを自作しました. その際ファームウェアは,既存のMultiWiiを移植して利用しましたが,ファームウェアも一から作りたいと思っていました. そこでSTM32F3マイコンを使って,ドローン用フライトコントローラーのハードウェアとソフトウェアを自作しました.

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

ハードウェア

マイコンにはFPU内蔵のSTM32F303CCT6を使いました. 8MHz内部クロックをPLLで逓倍して64MHzで駆動します. 加速度センサーとジャイロセンサー(IMU; Inertial Measurement Unit)にはMPU-6000を使います. 受信機のためにNRF24L01を使いますが,サイズを減らすためにアンテナパターン部分は切り取ってアンテナ用ワイヤーをハンダ付けして使います. その他に,フラッシャー用のLED(前後)とバッテリー電圧検出用の分圧回路がついています.

電源レギュレータICは,使用するLiPoバッテリーが1Sの場合は2.85V用を,2Sの場合は3.3V用を使います.

利用目的に応じて使い分けるために,標準サイズ版と小型サイズ版の2つの基板サイズを用意しました. 回路自体は同一ですが,標準サイズ版はESC用の電源端子がついており,無理のない配置でメンテナンス性がよくなっています. 以下に回路図・配線図と完成した基板を示します.

ソフトウェア

概要

ファームウェアの開発にはArduino IDEのArduino Core STM32を使用しました. このSTM32用Arduino CoreのおかげでSTM32のソフトウェア開発が非常に便利になりましたが,一方でバグもまだ存在しバージョンアップの度にAPIが変わったりします. 今回は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 ユーティリティファイル

各モジュールは以降の各節で詳細を説明します.

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

  1. X: 前方向
  2. Y: 右方向
  3. Z: 下方向

フライトコントローラーに関する実際的な情報を得るのに,Cleanflight flight controller: A nose dive into the Frequency Domain (A. Farrukh)が特に参考になりました.

メイン関数

このフライトコントローラーのメイン関数(Arduinoではmain()の代わりにsetup()とloop()があり,前者を一度だけ呼び初期化を行い以後後者を無限に繰り返す)はとても単純なので,下記にコード全体を示します.

void setup() {
  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() {
}

com_, ctl_, rec_, sen_, sys_で始まる各関数はそれぞれCommunication, Control, Receiver, Sensor, Systemモジュールで,*init()がモジュールの初期化を行い,*process()が一定時間毎に呼び出されて処理する関数です. timer(タイマー識別子, 周波数, 優先度, 関数)はUtility.hで定義された関数で,指定された関数をタイマーを使ってその周波数毎に呼び出します. つまり,このフライトコントローラーはこれらの5つのモジュールがマルチタスクで並列動作する構造となっています. IMUからのデータ取得とPIDコントローラによる姿勢制御は優先度がもっとも高く,次に受信機とシステム関係の処理が優先され,通信関係の処理は最も低くなっています. 注意しなくてはいけないのは,各モジュールが非同期で動くため,あるモジュールが別のモジュールの状態を変える必要がある場合は,相手側モジュールの状態を直接変更しないようにしています.

なおハードウェア自体はサポートしていますが,現在のプログラムでは受信機とセンサーとの通信(どちらもSPI)は割り込みやDMA転送は使用せずにポーリングで処理しています. MPUの処理速度に余裕がありコードもシンプルになるのでそうしていますが,割り込みとDMA転送を使えばこれらの処理にMPUクロックを消費する必要はほとんどなくなります.

Communication

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

Controller

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

一般的なフライトコントローラー同様に,次の2つの操縦モードに対応しています:

  1. Angleモード
    初心者向きの操縦モードです.操縦スティックを傾けるとその方向にドローンが移動し,スティックを戻すと空中に静止します.安定した操作ができます.Self-levelモードとも呼ばれます.
  2. 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;
  }

ここで,sen.angle[i]は現在の機体の軸iの角度,sen.gyro[i]は現在の機体の軸iの回転速度,STICK_POSITION[i]は操縦スティックの軸iの傾きを表します(0から1の値で0.5が中立). iは0, 1, 2がそれぞれロール,ピッチ,ヨー軸を表します. まず操縦モードに応じてスティックの傾きから目標値(setpoint)を求めて,その目標値と現在値との誤差(error)を計算し,P, I, Dの各項の値を計算します. ANGLEモードの場合はスティックの傾きと現在の機体の姿勢から,目標となる回転速度を決定しています. ヨー軸については操縦モードによらずに,常にスティックの傾きと回転速度が連動するようにします. I項は積分値が大きくなりすぎないようにクリッピングを行い,D項はノイズの影響を減らすためローパスフィルターを適用します. ローパスフィルターには,後述する指数移動平均を使っています.

PID制御部で各軸の回転速度をどれだけ変化させるかという出力値が得られた後は,それを4つのモーター(クアッドコプターの場合)に分配するミキシング処理を行うことで各モーターに対する出力値を決定しています.

Receiver

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

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

Sensor

このモジュールは,加速度・ジャイロセンサー(IMU)からの入力を処理します. センサーから入力した生の値からオフセットやノイズを除去し,異なるセンサーの情報を統合して現在の機体の姿勢を推定します.

姿勢推定を行う場合,加速度センサーとジャイロセンサー(の積分値)のどちらも使うこともできますが,それぞれ異なる特徴があります. 加速度センサーは,短い時間レンジではノイズが多いですが,長い時間レンジでみた値は信頼できます. ジャイロセンサーは,短い時間レンジでは比較的正確ですが,長い時間レンジではドリフトが起こり信頼できません. このように異なる性質を持つセンサーの情報を統合して,より精度の高い推定値を得る処理はセンサーフュージョンと呼ばれます. 色々な方法がありますが,ここでは少ない計算量で実用的な性能を持つ相補フィルター(complementary filter)を利用します. 姿勢推定を行う場合,オイラー角で計算する方法とクォータニオンで計算する方法がありますが,前者の場合は90度付近で正接が不安定になるなど今回の用途では扱いにくいので,クォータニオンを使うことにします. センサー値を使って姿勢の推定を行った後で,機体の角度をクォータニオンから変換して求めています.

なおセンサーフュージョンの処理を行う前に,センサーの値からノイズを除去するためにローパスフィルターを適用しています. 今回使ったのは指数移動平均(Exponential Moving Average)フィルターで,PT1フィルターと呼ばれるものとほぼ同じです. Betaflightなど既存のフライトコントローラーでは,他にもbiquadのように急峻なフィルターやノッチフィルタ(周波数固定のstatic notchや,FFTでモーターノイズに自動追従するdynamic notch),またRPM filterというESC telemetryにより得たモーターの回転数に基づきモーターノイズ除去を行うフィルターなど多様なフィルターが使われています.

System

このモジュールは,システムモジュールという名前がついていますが,他のモジュールに入らない雑多な処理を担当します. 具体的には,現在の機体の状態に応じてLEDフラッシャーを点滅させたり,EEPROMからの設定情報の読み込みや書き込み,またESCのPWM信号の設定などを行います.

使用方法

モーターの配置

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

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

操作 内容
左スティックを左上 センサーキャリブレーション開始(disarmed時)
左スティックを右下 armed状態に移行
左スティックを左下 disarmed状態に移行

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

パターン(1秒周期) 意味
10001000 操縦信号受信不可能
10101010 センサーキャリブレーション中
11110000 disarmed状態
10000000 バッテリー無し
11111100 バッテリー警告
11111111 armed状態

新しい機体にファームウェアを書き込む方法

  1. Config.hのESC_CALIBRATEをtrueに書き換えてファームウェアを実行しESCキャリブレーションを行う.その後ESC_CALIBRATEはfalseにする.
  2. Config.hのBAT_CELL(1または2)とBAT_VREF(2.85または3.3)を使用するLiPoバッテリーのセル数により書き換える.
  3. MultiWiiConfに接続してパラメータのリセットを行う.
  4. 送信機をbindさせる

実際に使ってみて

作成したフライトコントローラーを120mmサイズのクアッドコプターに搭載して使ってみました. PIDパラメータの微調整を終えるまで何度も墜落させることを覚悟していましたが,はじめにモーターを接続せずにPCに接続して,スティック操作に対して自然なモーター出力になるように調整してから飛ばしたところ,そのままでかなり安定して飛びました. Pパラメータの許容範囲は比較的広いようですが,IとDのパラメータは増やすと不安定になりやすいので,機体を手で持ってテストしながらPCでモニターした場合には影響が少ないようでも慎重に変更した方がよさそうです.

フライトコントローラーを取り付けたクアッドコプター

終わりに

今回フライトコントローラーのソフトウェアを自作してみて,ノイズの影響が予想以上に大きいことを知りました. センサーの間近にモーターという振動発生源があるので当然かもしれませんが,モーターを止めた場合と動かした場合ではセンサー出力が大きく違います.

昔AVRでフライトコントローラーのハードウェアを作成した直後にこのSTM32版のハードウェアも完成させていましたが,その後他のことをやっていて長いこと放置していました. フライトコントローラーのソフトウェア自体にずっと興味はあったので,今回重い腰を上げてようやく動かしました. 数年間ドローンの情報から離れていましたが,その間のホビー用ドローン界の変化は小さかったようにも大きかったようにも感じます. 部品の性能やコストの進歩は以前と比べて非常に遅くなっているように思います. 数年前にディスコン間近と言われていたMPU-6000をいまだに目にして,ブラシレスモーターやバッテリーもあまり安くなってはおらず,むしろ入手性が悪化した部品もあります. 一方で,以前は1S用のブラシレスモーターやESCはマイナーでしたが,TinyWoopなどの流行のおかげか小型ドローンを多く目にするようになりました. AVR版の後にすぐにSTM32F3版を作ったのは,AVRだと浮動小数点演算が遅く利用可能なPWMポートの制約も多いためですが,フライトコントローラーの基本的な処理に必要な性能を考えるとSTM32F3でもややオーバースペックに感じていました. 数年前はESCプロトコルにもどんどん新しい規格が登場して,ループ周波数の高速化も宣伝されるようになっていました. モーターの機械的な応答速度,送受信機内部の遅延,人間の反応速度などのボトルネックが存在するのでどこまで意味があるのかやや疑問でしたが,しばらく前にメジャーなフライトコントローラーで32kHzループ周波数の対応が終了するなど,そのような方向は必要なところまで発展し終えたようにも感じます.

フライトコントローラーのハードウェアを自作するのはこれが2回目でしたが,また作る機会があればいくつか改善したいと感じた点がありました:

ダウンロード


[Back]
2021-02 製作
2021-04-11 ページ作成
T. Nakagawa