X,Y,Z3軸傾斜角をはじめから求めようとするとかなり複雑のため、この記事では1軸分(Y軸)まわりの傾斜角を加速度センサおよびジャイロセンサから算出する方法を解説します。

MPU-6050では下図の向きにX、Y、Z軸を設定しています。

加速度センサでY軸まわりにθ_y傾斜する場合はZ軸方向およびX軸方向の重力比率より傾斜角θ_yは逆三角関数を使用すると算出できます。

加速度センサアプリでも説明しましたが、加速度センサの重力(1G)成分による各軸の値a_x,a_y,a_zは重力に対する抗力の向きとなりますので、下図のようにY軸中心に座標を正方向θ_Y傾斜したとき、重力Z軸成分(1G_Z)の抗力a_zは正値で、X軸成分(1G_X)の抗力a_xは負値となります。

ここでは動作による加速度は考えていませんので算出データは地上に対して傾斜させて静止した角度(絶対角度)となります。

算出した絶対角度にオフセットθ_offset分を差し引くと、任意の角度で基準0°にプリセットできるので基準からの相対角度とすることができます。逆三角関数を使用した場合の角度は単位がradのため、必要に応じて°に変換します。

ジャイロセンサの場合、データg_yはY軸まわりの回転時の角速度のため、角度を得るにはプログラム内で積分して算出する必要があります。

まず得られた16ビットデータをスケールに応じた角速度（°/s）に変換します。それからオフセットがある場合は予め差し引いておいてから積分して角度を算出します。

プログラム内での積分は離散化した近似です。最もシンプルな積分は下図のアルゴリズムです。サンプリングタイムTsが十分小さい場合はこれでも問題はありません。

MPU-6050をNucleo-F103RBに接続して使用する場合の配線例を下図に示します。

センサボードGY-521の電源にはLDOレギュレータを内蔵していますので5Vを接続します。

データ出力インターフェースはI²C通信のため、STM32側ではI²C2に接続します。I²CのI/Oはオープンドレインに設定しますので通常ではプルアップ抵抗が必要ですが、センサボードGY-521には予め含まれています。

まず、はじめの関門はI²C通信の設定およびデータ読み書きを正常に実現することです。データを任意に安定して読み込めることができればあとはこれまでに解説した方法でデータを演算して角度を算出するだけです。

I²C通信デバイスを使用するために、I2C2_Configuration()でまず初期設定をします。これは他のI²Cデバイスと同様です。詳細はシリアル通信I2C【STM32のI2C詳細】を参照してください。

I2C2の初期設定のあとはMPU-6050の設定をします。設定はレジスタに値を書き込みます。すべてデフォルトで使用する場合は必要ありませんが、デバイスに内蔵のデジタルフィルタを使用したり、データのフルスケールを変更設定する場合には必要です。

I²C通信では1バイト単位でデータを読み書きするのですが、データ読み込みでは個々のバイトデータを単独で読み込むのではなく、一度に複数バイトを読み込む方が効率的です。

そうなると読み込みデータアドレスは加速度センサのX軸のデータアドレス3Bから14バイト分を読み込んでデータを配列に格納するだけでよいのです。

話は前後しますが、所望のレジスタに1バイトデータを書き込むための関数は自作します。

ここでのポイントはスレーブアドレスで、I²Cのアドレス送信関数I2C_Send7bitAddress()ではアドレスは1バイト枠で左詰め7ビット分です。これに最下位ビットに送信か受信フラグを付加した１バイトデータとして扱います。

ライブラリ関数の中身を確認して動作をたどると理解できると思います。

例えばI²Cデバイスのスレーブアドレスが0x68の場合は1ビット分左シフトした0xD0を送信関数I2C_Send7bitAddress()に与えるアドレスとします。

データ読み込みは書き込みの延長のようなものですが、複数バイトデータも連続して読み込むことができます。

これも自作の複数バイト読み込み関数ですが、データが複数ある場合は最後のバイトデータ受信直前でACKを返さないNACKとすることがポイントです。

MPU-6050 は初期設定としてレジスタへセンサへのクロック源を設定したり、加速度センサ、ジャイロセンサのフルスケールや出力へのフィルタを設定します。

加速度センサおよびジャイロセンサにより角度を取得するだけならばデフォルト設定でも使えますが取扱うレジスタはせいぜい下記の程度です。

クロック源を指定することで MPU-6050は機能始めます。クロック源はレジスタアドレス6Bの下位3ビット分に設定します。電源投入時のデフォルトでは内部発振8MHzとなっています。

MPU-6050には出力にノイズ対策のローパスフィルタを設定することができます。必要に応じてレジスタアドレス1Aの下位3ビットに0から6までの値を設定します。

レジスタアドレス1Bではジャイロセンサフルスケールを0から3までの値で設定します。デフォルトでは±250°/sです。

レジスタアドレス1Cでは加速度センサフルスケールを0から3までの値で設定します。デフォルトでは±2gです。

加速度センサの各軸データは16ビットですが、データ格納アドレスは8ビットですので上位8ビット、下位8ビットで構成されます。X軸、Y軸、Z軸のデータはレジスタアドレス3BよりX軸上位、下位...と順に格納されます。

データについては例えばフルスケールが±2gの場合は、+2g時に32768となるので1g当たり16384となります。

ジャイロセンサの各軸データも加速度センサと同様にX軸、Y軸、Z軸のデータはレジスタアドレス43よりX軸上位、下位...と順に格納されます。

データについては例えばフルスケールが±250°/sの場合は、+250°/s 時に32768となるので1°/s当たり131となります。

加速度およびジャイロセンサからのデータが取得できるようになるといよいよそれぞれのデータから角度を算出できるようになります。

I2C_Read_multibyte()関数による複数バイト読み込みで格納したデータにより加速度およびジャイロセンサ各軸の16ビット値が取得できます。

取得できた値により加速度センサによる角度およびジャイロセンサによる角度が算出できます。

ジャイロセンサからの角速度データを積分して角度を算出するためのサンプリングタイムを正確に5msとするためにRTOSの機能を利用しています。詳細はFreeRTOSタスク管理の基本を参照してください。

加速度センサおよびジャイロセンサから単独で算出された角度はそれぞれ一長一短あり安定しません。そこで、両センサの長所である部分をいいとこ取りするための手段に相補フィルタというものを使います。

加速度センサでは重力変化による傾斜角度が得られるので特に静止状態の場合の絶対的な角度変化は信頼性がありますが、情報にノイズが含まれていたり、動作が伴うと重力以外の成分も加わることにもなります。

対して、ジャイロセンサでは回転を検知した角速度が得られるので動作の伴う角度の変化には信頼性がありますが、プログラム内で積分をしているので誤差も同時に累積しドリフトを発生してしまうことになります。

そこで相補フィルタを使用して、加速度センサ出力にはローパルフィルタ(LPF)により低周波部分だけをとりだし、ジャイロセンサ出力にはハイパスフィルタ(HPF)により、ドリフト分をキャンセルすることができます。

相補フィルタの式だけをみると、至ってシンプルなのですが、奥が深いので別途、相補フィルタのしくみを解明してみる【加速度・ジャイロセンサ】で詳細を解説しています。

下図は加速度センサ、ジャイロセンサおよび相補フィルタを通した出力による角度変化の様子をグラフ化したものです。横軸は経過時間(s)、縦軸は角度(°)です。

加速度センサによる角度は細かなノイズが含まれていますが絶対的な角度を示すのでドリフトは発生していません。

ジャイロセンサによる角度はプログラム内で積分したものなのでノイズもなく滑らかですが、累積誤差によるドリフトが発生しています。

相補フィルタを通した出力では加速度センサによる出力とジャイロセンサによる出力の長所のみを引き継いだものになっていて安定して信頼性があります。相補フィルタのカットオフ周波数は2Hzに設定したものです。

相補フィルタのカットオフ周波数によりどのような違いがあるのかを確認するために4Hzと2Hzで比べてみました。

カットオフ周波数を4Hzに設定した場合はジャイロセンサによるドリフトの影響はないのですが、加速度センサの敏感なノイズ成分が多少含まれています。加速度センサ寄りの影響を受けた出力になっています。

カットオフ周波数を2Hzに設定した場合は加速度センサのノイズ成分はほぼキャンセルされ、ジャイロセンサ寄りの影響を受けた出力になっています。ジャイロセンサ出力からドリフト分だけをキャンセルした出力となっています。

下の図はカットオフ周波数を2Hzの場合で、少し速い動作をさせてみた出力結果です。

加速度センサでは速い動きほどノイズ成分が顕著に現れ、ジャイロセンサではドリフト成分によるオフセットが突然大きくなったりでこのままでは使用できません。

対して、相補フィルタを通した出力ではこのような動作でも遅れは見られずしっかりと追従し、ドリフトの影響もなくほぼ理想の状態となっていてフィルタ効果が確認できます。

ここではI2C2を使用するためにPB10とPB11を使用します。メモリから読み込んだデータを表示するためにUSART3を使用してモニターへ転送しています。

I²C EEPROMのチップアドレスは上図のようにA0,A1,A2を０VにしているのでA0です。詳細はI²C EEPROM 24LCxxのデータシートで確認してください。

3つのデータ（data1,data2,data3）をI²C EEPROMに書き込んだり読み出したりしています。

プログラム起動直後はあらかじめEEPROMに保存されているデータを表示します。

３種類の文字列コマンド"1@", "2@", "3@”のどれかをPCで入力すると、そのコマンドに対応したデータがEEPROMに書き込まれ、1000msサイクルごとに保存されたデータを読み出し表示します。

データはコマンドを入力するたびに書き換えられます。

シリアルUART通信によりPCから操作して、読み書き（送受信の機能）の状態をモニターしています。

シリアル通信I²Cは同期型の双方向通信です。Philips社が提唱したシリアル通信方式です。信号線が2本だけでそのうちの一本がデータを双方向でやり取りする信号線SDA（Serial Data Line）で、他方が同期用クロック信号線SCL(Serial Clock Line)です。

特徴としてはマスタ側とスレーブ側に分かれ、マスタ側が通信をすべてコントロールしています。I²C では一つのバスに複数のスレーブを接続することができます。バスはオープンドレインで接続されているためプルアップ抵抗（電源電圧が3.3Vのとき、2.2k程度）が必要です。

I²Cでは1文字（1バイト）ずつ送受信し,1バイト転送毎に受信側からACK（アクノリッジ）信号を返送し、互いに確認を取りながら（ハンドシェイク）転送を行います。マスタが受信を終了させたい時点でACKを返さないNACK（ノットアクノリッジ）とします。

その後ストップコンディションを生成して通信を終了させます。I²Cデバイスは個々にスレーブアドレスを持っていて、スレーブデバイス内の指定アドレスのレジスタやメモリとアクセスしてデータのやり取りをします。

ここではI²C通信仕様のEEPROMを使用したデータの送受信を実際のプログラムで解説していきます。
I²C通信を使った送受信はルールが規格できまったものであるためメーカーなどが作成したサンプルプログラムなどを参考に、ある程度決まった形式のものを使用するのが無難です。

まず、他のペリフェラルと同様に初期化を解説します。

使用するI²C1にクロックを供給します。APB1バスに接続していますのでAPB1に供給します。

I²C1に使用するIOは送受信にPB6とPB7をオルタネートオープンドレイン出力に指定します。

I²C通信の初期設定を実行します。初期化はI2C_Init関数を実行して上記のパラメータを設定します。

関数の第1引数は設定対象のI²C（I2C1,I2C2）、第2引数は構造体メンバになっていて以下に示すとおりです。

I²C_ModeメンバにはI²C通信ではI²Cモードを指定します。

I2C_DutyCycleメンバには通常クロックパルスデューティ比で1:1にしておきます。このメンバは高速クロックの波形整形のためのものでクロック周波数がファストモードの場合にのみ有効なメンバです。

I2C_AckメンバはACK（アクノリッジ）を返す指定にするためI2C_Ack_Enableとします。

I2C_AcknowledgeAddressメンバはスレーブアドレス幅を指定します。スレーブデバイスの仕様に従い、ここでは7ビットを指定します。

I2C_ClockSpeedメンバはクロック周波数を指定します。ここではI2C_CLOCK(200000)を指定します。

クロック周波数が100kHz以下であれば標準モード、それ以外ではファストモードが自動的に選択されるようになっています。200000の場合はファストモードとなります。

これまででI²C通信の初期化ができましたのでI2C_Cmd関数を実行してI²Cポートを有効にします。

関数の第1引数には設定対象のI2C1かI2C2を指定し、第2引数はENABLEで有効、DISABLEで無効となります。

I²C書き込みは下記のフォーマット手順に従った書き込み関数I2C_EEPROM_Write()を作成し実行します。関数内ではコマンドを時系列で実行していきます。

STM32のI2C通信ではデータ転送の合間にさまざまなイベントが発生しています。実際のプログラムにおいてはイベントの状態を確認しながら手順を進めていくことになります。

スタートコンディションはI2C_GenerateSTART関数を実行して生成すると自動的にマスターモードとなり、スレーブ側との通信を開始します。

関数の第1引数は設定対象のI²C（I2C1,I2C2）、第2引数はENABLEで有効、DISABLEで無効となります。

I2C_CheckEvent関数を実行して、ここではマスターモードになったかどうかを確認しています。

ここではI²C通信のモードを設定します。I2C_Send7bitAddress関数を実行して、スレーブ側（EEPROM）のアドレスとデータ方向（受信か送信）を指定します。

関数の第1引数は設定対象のI²C（I2C1,I2C2）、第2引数は8ビットアドレスなのでスレーブアドレス7ビットを左詰めにして指定します

採用している回路ではEEPROMアドレスは上位4ビットが1010の固定で、下位3ビットがA0/A1/A2の指定で000の1010000(7ビット）です。

これを左詰めにして10100000(0xA0)（EEPROM_SLAVE_ADDRESS）で指定します。第3引数には書き込みの場合ですのでデータ方向I2C_Directionに送信(Transmitter)を指定します。ここでもI2C_CheckEvent関数を実行して、ここでは送信（トランスミッタ）モードになったかどうかを確認しています。

I²Cスレーブ側（EEPROM）に書き込むデータを格納するアドレス2バイト分を指定します。I2C_SendData関数を実行して上位、下位に分けて指定します。

関数の第1引数は設定対象のI2C（I2C1,I2C2）、第2引数はデータを格納する2バイトアドレスを指定します。アドレス指定の範囲は各EEPROMの容量によりますのでデータシートで確認して使用できる範囲で指定します。

EEPRONに書き込みたいデータをデータレジスタに転送します。③のアドレス転送時と同様でデータの上位と下位に分けて転送します。

ストップコンディションはI2C_GenerateSTOP関数を実行して生成します。関数の第1引数は設定対象のI²C（I2C1,I2C2）、第2引数はENABLEで有効、DISABLEで無効となります。

EEPROMへの書き込みは前述した書き込み関数を実行してストップコンディションを生成してから開始します。EEPROMへの書き込みにはある程度時間がかかるため、連続してデータを書き込むにはデータ転送が完了したことを確認してから次のデータを書き込まなければなりません。

この処理に関してはSTM32メーカーにより作成したI2C_EEPROM_Poll関数がありますのでそのまま使用します。EEPROM書き込みの際にはI2C_EEPROM_Write関数とセットで実行します。

I²C読み込みは下記のフォーマット手順に従った読み込み関数I2C_EEPROM_Read()を作成し実行します。この関数ではEEPROMの特定のデータが格納しているアドレスを指定するとデータを返します。

読み込みの場合はI²Cバスが事前に空いているかどうかを確認してから手順を始めます。
①から③までは書き込み関数の場合と同じです。ここで読み込みたいデータが格納しているアドレスを指定します。ここまではアドレスを指定するために送信モードで行っていました。

ここから指定したアドレスのデータを受信するための切替にスタートコンディションを再度生成します。

I²C通信モードを受信にするためにI2C_Send7bitAddress関数を実行して、②と同様に第2引数はスレーブアドレスを、第3引数には読み込みの場合ですのでデータ方向I2C_Directionに受信(Receiver)を指定します。ここでもI2C_CheckEvent関数を実行して、ここでは受信（レシーバ）モードになったかどうかを確認しています。

データ受信の準備ができたところでI2C_CheckEvent関数を実行して、受信レジスタにデータ転送完了の確認に続いて2バイトデータを上位1バイト分、下位1バイト分の順に受信します。

この時点で受信を終了させたいのでACKを返さずにNACK（ノットアクノリッジ）とするために、I2C_AcknowledgeConfig関数でACKを無効にしています。この例では受信する2バイトデータのうち最後の1バイト（下位バイト分）を受信する前に実行してNACKを返しています。

ストップコンディションを生成します。

次回の通信に備えてACKを有効に戻します。

受信した上位、下位データから2バイトデータに処理し、読み込み関数I2C_EEPROM_Readの返り値とします。