公開日：2018年11月02日
| 更新日：2022年10月14日

新入社員が制作実習でドローンの自作に挑戦した話。(3)

ライター：umamiti
その他

水平に飛行するためには

ドローンを水平に保つためには、「今どれだけ傾いているか」という情報が必要です。そこで、今回は６軸の加速度・ジャイロセンサーを使い、現在の機体の角度をセンシングします。センシングすると言っても、データシートを読むと、マイコンが取得できる値は16ビットのデジタルデータのみ。これをプログラム上でわかりやすく角度の情報に変換する必要があります。

まずは接続！

今回使用する６軸センサーはI2Cスレーブとして動作するものでした。データの読み出しを行うMSP430はI2Cマスターとして動作させ、６軸センサーのI2Cの仕様に合わせてコマンドを送信し、データを取得することになります。具体的には下図のように接続します。

6軸センサーとのI2C接続

はじめてのI2C通信

通信は以下の図のように行い、データの読み取りを行います。

6軸センサーとのI2C通信

上図の流れを順番に説明すると以下のような処理が行われています。

スタートコンディションと呼ばれる、「これから通信を始めるよ。」という合図をI2Cマスターから送信する。
6軸センサーのI2Cスレーブアドレスと、書き込みモード（W）をI2Cマスターから送信する。
ACKと呼ばれる、「確認しました。」の合図が6軸センサーから送信される。
読み取りを行いたいレジスタアドレスをI2Cマスターから送信する。
ACKが返ってくる。
スタートコンディションを再送する。
6軸センサーのI2Cスレーブアドレスと読み取りモード（R）をI2Cマスターから送信する。
ACKが返ってくる。
データが６軸センサー側から送信される（必要であれば2バイト分の読み出しも可能）
読み取りを終了する時はI2CマスターからNACKを送信する。
ストップコンディション（P）と呼ばれる、「ここで通信を終了しますよ。」という合図を送信して、終了。

これらの処理をすべてプログラムで記述していきます。プログラムの詳細については割愛しますが、I2Cの送信完了割り込みや受信割り込みを組み合わせて記述しました。

波形が出力されない！なぜ？

手順通りにプログラムを記述したはずなのですが、そもそも波形が全く観測できません。

そこで、６軸センサーのデータシートをちゃんと確認したところ、クロックは通常100kHz（ファストモードで400kHz）となっており、MSP430側のI2Cのクロック周波数の設定とちゃんと合っていないことがわかりました。

また、センサーのレジスターマップを確認すると、初期化するためのレジスタが用意されており、最初にこのレジスタに値を書き込む必要があることがわかりました。この処理が抜けていたため、波形が観測できなかったようです。

これらの修正や、他にも割り込み処理の記述ミスなども見つかり、それらを修正した結果、、、

I2C波形

無事に加速度、ジャイロそれぞれ16ビットのデジタル値を取得することに成功しました！

角度を算出する！

６軸センサーから読み取ったデータの単位は角度ではないので、加速度（単位：G）と角速度（単位：deg/s）のデータを使って角度を算出する処理を作る必要があります。

角速度とは角度が変化している速度になります。なので、角速度に時間をかけると、角度になります。これは中学校で習う速度：時間：距離の関係と同じです。

電源投入時点に筐体が水平状態である仮定し、そこに角速度から求めた角度を足し合わせて現在の角度を求めます。

角度算出の概要

しかし、個体差や温度、周辺の影響によるノイズが乗ってしまうため、この方法で算出していくとそのノイズ分も足し合わされ、誤差が生じてしまいます。この現象をジャイロドリフトと呼び、このドリフトは別途補正する必要があります。

相補フィルターを使う

このジャイロドリフトを補正するために、今回は相補フィルターというものを使用しました。

相補フィルターとは、角速度から算出した角度と、加速度から算出した角度の両方から、改めて角度を算出するものです。ジャイロは長期的に見るとドリフトの影響を受けますが、短期的には精度がそれなりに良いです。加速度は短期的に見ると機体の運動や振動の影響を強く受けますが、長期的に見るとその観測値は鉛直方向を向いているはずです。よって、ジャイロにハイパスフィルタ、加速度にローパスフィルタをかけて、加算することでより精度の高い角度を算出します。

相補フィルター

a、bをどのような割合で加算すると最も精度が高くなるかは、センサーの種類や機体によって変わってくるので、試行錯誤が必要になります。今回はa=0.8、b=0.2の割合で加算しました。

補正処理を追加する

しかし、これだけではうまくいきません。先程も述べたように、センサーにはさまざまなノイズが乗ります。しかも、静止した状態でもノイズは乗ります。もちろんセンサー内部にデジタルローパスフィルタがありますが、それだけでは不十分だったので補正処理を追加することにしました。

具体的にはセンサーから取得する値にしきい値を設け、しきい値に満たない微小な変化（ノイズによる変化）は0として扱う、というものです。これによって取り除くことができました。

補正処理

次は・・・

次はこの角度の情報をブラシレスモーターの制御に反映させることに挑戦します。この制作実習における最大の山場です。果たしてうまく制御できるのか、お楽しみに！