夏シンポが終わってひと段落
前回まででGPIOでLチカをしてみたり，サーボを動かしてみたり，シリアル通信をしてみました
mozyanari.hatenablog.com mozyanari.hatenablog.com mozyanari.hatenablog.com
今回は，Raspberry Pi のROSからSPI通信でMPU9250を動かしてみますマイコン でやればいいんじゃないと思うかもしれませんが，ラズパイでやりたい理由は以下の二点です
すべてをRaspberry Pi のROSで処理させて，ROSパッケージの恩恵を受けたい
ロボットに乗るように出来るだけ小さく作りたい
センサを取得したり，オドメトリとのセンサフュージョン をしようと思うときには結局マイコン からラズパイにデータを送る必要がありますwiki.ros.org
また，STMでセンサデータを受信しようとした時，少ないピンの表面実装のパッケージを使えば小さくできなくもないですが，今考えている移動ロボットはLRFが載ってますし結構キツキツなスペースです
なので，出来るだけROS上でセンサデータを取得したいです
今回の目標はMPU9250の加速度と角加速度をSPIで取得して，重力加速度を観測します
環境 Raspberry Pi 3 BUbuntu Mate
プログラム github.com
pigpioの初期化
int SPI_MPU9250::pigpio_initialize(){
pigpio_check = pigpio_start(NULL ,NULL );
if (pigpio_check < 0 ){
ROS_ERROR("pigpio Initialize Error" );
throw ;
}else {
ROS_INFO("pigpio initialize OK" );
return pigpio_check;
}
}
ここではpigpioの初期化をしています
pigpioのSPIの使い方
int SPI_MPU9250::spi_initialize(){
unsigned int spi_channel = 0 ;
unsigned int baud = 500000 ;
unsigned int spi_flags = 0 ;
spi_flags = spi_flags | 0b00 ;
spi_flags = spi_flags | (0b000 <<2 );
spi_flags = spi_flags | (0b000 <<5 );
spi_flags = spi_flags | (0b0 <<8 );
spi_flags = spi_flags | (0b0 <<9 );
spi_flags = spi_flags | (0b0001 <<10 );
spi_flags = spi_flags | (0b0 <<14 );
spi_flags = spi_flags | (0b0 <<15 );
spi_flags = spi_flags | (0b1000 <<16 );
spi_handle = spi_open(pigpio_check,spi_channel,baud,spi_flags);
if (spi_handle < 0 ){
ROS_ERROR("SPI Initialize Error" );
throw ;
}else {
ROS_INFO("SPI initialize OK" );
return spi_handle;
}
}
ここではspiの初期化をしますabyz.me.uk
今回は，第二引数は0にして，MISOがGPIO9，MOSIがGPIO10，SCLKがGPIO11，CEがGPIO8のSPIを使います
最も設定が多くて面倒だった第四引数の設定について説明します
os.mbed.com
2~4bitはCEがアクティブLowかアクティブHighかを設定します
5~7bitはCEをチップセレクト(スレーブセレクト)として予約するかどうか
8bitはメインSPIを使うか，auxiliary SPIを使うかです
9bitは何線でつないでいるかです
10~13bitはいまいち理解できていないので，調べた物を引用させていただきます
nnnnの4ビットはMOSIからMISOに切り替わるバイト数を指定する。当然最大15バイトということになる。これはAが1のとき、つまり3本線の場合だけ使える、と書いてある。ということはMOSIとMISOを多重化するということなんだろうか。
decafish.blog.ss-blog.jp
14bitは送信時のMSBかLSBを設定します
0だとMSBから1だとLSBで通信します
ただ，この設定はauxiliary SPIだけです
15bitは受信時のMSBかLSBを設定します
0だとMSBから1だとLSBで通信します
ただ，先ほどと同様この設定はauxiliary SPIだけです
16~22bitは何ビットで通信するか設定する場所です
MPU9250は1byte = 8bitずつ読むので0b1000=8で十分です6 = 64bit通信出来るので．マイコン と通信するときにはうれしいかもしれません
ここまでつらつら書いてきましたが，この人のブログが一番参考になると思います
decafish.blog.ss-blog.jp
spiのread関数
char SPI_MPU9250::pigpio_spi_read(char address){
char read_data;
char send_address = address | set_bit_1;
gpio_write(pigpio_check,CS_pin,0 );
spi_write(pigpio_check,spi_handle,&send_address,1 );
spi_read(pigpio_check,spi_handle,&read_data,1 );
gpio_write(pigpio_check,CS_pin,1 );
return read_data;
}
引数では読みだしたいアドレスを取得します
spiのwrite関数
char SPI_MPU9250::pigpio_spi_write(char address, char write_data){
char send_address = address;
gpio_write(pigpio_check,CS_pin,0 );
char result1 = spi_write(pigpio_check,spi_handle,&address,1 );
char result2 = spi_write(pigpio_check,spi_handle,&write_data,1 );
gpio_write(pigpio_check,CS_pin,1 );
if (result1 < 1 | result2 < 1 ){
ROS_ERROR("write error result = %d %d " ,(int )result1,(int )result2);
throw ;
}else {
ROS_INFO("MPU9250 OK" );
return 1 ;
}
}
引数で書きたいアドレスとデータを取得します
MPU9250のチェック
SPI通信でMPU9250が存在するかチェックします
void SPI_MPU9250::who_am_i_check(){
char read_data = pigpio_spi_read(who_am_i);
if (read_data == 0x71 ){
ROS_INFO("who am i OK" );
}else {
ROS_ERROR("who am i Error read data = %d " ,(int )read_data);
throw ;
}
}
ここでは先ほどのpigpio_spi_read関数を使ってMPU9250のwho am iが返ってくるかをチェックします
見つからなけばノードを落とします
MPU9250の起動
void SPI_MPU9250::mpu9250_power_on(){
pigpio_spi_write(wake_up_1,wake_up_ward_1);
pigpio_spi_write(wake_up_2,wake_up_ward_2);
}
MPU9250 の内部アドレス0x6Bに0x00 を書き込み，内部アドレス0x37に0x02 を書き込みます
加速度，角加速度の取得
int16_t SPI_MPU9250::get_acc_x(){
int acc_x_h = pigpio_spi_read(ACCEL_XOUT_H);
int acc_x_l = pigpio_spi_read(ACCEL_XOUT_L);
int16_t acc_x = ((acc_x_h << 8 ) | acc_x_l);
return acc_x;
}
この関数では加速度，角加速度を取得します
加速度の変換
double acc_x = mpu9250.get_acc_x()/16384.0 *9.81 ;
mpu9250から取得した加速度はそのままだと使えません2 に変換します
mpu9250のデフォルトのレンジは，+-2gです2 となります
つまり，出てきた値を16384で割って，9.8をかければ加速度に変換できます
角加速度の変換
double gyr_z = mpu9250.get_gyr_z()/130.072
これも同様に変換します
(ただ，これの単位は度なので，後でラジアン が必要になりました
double gyr_z = mpu9250.get_gyr_z()/130.072 *0.01745
rad = deg * (pi/180)なので，pi/180 = 0.01745を掛けました)
実験結果 VIDEO youtu.be
まとめ 解説を書くのが長かったIPhone のカメラだけでARを表示してさらにオクルージョンしてるのをみたり，Oculus Questがハンドトラッキング をカメラだけでしているのを見ると，この後の人生で自分が価値のあるものを作れるのかと思います
