今回は、磁気センサ(Magnetometer)と2軸カメラマウントを使って、電子コンパスを作ってみます。
入手したのは、MPU9250と言うMCM(Multi-Chip Module)を搭載したセンサモジュール(下図)です。
MPU9250のデータシートを検索する(別ウインドウが開きます)
MCMと言うだけあって、内部に複数のチップを集積したLSIみたいです。データシートを読む限り、2チップが集積されているみたいですね。
ジャイロと加速度センサについては、また別途、応用を考えて使ってみたいと思います。今回使うのは、磁気センサチップだけです。MPU9250に集積されているのは、どうやら、AsahiKASEIのAK8963という磁気センサチップみたいなので、そちらのデータシートも確認しておきます(MPU9250のデータシートにも記載はありますので、必須ではありません)。
AK8963のデータシートを検索する(別ウインドウが開きます)
MPU9250モジュールには、SPIまたはI2Cでアクセスします。今回は、2線式で使い易いI2Cを使います。MPU9250モジュールからは、I2C用の信号線が2組出てています。以下、シルクの信号名で説明します。
実は、MPU9250のレジスタを叩いて、外部I2Cバスバイパスモードに設定してやると、SCL，SDA側からでもAK8963にアクセス可能なのですが、AK8963だけを使いたい今回の用途で、MPU9250のレジスタまで設定するのは面倒なので、ArduinoをECL，EDA側に接続して、AK8963だけにアクセスします。
AK8963のデータシートによると、I2Cスレーブアドレスは0x0cで、高速モード(400kHz)でアクセス可能です。プロトコルはSMBusに似ていますが、バーストアクセスなども可能となっていて、SMBusには準拠していないみたいです。
制御は、いたって簡単：
読み出した値は単位がテスラ(μT)なので、磁束密度でしょうか？調べてみると、地磁気の磁場は、結構複雑なことになっているようです(以下、調査結果まとめです)🌏。
AK8963の読み出しデータ(HX，HY，HZ)も、同様な座標軸が取られていて、(HX，HY，HZ)の方向が地磁気のベクタ方向となっています。センサ(AK8963)を水平に設置した場合、北半球の日本ではHZ＞0で測定されるわけです。
MPU9250モジュールの基板上に、シルクで座標軸が印刷されていますが、この座標軸は、ジャイロおよび加速度センサ用の座標軸です。AK8963の座標軸は地磁気のベクタと同様な方向なので、以下の様に対応しています。
シルクのx：y：z軸 ⇔ AK8963のy：x：−z軸
必要なのは北方位だけですので、z軸の測定値を無視して、x，y方向の成分だけで知ることができます。例えば、ベクタのy軸成分が≒0μTとなる位置にモジュールを回転させると、その時のx軸(シルクのy軸表示)の指す向きが北です。
サーボが2個搭載された2軸カメラマウント(下図)にMPU9250モジュールを取り付けます。
今回の2軸カメラマウントは、パン(旋回)側しか使いません。チルト(傾斜)側は、水平位置に設定しておきましょう。
取り付ける時、サーボの可動範囲とAK8963座標軸の関係は、以下図の様にします(上面図、top view)。
MPU9250モジュールとサーボで角度の＋，−が逆向きだったり、サーボは半周(180°)しか可動出来なかったり、バラバラだけど、プログラムで合わせ込めば済む話です（１＾＿＾；）。
上図とは別な方向で取り付けてしまうと、プログラム(e_compas.ino)が流用できないので、ご注意ください。MPU9250のx軸，y軸は、シルクに印刷されたy軸，x軸に対応していますので、向きに注意してください。
MPU9250(AK8963)で測定した地磁気ベクタがx軸となす角θを算出して、θ＝0°となるようにカメラマウント(サーボ)を回転させると、北方位を指す電子コンパスが完成します。
この時、サーボの可動範囲は半周(180°)しかないので、北方位が可動範囲と反対側に位置する場合には、エラー表示します。
エラー表示にはLEDを使います。
電子コンパスにUSBケーブルが繋がってPCの呪縛から逃れられないと、全く実用的ではありません。独立動作で使用できる様にモバイルバッテリ(や電池など)での動作を想定し、サーボがあまり高速かつ頻繁に回転しないように制御したいと思います。
以下、必要な回路図です。
MPU9250(AK8963)は、定格2.5Vのチップです。最大3.6Vまで供給可能なので、3.3Vで稼働させます。Arduino(5V)との接続なので、L/S(下図)が必要です。
Arduino UNOを使って、モバイルバッテリ(下図)からUSB端子経由で電源を供給します。VDDにはUNOの3.3V出力を接続、VCCもUNOの5V出力を接続します。
購入したMPU9250モジュールの基板を眺めていたら、どうも、レギュレータと思しきチップが載っていました。レギュレータのマーク(S2RK)からでは、メーカや型番などを特定できませんでしたが、Logiscopeの【ina】コマンドでレギュレータの出力電圧を測定してみたところ、約3.27Vでした。Arduinoから3.3Vを供給しちゃったからね……。3.3V−<ドロップアウト電圧>＝3.27Vということでしょうか？
※型番はわからなかったけれど、メーカシンボルからは、MicrOne社のチップらしいことはわかりました。
このモジュール、ほんとは、VCCに5Vを繋がないといけないのかな？レベルシフタが付いているわけでもないし、I/Oが5Vトレラントになっているわけでもなく、降圧レギュレータだけ載っけておいても仕方ないと思うのだけど……。ちょっと、ボードの回路が不明な状態で、VCCに5Vを供給するのは気が引けたので、上の回路図は3.3Vを接続してあります。
レギュレータ通過後の3.27VでもMPU9250の定格電圧範囲内だし、I/Oに3.3Vがかかるのも「＜VDDIO(＝3.27V)＋0.5V」で定格範囲なので、上回路図のままで問題ないと思います。
次はプログラムの設計に移ります。いくつかポイントとなる項目を解説しておきます。
AK8963の内部レジスタにデータを書き込む手順ですが、普通にI2Cライトトランザクションを発行したらおしまいです。
Wire.beginTransmission (i2c_slv_adr);
Wire.write (adr);
Wire.write (dat);
Wire.endTransmission ();
上コードの様に、レジスタアドレスadrと、書き込みデータdatを送信するだけです。連続するレジスタアドレスに連続書き込みするプロトコルもあるみたいですが、使い道が無いので実装不要です。
レジスタのリードは、SMBusの1バイトリードみたいになっています。I2Cダミーライトでレジスタアドレスを指定しておいて、改めてリードアクセスを発行する形式です。以下、該当するコードを示してその下に解説を加えます。
// register address
Wire.beginTransmission (i2c_slv_adr);
Wire.write (adr);
Wire.endTransmission (false); // do not send stop condition
ダミーライトでレジスタアドレスadrを通知します。書き込みはしないので、データは送りません。
ここのダミーライトと、次のリードの間で一度バス権を手放しても、アクセスは可能だと思われます。ただし、I2Cをマルチバスマスタで使う場合、ダミーライトとリードの間で他のバスマスタに割り込まれると、アクセスに失敗するケースがあるので、やはり仕様通り、ダミーライトとリードの間でバス権を離さないようにアクセスすべきです。
処理としては、ダミーライトのWire.endTransmission()に、falseを渡します。こうすることで、WireライブラリはSTOPコンディションを発行しないので、バス権を握ったまま、次(↓コード)のWire.requestFrom()で反復STARTコンディションが発行され、連続してリードアクセスが実行されます。
// receive data
Wire.requestFrom (i2c_slv_adr, cnt);
ダミーライトで通知したアドレスからcntバイト読み出します。AK8963内でレジスタアドレスが自動インクリメントされるので、次々と連続するレジスタ値を読み出すことが可能です。
※Arduinoを使って、SMBus準拠のモジュールにアクセスする場合も、上と同様な制御となります。
AK8963から読み出したデータは、16(または14)ビットの符号付整数です。補正値ASAX，ASAY，ASAZを使って補正すると、以下の式で磁束密度を取得することができます。
14ビット読み出し時：H＝Hadj×4,912/8,190[μT]
16ビット読み出し時：H＝Hadj×4,912/32,760[μT]
今回は、磁束密度[μT]の値その物には興味がなく、地磁気ベクタ(方向)だけ判れば良いので、以下の様にベクタを正規化して終了です。
// normalize vector
hh=sqrt (sq (hx) + sq (hy) + sq (hz));
hx=hx/hh;
hy=hy/hh;
hz=hz/hh;
もっと言うと伏角も必要ないので、z成分hzも無視してしまって良いのだけど、後で無視することが可能なので、ここでは、三次元ベクタとして正規化しています。
地磁気ベクタ(hx，hy，hz)を使って、AK8963のx軸(MPU9250モジュールシルクのy軸)方向と、地磁気ベクタのなす角θを知る必要があります(下図、再掲)。
普通に、アークタンジェントを使って、θ＝atan(hx/hy)で良いのですが、求めた角度θは、hx＝0を境にhxの符号で不連続となってしまって、上図の様な極座標値で算出されません。プログラムで以下の様に補正して使います。
if (hx==0.) // ゼロ除算チェック
th=(hy<0.)? -90: 90;
else
th=atan (hy/hx)*(180./M_PI) + ((hx>=0.)? 0: (hy<0.)? -180: 180);
このθ(＝th)は、x軸正方向が0°となり、負方向で−180°⇔ 180°をまたぐ、上図の様な極座標となります。
地磁気ベクタとx軸(シルクy軸)との角度θが算出できたら、θ＝0°となるような方向にサーボを回転させたら終了です。
モバイルバッテリでの稼働を想定して、あまり頻繁に、また高速にサーボを回転させないよう、「#9-8 3D首振りサーキュレータ」で実施したのと同様に、実際の角度act_panと、目標の角度tgt_panを設定して単位時間(msec_ITVL)当たりの最大回転量(srvo_MAX_SPD)を小さく絞ってあります。
更に、地磁気ベクタは測定する度に時間変動があるので、サーボが小刻みに震えないように、計算したθの1/2しか回転に寄与しない様にしてあります。
// to prevent high frequency revolution
th=th/2;
ですので、実際にやってみるとわかりますが、サーボの回転はとっても遅いです。電子コンパスをあまり速く動かすと、北方位を追従できませんのでご注意ください。
※θ(変数th)や、単位時間(msec_ITVL)当たりの最大回転量(srvo_MAX_SPD)を変更すると、もっと高速に追従できる様になります。ただし、地磁気ベクタ自体、測定値が時間変動するので、北方位が一定方向に定まらず常にサーボが左右に動き続け、電流消費量が増大します。適切に電源設計(電流容量や稼働時間含め)してお使いください。
完成したプログラムをリンクしておきます。
完成プログラム：e_compas.ino
コンパイル＆書き込むと、シリアルモニタから以下のコマンドを受け付けるようになっています。
コマンド仕様：
いずれも、シリアルモニタを使った、システムのデバッグ用コマンドなので、実際に電子コンパスとして使用する時にはシリアルモニタは不要です。モバイルバッテリなどで駆動すると、独立動作で使用可能です。
いきなりモバイルバッテリを使って独立動作させるのではなく、まずはUSBでPCと接続し、シリアルモニタ経由でデバッグしてみましょう。
実験サンプル：【 】内はシリアルモニタからの送信データ
【comp,0】
OK: compass off
独立動作可能な様に、デフォルトでコンパスモードになっています。シリアルモニタから制御するために、コンパスモードをOFFにしました。カメラマウントのパン角度が初期(0°)位置に戻り、LEDが消灯します。
【rd1,00】
OK: rd1[0x00]=0x48
AK8963データシートに記載のある通り、デバイスID＝0x48が読めました。
レジスタへ書き込む【wr1】コマンドも用意しておいたので、一通り手動で制御可能だと思います。
【mag】
OK: mag H=(0.709, -.581, 0.398)
OK: mag H=(0.721, -.553, 0.415)
OK: mag H=(0.734, -.550, 0.396)
地磁気データを測定してベクタ表示しました。何度か実行してみると、かなり、値がばらつくことが解ります。
普段生活していて、地磁気を感じたことは無いと思うのですが、非常に弱い磁気ですので、ちょっとした周りの状況などで測定値が変動するのだと思われます。「ピッタリ真北を指して微動だにしない」なんて幻想はお持ちになりませぬ様。
PCに接続する時に、USB電圧・電流計(下図)で電流値を測定したところ、サーボを切断した状態で約60mA、サーボを接続して回転させると約80mAの電流を観測しました。
eneloop(1.2V，1,900mAh)×2本を使ったモバイルバッテリを使うと、稼働時間は1,900mAh×1.2V×2本/(80mA×5V)≒11.4hと計算できます。方位を測定したい時だけ電源を投入すれば良いので、十分な稼働時間でしょう。
いよいよ、モバイルバッテリから給電した装置を持って動き回り、北方位を指すか試してみましょう。以下、実験中の様子です。
半周(180°)しか可動できないサーボでの実装のため、北方位を指示できない角度が存在するのは今一つ、ですが、360°回転できてしまうと、それはそれでケーブルの取り回しとか面倒になるし、丁度良い仕様制限だと思います。
北方位を指せない時は、LEDが点滅して教えてくれるので、自分がクルっと180°回転すれば、指示できる角度になります。
※サーボモータを使った物理的な磁針に拘らないのであれば、例えば、複数のLEDを丸く(円周状に)配置しておいて、北方位に当たるLEDだけを光らせるとか、そういうアプリケーションにすることも可能だと思われます。
今回のお買い物は、以下の通りです。
上(↑)は、Amazon販売ページに表示された商品名です。「MPU-9255」になっていますが、チップはMPU9250でした(WHO_AM_Iレジスタの読み出し値＝0x71)。中華ショップによくある「看板に偽りあり」です（１＾＾；）。
もし、MPU9250ではなくて、「どうしてもMPU9255が欲しい」と、言う方は、信頼のおける国内ショップで購入することを推奨いたします。
ドローンのヘッドレスモードとかは、こういった磁気センサを使っているのでしょうかね？厳密に実行しようとすると、ドローン本体と送信機の両方にセンサが必要な気がしますが……。ロボットカーの自動操縦とかにも応用できそうですね。
