本記事では、フライトコントローラ側と送信機側のソースコードのソフトウェア設計について書いていきます。2021/7時点でこの記事を執筆していて、今後リファクタリング等で最新版のアーキテクチャと異なってしまうかもしれませんが、基本的にはこうなっているんだなと参考にしていただければ幸いです。
ソースコード自体は github に載せているのでコチラをご覧ください。フライトコントローラ側ソースコードは main、送信機側ソースコードは transmitter を参照ください。
前提
フライトコントローラ側、送信機側ともに ESP32 のマイコンを使用しており、ソースコードのベースは Arduino（基本的には c++ のコード）となっています。
フライトコントローラ側ソースコード
コンポーネントと役割分担
| コンポーネント | 役割 |
| def_systems.h | モータのポート番号・制御周期・デバッグ用ログの出力 ON / OFF等を指定 |
| main.ino | メイン処理のソースコード |
| emergency.cpp | 緊急停止処理 |
| imu_bmx055.cpp | IMU（秋月電子で購入）の計測結果を読み取る |
| motor.cpp | モータ出力の計算 |
| pid.cpp | PID制御計算 |
| arm.cpp | アーム / ディスアーム処理（ディスアーム状態ではモータが回らない。） |
| recv.cpp | 送信機からのコマンドを受信して処理する |
シーケンス図（処理の流れ）
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)
| 関数名 | 概要 |
| setup() | 初期設定 |
| loop() | main ループ |
Emergency:: 緊急停止処理 (emergency.cpp)
| 関数名 | 概要 |
| setup() | ポート設定 |
| emergency_stop(Arm &arm, Motor &motor) | 緊急停止処理 |
imu_bmx055:: IMU 読み取り処理 (imu_bmx055.cpp)
秋月電子で購入した BMX055 のモジュールを使っており、商品購入サイトの「Arduinoサンプルプログラム」を参考にソースコードを改変しています。
| 関数名 | 概要 |
| setup() | Madgwick フィルタ・I2C 通信セットアップ |
| get_attitude_data(float data[3]) | ロール・ピッチ・ヨー角取得 |
| calculate_attitude() | ・センサ読み取り値更新 ・Madgwick フィルタで、ロール・ピッチ・ヨー角計算 |
| calculate_accel() | 加速度計算 |
| calculate_gyro() | 角速度計算 |
| print_all_data() | 加速度・ジャイロ・磁気センサ計測値をプリント出力 |
| print_attitude_data() | ロール・ピッチ・ヨー角をプリント出力 |
Motor:: モータ出力計算処理 (motor.cpp)
| 関数名 | 概要 |
| setup() | ポート・モータ回転方向・PWM 周波数の設定 |
| limit_command(int &cmd, int min, int max) | cmd を min 〜 max でサチュレーションする |
| stop_motor() | モータを停止する（出力値をゼロにする） |
| format_cmd_data(int cmd_data[4]) | ・送信機から受信したコマンドを処理する ・各モータに出力値を割り振る ・サチュレーション |
| format_pid_data(float pid_data[3]) | ・計算後の PID 値を処理する ・各モータに出力値を割り振る ・正規化・サチュレーション |
| control(int cmd_data[4], float pid_data[3] , Arm &arm) | ・ディスアーム状態ならモータを止める ・PID 値・ジョイスティック値から出力を計算 ・モータ出力 |
各モータへのコマンドの割り振りは以下のようにしています。
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)
| 関数名 | 概要 |
| setup() | – |
| get_pid(float data[3]) | メンバー変数である PID 指令値の getter 関数 |
| calculate_pid(float data[3]) | 姿勢角を元に PID 制御指令値を計算 |
| calculate_id_term() | PID 制御計算用に I項、D項を計算する |
ロール・ピッチ・ヨー角の 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)
| 関数名 | 概要 |
| setup() | – |
| set_arm_status(bool armed) | アーム状態設定（setter 関数） |
| get_arm_status() | アーム状態取得（getter 関数） |
Receiver:: コマンド受信処理 (recv.cpp)
| 関数名 | 概要 |
| setup() | ボーレート設定・LED点滅 |
| notify_bluetooth_setup_finished() | ESP32 のビルトイン LED を 3 回点滅 |
| calculate_checksum() | チェックサムを計算 |
| is_left_switch_pressed() | ジョイスティック左ボタンが押された時 true を返す |
| update_data() | ・受信データの更新 ・受信コマンドのフォーマットチェック ・bluetooth 通信成功可否チェック |
| get_command(int data[4]) | 送信機から受け取ったデータを data[] 配列に格納 |
| set_arm_status(Arm &arm) | ジョイスティックが左下に倒されたときアームする |
| emergency_stop(Arm &arm, Motor &motor) | 以下のとき緊急停止（ディスアームとモータ停止） ・ジョイスティック左ボタンが押下された時 ・11回以上連続でコマンドを受信できない時 ・チェックサムの値がおかしいとき |
チェックサムの計算は以下のようにやっています。
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 同士のデータ送受信のソースコードに関してはこの記事でまとめているので、よければご覧ください。
コンポーネントと役割分担
| コンポーネント | 役割 |
| def_systems.h | ポート・送信データ / 周期・デバッグ用変数設定 |
| transmitter.ino | メイン処理 |
| InputCmd.cpp | ジョイスティックデータ読み取り |
| Transmit.cpp | 受信機へのデータ送信 |
シーケンス図
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)
| 関数名 | 概要 |
| setup() | 初期設定 |
| loop() | main ループ |
Transmit:: 送信機の処理 (Transmit.cpp)
| 関数名 | 概要 |
| setup() | 初期設定 |
| bluetooth_setup() | 受信機とバインド |
| notify_bluetooth_setup_finished() | 3回 LED を点滅させる |
| calculate_checksum (uint8_t *data) | チェックサムの計算 |
| pack_switch_data() | ジョイスティックの左右のスイッチの押下情報を、1 byte のデータにまとめる |
| transmit_data() | ・送信データを7 byte のデータにまとめて、受信機に送信 ・20 ms ウェイトする |
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)
| 関数名 | 概要 |
| setup() | ポート設定 |
| sense_value() | ジョイスティックのデータを読み取る |
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 でまとめてみたいなという気持ちもありますが、これは別の機会にやろうかなと思います。
もし何か質問や改善点等ありましたらお気軽にコメントいただければうれしいです。
コメント