フライトコントローラーのプログラム読み解いてみた
前回に引き続き、Arduinoを用いてドローンを自作していきます。
※第1弾：準備編
今回用いたプログラムは、オランダの技術系YouTubeチャンネル：Joop Brokkingにて公開されているものです。
結論、このHPからZIPファイルをダウンロードして、解凍したArduinoスケッチ(YMFC-AL_Flight_controller.ino)をコンパイルしてそのままドローンに搭載したArduinoに書き込んだだけ…なのですが（厳密にはPIDチューニングをする必要があります）、当然それだけだと何の学習にもならないので、このコードを読み解いてみました。
細部まで見ていくとキリがないのですが、とりあえず本記事ではドローンを制御するにあたって肝心のところを説明していきます。
コレを読んでいただければこのプログラムの概要はおおよそ把握できるかと思います
前提知識：Arduinoコードのスケッチ
Arduinoについて明るくない方のために、Arduinoで用いられるプログラム（Arduinoではスケッチと呼ばれる）：.inoファイルの構成についてざっとご説明します。
Arduinoプログラム構成
ライブラリ読み込みや変数宣言などがプログラムの冒頭にありますが、Arduinoのプログラムはざっくり以下の3部構成になっています。
①setup関数
②loop関数
③その他関数
①setup関数
Arduino起動時に実行される関数です、ここで入出力に用いられるピン番号の宣言やセンサーキャリブレーションなど、プログラムの初期設定が行われます
②loop関数
次に、loop関数内に記述された処理が実行されます。
名前の通り、この中の処理は繰り返し実行されます
③その他関数
①や②で実行される関数を記述します、このあたりは他の言語と同じです
フライトコントローラープログラム解説
起動してから実行される順に機能を解説していきます
1.PIDパラメーター
setup関数手前の冒頭に、以下の通りPIDパラメーターを記述する箇所があります。ハードコーディングされていますので、実機でチューニングする際は値を手書きで書き換え、都度PCとArduinoを接続して書き換えて飛ばして…を繰り返します。
float pid_p_gain_roll = 1.3; //Gain setting for the roll P-controller
float pid_i_gain_roll = 0.04; //Gain setting for the roll I-controller
float pid_d_gain_roll = 18.0; //Gain setting for the roll D-controller
int pid_max_roll = 400; //Maximum output of the PID-controller (+/-)
float pid_p_gain_pitch = pid_p_gain_roll; //Gain setting for the pitch P-controller.
float pid_i_gain_pitch = pid_i_gain_roll; //Gain setting for the pitch I-controller.
float pid_d_gain_pitch = pid_d_gain_roll; //Gain setting for the pitch D-controller.
int pid_max_pitch = pid_max_roll; //Maximum output of the PID-controller (+/-)
float pid_p_gain_yaw = 4.0; //Gain setting for the pitch P-controller. //4.0
float pid_i_gain_yaw = 0.02; //Gain setting for the pitch I-controller. //0.02
float pid_d_gain_yaw = 0.0; //Gain setting for the pitch D-controller.
int pid_max_yaw = 400; //Maximum output of the PID-controller (+/-)
ロール＆ピッチとヨーのP・I・Dパラメーターを設置します。具体的なチューニング手順は次の記事にて解説します。
※PID制御とは？
2.センサーキャリブレーション
ドローンが飛び立つ前に、まずセンサーのキャリブレーションをする必要があります。
そのため、setup関数内にて以下の処理が実行されます。
for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Take 2000 readings for calibration.
if(cal_int % 15 == 0)digitalWrite(12, !digitalRead(12)); //Change the led status to indicate calibration.
gyro_signalen(); //Read the gyro output.
gyro_axis_cal[1] += gyro_axis[1]; //Ad roll value to gyro_roll_cal.
gyro_axis_cal[2] += gyro_axis[2]; //Ad pitch value to gyro_pitch_cal.
gyro_axis_cal[3] += gyro_axis[3]; //Ad yaw value to gyro_yaw_cal.
//We don't want the esc's to be beeping annoyingly. So let's give them a 1000us puls while calibrating the gyro.
PORTD |= B11110000; //Set digital poort 4, 5, 6 and 7 high.
delayMicroseconds(1000); //Wait 1000us.
PORTD &= B00001111; //Set digital poort 4, 5, 6 and 7 low.
delay(3); //Wait 3 milliseconds before the next loop.
}
//Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset.
gyro_axis_cal[1] /= 2000; //Divide the roll total by 2000.
gyro_axis_cal[2] /= 2000; //Divide the pitch total by 2000.
gyro_axis_cal[3] /= 2000; //Divide the yaw total by 2000.
最初のfor文にて、ジャイロでロール・ピッチ・ヨーそれぞれの角度の値(gyro_axis[1,2,3])を2000回計測し、その結果が変数：gyro_axis_cal[1,2,3]に格納されます。
そしてその結果を2000で割り、平均を出します。
この結果が、ジャイロから角度を計算する関数内にて差し引かれ、正しい角度が計算されます。
3.機体角度の算出
ここからloop関数内の処理に入ります。
//65.5 = 1 deg/sec (check the datasheet of the MPU-6050 for more information).
gyro_roll_input = (gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3); //Gyro pid input is deg/sec.
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch / 65.5) * 0.3);//Gyro pid input is deg/sec.
gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3); //Gyro pid input is deg/sec.
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_pitch * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable.
angle_roll += gyro_roll * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable.
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch -= angle_roll * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel.
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel.
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector.
if(abs(acc_y) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle.
}
if(abs(acc_x) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle.
}
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration.
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch.
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll.
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle.
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle.
まずこの処理全体に言えることですが、ジャイロで計測・計算された角度を、そのまま現在の機体角度とはしておらず、
現在の機体の角度 ＝ 1つ前のループで計算した機体の角度 x A + 今回のループでジャイロの値から計算した機体の角度 x B
※ A + B = 1
といった具合に計算されています（言葉にするの難しい、伝われ…
具体的には
gyro_roll_input = (gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3);
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch / 65.5) * 0.3);
gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3);
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
がコレに当たります。
つまり、現在の計測結果をそのまま反映させるのではなく、1つ前の計測結果とブレンドして、変化がマイルドになるように調整されているわけです。
各項の係数を変えて検証しているわけではないのでなんとも言えませんが、こうしないと機体の変化が急激になりすぎてスムーズに飛ばないのだと思われます。
本当はこのA、Bの割合をカルマンフィルターとか使って設定するのがベストなのかもしれませんが、今回はそこまではやらずハードコーディングしちゃいます。
また、この箇所では単位変換のためにいくつかの定数(65.5、0.0000611、0.000001066等)で掛け算・割り算が行われています。このあたりはアルゴリズムの肝ではないので説明は省略します。
※ちなみにこれらの値の定義などはプログラム内のコメントやJoop Brokkingの動画にて丁寧に説明されていますので、詳しく知りたい方はそちらをご覧ください
次に、以下の箇所ではオイラー角の計算処理をしています。
angle_pitch -= angle_roll * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel.
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel.
機体が傾いた状態でヨー方向に回転してもロール・ピッチの値が正しくなるように補正がなされています。
※オイラー角をご存じない方へ：このJoop Brokkingの動画がわかりやすいです
次に、今度はジャイロから送られる並進の加速度情報から角度を算出します。これまでで（各加速度が元となって）計算してきた角度情報ではなく、並進の加速度からも角度を計算します。
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector.
if(abs(acc_y) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle.
}
if(abs(acc_x) < acc_total_vector){ //Prevent the asin function to produce a NaN
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle.
}
acc_x,y,zが各方向の加速度で、acc_total_vectorがその合成ベクトルです。
これらを元に計算されたピッチ・ロール角がangle_pitch_acc,angle_roll_accです。
最後に、
・ジャイロ（角加速度）
・加速度
から計算されたそれぞれの角度の値をミックスして、最終的な機体の角度であるangle_pitch, angle_rollを算出します。
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
4.PID制御
loop関数から呼び出されるcalculate_pid関数内にて、以下の処理が実行されます。この処理ではロールについて扱っていますが、ピッチ・ヨーについても同様の処理がされています
pid_error_temp = gyro_roll_input - pid_roll_setpoint;
pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
if(pid_i_mem_roll > pid_max_roll) pid_i_mem_roll = pid_max_roll;
else if(pid_i_mem_roll < -pid_max_roll) pid_i_mem_roll = -pid_max_roll;
pid_output_roll = pid_p_gain_roll * pid_error_temp +
pid_i_mem_roll +
pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
pid_last_roll_d_error = pid_error_temp;
pid_error_tempが、現在の角度と送信機から送られた目指す角度とのギャップを意味します。
例えば機体がホバリングしている(0°)状態から右に移動させたい(機体を5°傾ける)とき、pid_error_tempは 0° - 5° = -5° となります。
pid_i_mem_rollにpid_error_tempにゲインをかけ合わせた値を足します、つまり積み重ねていって積分していくわけです。
そして最終的なロールの出力であるpid_output_rollが、
・pid_error_tempにゲインをかけた値：P
・pid_i_mem_roll：I
・前回のループ時のpid_error_tempと今回のpid_error_tempの差異（つまり微分値）にゲインをかけた値：D
を足し合わせることで得られます。
5.モーターのスロットル制御
4にて計算されたロール・ピッチ・ヨー、そして送信機から送られるスロットルの値を元に、4つの各モーター(性格にはESC：アンプ)へと出力を伝えます
esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw;
esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw;
esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw;
esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw;
角モーターへはPWMで出力を伝えます、スロットル範囲は1100〜2000μsになります
以上がフライトコントローラーのしくみの概要です、
他にもバッテリーの減少を検知してLEDを点滅させる機能やエイム機能（特定の操作をしないとモーターが回転しないようにする安全機構）もあったりするのですが、今回は割愛します。
興味のある方はぜひJoop Brokkingの動画を見て、HPからDLしてみてください！
【引用】
・YouTube：Joop Brokking
・HP：Brokking.net
◯第1弾：準備編
◯第3弾：実機組み立て・PIDチューニング編