はじめに
このプログラムは作りかけです。
RollとPitchはいい感じに取得できるのですが、Yawが時々暴走して明後日の値になってしまいます。きっと何か計算が間違っていると思うのですが、未だ原因特定できません。
概要
TI SensorTagは、いくつもセンサーを搭載した小さなデバイスです。BLEで接続し、それなりに簡単(?)にセンサーから値を取得できます。
そのセンサーのうち、加速度、角速度、磁気（いわゆる9軸ジャイロ）から、デバイスがどちらに傾いているか、どちらに向いているかを計算します。
環境
- TI SensorTag CC2650 (Firmware 1.40 Mar 30 2017)
- macOS High Sierra
- Python 2.7.10
- AdaFruit_BluefruitLE
macOSで使えるPythonライブラリが無くて探しました。
macOSで開発するなら、PythonよりNodeの方がいいかもしれません。
BLE接続について
BLEって低電力なBluetoothだと思っていました。
いえいえ、全然違いました。
BLEデバイスは、以下のような手順で統一的に操作できる便利なやつでした。
- ご近所のデバイスをスキャン
- デバイスにあるサービスをリストアップ
- サービスにあるキャラクタリスティクスをリストアップ
- キャラクタリスティクスを指定して値を読み書き
Lightblueというアプリを使うと、上記の操作が簡単にできます。
Lightblueを使ったお試し
Lightblueを起動すると、デバイスを検索してくれます。
デバイスが見つかったら、F000AA80-0451-4000-B000-000000000000というサービスを辿ります。
F000AA82-0451-4000-B000-000000000000というキャラクタリスティクスが設定とセンサーのOn/Off切り替えです。
接続した直後はOffになっています。
ここに0x7f00を書き込むと、F000AA81-0451-4000-B000-000000000000というキャラクタリスティクスからセンサーの値を読み込めます。
どんな値を読み書きすればよいのかは、下記をご覧ください。
http://processors.wiki.ti.com/index.php/CC2650_SensorTag_User%27s_Guide#When_connected
Roll/Pitch/Yawの計算について
センサーから取得した値から単純に計算すると、何もしてなくてもどんどんずれていってしまいます。
Qiitaを検索すると、Kalmanフィルタを使えという記事が見つかりました。
でも、欲しいのはRoll/Pitch/Yawなのです、さらに探すとMadgwickというものが見つかりました。
ありがたいことにC#のソースが公開されていたので、Pythonへ必要分を移植しました。
http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
論文はこちら ＞ Madgwickアルゴリズム
作成したプログラム
githubに置きました。
プログラムの実行
sample/sample.pyファイルを実行すると、60秒間Roll/Pitch/Yaw（というかオイラー角）を読み込んで画面に出力します。
Help Me!
地磁気を基準値として使うYawがおかしいです。
センサーを動かしていなくても、どんどん値が変わっていくことがあります（変わらないこともあります）。
どこをチェックすべきでしょうか。
感想
初Python、初BLE、初センサーと初ものづくしで、右も左もわからない。
さらに4元数という未知の数字。
なんにもわからないけれど、初めてできることは楽しいですね。