カルマンなジャイロ・地磁気センサ「JY901」の使い方
ロボットなどの工作物で角度をセンシングしたくなるときってありますよね。私の場合は, RCJ (ロボカップジュニア) でロボットを作ってるので, 正確かつ外乱に強い,しかもYaw軸(Z軸)が読める角度センサを探してたのですが, メジャーなMPU6050などを使ってみても会場の電力系統の影響をモロに受けたり, どうしてもドリフト (静止していても角度が徐々に変化してしまう事) したりしてしまって, どうもうまくいかない時がありました。そこで, JY901というジャイロ&地磁気センサの存在を知り, 実際にAliexpressで購入, 動かしてみるとかなりうまくいったので使い方を記事に書いておきます。
購入したのはここ。
以前はもうちょっと安かったんですが, なんか微妙に高くなりました。Wit-Motionという中国のメーカが製造してるセンサで, 地磁気, 加速度, ジャイロ, 演算角度, 温度, 気圧, 高度などがUART, I2C, PWMのいずれかの方法で出力できます。この値段でこの性能はなかなか強い方ではないでしょうか。
JY901の使い方
このセンサ, Arduino用のライブラリなど公開されているのでそれを引っ張ってきて使うのが一番楽かもしれませんが, 私の場合愚直にUARTで操作してもそんな難しくないなぁと思いました。データシートは以下。
http://wiki.wit-motion.com/english/lib/exe/fetch.php?media=module:wt901:docs:jy901usermanualv4.pdf
シリアルのフォーマットは単純で, センサからマイコン等ホストに送信されるデータは
0x55 | データ種類 | 8バイトのデータ本体 | チェックサム
となっています。データ種類バイトの値によってホストは何のデータかを判断できます。また, 最後にはサムバイトが返ってくるのでデータの破損もチェックできます。データ種類の値とデータ内容の関係はデータシートを見れば分かるとおもいます。
例えば, Arduinoで加速度を取得したい場合はおおよそ次のようになります。
(手順説明用に適当に書いたのでこのままでは動かないよ!)
- void setup(){
- Serial.begin(9600); //JY901のUARTボーレートはデフォで9600 baudです。
- }
- void loop(){
- byte buff[11];
- if(Serial.available()){ //シリアルバッファにデータがあることのチェック
- buff[0] = Serial.read();
- if(buff[0] == 0x55){ //データが来たことのチェック
- buff[1] = Serial.read();
- if(buff[1] == 0x51){ //加速度のデータ種類は 0x51
- for(int i=0; i<9 ;i++){
- buff[i+2] = Serial.read(); //データ本体+サムの取得
- }
- }
- }
- }
- }
実際には取りこぼさないような工夫やマイコンの処理速度等考えて書かないといけないんですが…。まぁこの手順で buff 配列に加速度のデータが入りますので, あとは煮るなり焼くなり。
データの取得はデータシート通りのフォーマットに従えば簡単にできました。次に, ホストからJY901に命令を出すシリアルフォーマットについて。データシートによれば, ホストからJY901へのシリアルフォーマットは次のようになります。
0xFF | 0xAA | 操作種類 | データ下位バイト | データ上位バイト
操作種類バイトの値はデータシートの Register Address table に乗ってます。ホストからJY901へのデータ送信は受信より簡単かもしれません。例えば, リターンレート (1秒間に何回データを送ってもらうか) を200 Hz に設定するのは以下のようです。
- void setup(){
- Serial.begin(9600);
- }
- void loop(){
- byte buff[5] = {0xFF, 0xAA, 0x03, 0x0B, 0x00}; //return rate を 200 Hz にする命令
- Serial.write(buff, 5); //バッファの内容を5バイト送信
- }
これでJY901は1秒に200回データを返してきます。(デフォでは10回) 実際には, デフォのボーレート 9600 で 200Hzデータもらうのはハードウェア的に厳しいので, ボーレートを上げるかリターンレートを下げることになるでしょう。ボーレートを115200に上げるのは次のようにします。
- void setup(){
- Serial.begin(9600);
- }
- void loop(){
- byte buff[5] = {0xFF, 0xAA, 0x04, 0x06, 0x00}; //baud rate を 115200 にする命令
- Serial.write(buff, 5); //バッファの内容を5バイト送信
- }
当然この命令を実行した瞬間にJY901のボーレートが9600から115200に変わるので, 通信できなくなります。ボーレートが正常に変更できていれば Serial.begin(115200); に変更することで通信を再開できます。
以上の通りJY901はUARTで制御すると簡単なんですが, I2Cでも基本やる事は同じです。フォーマットは違いますが手順は同じですね。
Processed Angle (演算角度) について
JY901は, 地磁気, 加速度, ジャイロのデータを使ってカルマンフィルタをかけ信頼できる角度を算出する 9軸演算モード と, 加速度, ジャイロのデータを使う 6軸演算モードがあります。9軸演算モードで計算されたアングルはJY901のロットにも精度が大きく変わることが分かっていますので, 6軸演算モードを使うのが無難です。一時期のロットでは, 9軸演算モードにするとキャリブレーションしてもドリフトが発生したり, 角度の収束に数秒の遅延が発生したりと散々な結果になってましたね(^_^;) 演算モードの設定はデータシート 7.2.6 Algorithm transition の通りです。
センサキャリブレーションについて
加速度センサ, 地磁気センサは購入直後の状態ではまともに使えませんので, キャリブレーション (較正) を行う必要があります。キャリブレーションはシリアルで命令を送ることでもできますが, 目で状態が見えないので miniIMU という公式(?)GUIソフトを使う事をオススメします。といっても, なんかバグだらけのソフトなんですが, キャリブレーションくらいはちゃんと動くみたいですので。ダウンロードは以下のgithubから。
隠しコマンドについて
データシートに書いてない命令を見つけてしまったので置いときます。ロボカップではもはや必須な操作, 「Yaw(Z)軸の0点設定」です。この命令を送信すると, 現在のYaw角度を0度として演算角度を返すようになります。
0xFF | 0xAA | 0x01 | 0x04 | 0x00
I2Cについては使ってないので未調査です(^_^;)