はじめに

このプログラムは作りかけです。
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元数という未知の数字。
なんにもわからないけれど、初めてできることは楽しいですね。