自作ドローンのソフトウェア設計 + ライセンスに関するまとめ

本記事では、フライトコントローラ側と送信機側のソースコードのソフトウェア設計について書いていきます。2021/7時点でこの記事を執筆していて、今後リファクタリング等で最新版のアーキテクチャと異なってしまうかもしれませんが、基本的にはこうなっているんだなと参考にしていただければ幸いです。

ソースコード自体は github に載せているのでコチラをご覧ください。フライトコントローラ側ソースコードは main、送信機側ソースコードは transmitter を参照ください。

前提

フライトコントローラ側、送信機側ともに ESP32 のマイコンを使用しており、ソースコードのベースは Arduino（基本的には c++ のコード）となっています。

フライトコントローラ側ソースコード

コンポーネントと役割分担

シーケンス図（処理の流れ）

vscode の draw.io のプラグインを使って書いています。github で見たい場合はコチラの main.drawio.png（github の方が大きくてみやすい）。

ちなみに UML 図の書き方は以下の本で勉強しました。

各クラスの関数・設定について

設定 (def_systems.h)

具体的には以下のような設定をこのファイルでしています。

• モータの PWM 値を指定する出力ポート：12 〜 15
• モータの回転方向を指定する出力ポート：18, 19, 32, 33
• 緊急停止スイッチの入力ポート：4
• システムの制御周期：10 ms
• 送信機から受信するデータのサイズ：7 byte
• モータ出力値の最大値
• デバッグ用設定

モータとポート番号の対応は以下のとおりです。

メイン処理 (main.ino)

Emergency:: 緊急停止処理 (emergency.cpp)

imu_bmx055:: IMU 読み取り処理 (imu_bmx055.cpp)

秋月電子で購入した BMX055 のモジュールを使っており、商品購入サイトの「Arduinoサンプルプログラム」を参考にソースコードを改変しています。

Motor:: モータ出力計算処理 (motor.cpp)

各モータへのコマンドの割り振りは以下のようにしています。

    m_recv_cmd[0] = + cmd_roll - cmd_pitch - cmd_yaw + cmd_thrust;
    m_recv_cmd[1] = + cmd_roll + cmd_pitch + cmd_yaw + cmd_thrust;
    m_recv_cmd[2] = - cmd_roll + cmd_pitch - cmd_yaw + cmd_thrust;
    m_recv_cmd[3] = - cmd_roll - cmd_pitch + cmd_yaw + cmd_thrust;

PID 制御指令値とジョイスティック指令値を 0.45 : 0.55 の比率でかけてから足し合わせて、最終的な指令値を計算しています。

    float pid_ratio = 0.45;
    int motor_data[4] = {0, 0, 0, 0};i

    for (int i=0; i<4; i++) {
        motor_data[i] = m_pid_cmd[i]*pid_ratio + m_recv_cmd[i]*(1.0f-pid_ratio);
        ledcWrite(i, motor_data[i]);
    }

PID:: PID 計算処理 (pid.cpp)

ロール・ピッチ・ヨー角の PID 制御指令値計算はこんな感じです。pid_rpy[0], pid_rpy[1], pid_rpy[2] はそれぞれロール・ピッチ・ヨー角の PID 制御指令値を表しています。

    for (int i=0; i<3; i++) {
        pid_rpy[i] = Kp[i]*rpy_p[i] + Ki[i]*rpy_i[i] + Kd[i]*rpy_d[i];
    }

Arm:: アーム / ディスアーム処理 (arm.cpp)

Receiver:: コマンド受信処理 (recv.cpp)

チェックサムの計算は以下のようにやっています。

  uint8_t checksum = 0;
  checksum |= 0b11000000 & recv_data[1];
  checksum |= 0b00110000 & recv_data[2];
  checksum |= 0b00001100 & recv_data[3];
  checksum |= 0b00000011 & recv_data[4];

ジョイスティック左ボタンの緊急停止には、実験中ドローンが制御不能になったとき何度もすくわれました…。

改善点

全体を通して、以下のような改善をした方がいいなと思いました。

• コーディング規約の統一
  • 変数名の名付け方：メンバ変数は is_armed_ のように最後にアンダーバーをつける
• エラー処理がほとんどない。→エラー判定 + 処理追加。
• 関数への配列の渡し方を以下のように変える。（参考）
  • get_command(int data[4]) -> get_command(int data[])
• 制御入力値の値を、より小さい値でサチュレーションする。（フェイルセーフ）

送信機側ソースコード

ESP32 同士のデータ送受信のソースコードに関してはこの記事でまとめているので、よければご覧ください。

コンポーネントと役割分担

シーケンス図

github で見たい場合はコチラの transmitter.drawio.png（github の方が大きくてみやすい）。

各クラスの関数・設定について

設定 (def_systems.h)

• 左ジョイスティック
  • スイッチ：32
  • X 方向：12
  • Y 方向：13
• 右ジョイスティック
  • スイッチ：33
  • X 方向：14
  • Y 方向：15
• 送信周期：20 ms
• 送信データサイズ：7 byte

※ジョイスティックを右に動かした時 +x 方向の値が増加し、上に動かした時 +y 方向の値が増加するように配線を組みました。（参考：送信機の PCB 基板設計）

メイン処理 (transmitter.ino)

Transmit:: 送信機の処理 (Transmit.cpp)

pack_switch_data() では以下のような処理をしています。

uint8_t Transmit::pack_switch_data() {
    uint8_t left_sw_data  = 0x01 & input.get_left_sw_val();
    uint8_t right_sw_data = 0x02 & (input.get_right_sw_val() << 1);
    return left_sw_data | right_sw_data;
}

InputCmd:: 入力コマンド処理 (InputCmd.cpp)

sense_value() では、以下のようにジョイスティックの値を読み取っています。ジョイスティックの方向キーの読み取りデータは、右シフト演算をして $2^4$ で割るようにしています。

ジョイスティックの読み取り値は最大 4095 となっているので、最大値を255 にして 1 byte におさまるようにしています。

void InputCmd::sense_value() {
    left_sw_val = digitalRead(LEFT_SW_PIN);
    left_x_val  = analogRead (LEFT_X_PIN) >> 4;
    left_y_val  = analogRead (LEFT_Y_PIN) >> 4;

    right_sw_val = digitalRead(RIGHT_SW_PIN);
    right_x_val  = analogRead (RIGHT_X_PIN) >> 4;
    right_y_val  = analogRead (RIGHT_Y_PIN) >> 4

ライセンスに関して

結論から言うと、本記事のソフトウェアに関しては、自由に改変・再配布してもらって構いません。一応、使っているライブラリ的に問題ないかを少し調べてみました。

本記事のソフトウェアで使っている外部のライブラリ / ソフトウェアは以下です。

• Arduino.h
• MadgwickAHRS
• BMX055 9軸センサ のソースコード（秋月電子のサンプルプログラム）

上の2つは、LGPL 2.1 のライセンスが付与されていました。Arduino.h も MadgwickAHRS も改変はしていないので、LGPL 2.1 のライセンスに従う必要はなさそうです。非コピーレフトで自由度の高い MIT ライセンスを付与することにしました。

IMU のコードに関してはサンプルプログラムとして配布されているものを改変しており、ライセンス等に関する記載がなかったので、秋月電子さんに問い合わせしてみました。自己責任の範囲であれば改変したものを公開しても構わないとのことでした。

以下、参考にしたサイトです。

• エンジニアが最低限理解しておくべきOSSライセンスの基礎知識
• ライセンスをつけないとどうなるの？
• How does using a LGPL gem affect my MIT licensed application?
• 自由度の高い MIT ライセンスの概要と他ライセンスとの比較

上記サイトを参考にすると、ライセンスをつけないでソフトウェアを公開するのはコミュニティの活発なソフトウェア開発の妨げになってしまうため、可能な限りソフトウェアにはライセンスは付与して公開した方がよさそうですね。

まとめ

どんなコードを書いたっけなと思い出しながら、設計書を書く感覚でまとめてみました。コンポーネントごとの役割分担はちゃんとしていて読みやすいコードだったりするかなと思っています。

できれば doxygen でまとめてみたいなという気持ちもありますが、これは別の機会にやろうかなと思います。

もし何か質問や改善点等ありましたらお気軽にコメントいただければうれしいです。