くそ長タイトル
前回はMPU9250の値を取りました
MPU9250の値を取るだけでは，重力加速度しか分からないので姿勢を計算していきます
ただ，修論もあるし手軽にROSのパッケージがあるMadgwick Filterを使います
ドローンなら，ラズパイとMPU9250で姿勢が取得出来て，PWMをだしてアンプを動かして飛ばせるのではと思います
環境
前回と同じです
Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic
9軸センサ:MPU9250
プログラム
github.com pigpioを使ったSPI通信とMPU9250の値の取得は前回の記事を参照してください
センサデータ変換
double acc_x = mpu9250.get_acc_x()/16384.0*9.81; double gyr_x = mpu9250.get_gyr_x()/130.072*0.01745;
今回は，加速度と角速度だけを使います
imu/data_rawというデータをimu_filter_nodeに与えます
なので，imu/data_rawを自分で作る必要があります
センサデータをimu_filter_nodeが求めている単位に変換して与えます
何度か実験してみた所，
加速度は，m/s2
角加速度は，rad/s2
が必要ということが分かりました
式の意味は前回の記事を参考にしてください
imu/data_rawの作成
sensor_msgs::Imu Imu;
Imu.header.frame_id ="imu";
Imu.header.stamp = ros::Time::now();
Imu.linear_acceleration.x = acc_x;
Imu.linear_acceleration.y = acc_y;
Imu.linear_acceleration.z = acc_z;
Imu.angular_velocity.x = gyr_x;
Imu.angular_velocity.y = gyr_y;
Imu.angular_velocity.z = gyr_z;
もしかしたら，分散の場所に値を入れればMadgwick Filterが上手いこと何か処理してくれるかもしれませんが，ここでは値を入れてpublishするだけです
Madgwick Filterの追加
ソースにgit cloneで持ってきてコンパイルします
ただ，ラズパイだとフリーズする可能性があります（一度フリーズしました）
なので，makeするときは
catkin_make -j1 -l1
でしましょう
同時に起動するlaunchファイル
MPU9250から加速度，角加速度を取得するノード，Madgwick Filterノードを同時に起動するlaunchです
この時，パラメータのuse_magをfalhseにします
これで地磁気を使わずに計算します
また，パラメータのreverse_tfはtrueにします これによって，imuリンクからodomリンクを生成します (これがないと，rvizで表示した時に基準となるtfがないために姿勢が変化しているか分かりません)
実験結果
起動するとなぜかodomリンクからyaw軸に90度回転したところから始まります
yaw軸は地磁気がないと補正は難しいようです
ですが，十分使える精度だと思います
まとめ
今日はまとめで一日使ってしまった
明日からは研究しよ
