2019/10/7(Raspberry PiのROSからpigpioを使ったSPI通信でMPU9250を動かす)

夏シンポが終わってひと段落

前回までで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 B
OS:Ubuntu Mate
ROS:Kinetic
9軸センサ:MPU9250

プログラム

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の初期化をしています
以前の記事とは異なるのは，初期化が失敗するとthrowをしてノードを落としていることです
また，クラス内変数にpigpio_checkを持たせておくことで他の場所でのpigpioの初期化の結果を使えるようにしています

pigpioのSPIの使い方

int SPI_MPU9250::spi_initialize(){
  //spi config
  unsigned int spi_channel = 0;          //0ch
  unsigned int baud = 500000;            //500kHz
  
  //spi_flags setting
  unsigned int spi_flags = 0;
  spi_flags = spi_flags | 0b00;            //select mm
  spi_flags = spi_flags | (0b000<<2);   //select p0-p3 
  spi_flags = spi_flags | (0b000<<5);   //select u0-u3
  spi_flags = spi_flags | (0b0<<8);     //select A
  spi_flags = spi_flags | (0b0<<9);     //select W
  spi_flags = spi_flags | (0b0001<<10); //select n
  spi_flags = spi_flags | (0b0<<14);    //select T
  spi_flags = spi_flags | (0b0<<15);    //select R
  spi_flags = spi_flags | (0b1000<<16); //select b
  //ROS_INFO("%d",spi_flags);

  //spi open
  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の初期化をします
pigpioのSPIの設定は以下のページにあります abyz.me.uk
第一引数には，pigpioの初期化の結果，第二引数にはどのSPIを使うか，第三引数はSPIの通信速度，第四引数には，SPIのその他の設定を入れます

今回は，第二引数は0にして，MISOがGPIO9，MOSIがGPIO10，SCLKがGPIO11，CEがGPIO8のSPIを使います
第三引数は500Khzにしました

最も設定が多くて面倒だった第四引数の設定について説明します
ここには22bit必要で自分はunsighd intを用意しました

0~1bitはSPIモードを設定します

os.mbed.com 立ち上がりでデータを読むかなどを設定するところですが，基本的に00を書き込んでおけばいいと思います

2~4bitはCEがアクティブLowかアクティブHighかを設定します
ただ，今回はCEピンが動いていない気がしたので，スレーブセレクトは自分でしました
5~7bitはCEをチップセレクト(スレーブセレクト)として予約するかどうか
予約していますが，自分で動かしているので一つ前の設定も含めてここは少し謎です
8bitはメインSPIを使うか，auxiliary SPIを使うかです
auxiliary SPIはメインSPIのピンが何かしらの理由で埋まっていれば使えばいいと思いますが，特殊なSPIな気がします
9bitは何線でつないでいるかです
SPIはチップセレクト(スレーブセレクト)を無視した3線と，チップセレクト(スレーブセレクト)を含んだ4線で通信します
今回は4線で設定しています
この設定はメインSPIでしか行えませんので，auxiliary SPIではただ，ここも4線で設定していますが結局チップセレクト(スレーブセレクト)は自分で動かしているので設定として間違っているかもしれません
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で十分です
ただ，最高2⁶ = 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;
}

引数では読みだしたいアドレスを取得します
MPU9250でSPI通信を使ったリードを行うときにはアドレスの先頭bitを1にします(ここ大事)
そして，チップセレククトピンを下げて(アクティブLow)，読みたいアドレスを書き込み，読み出します
最後にチップセレクトピンを上げて終了します
その結果を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;
  }
}

引数で書きたいアドレスとデータを取得します
readとは異なりアドレスには何も手を付けません最初に，チップセレククトピンを下げてアドレスを送信して，その後データを送信します
最後にチップセレククトピンを上げます
書き込み関数の結果は書き込んだバイト数が返ってきます
つまり1byteの書き込みに成功すれば1が返ってくるはずなので，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 を書き込みます
ここら辺はMPU9250のマニュアル通りです

加速度，角加速度の取得

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;
}

この関数では加速度，角加速度を取得します
リードしたデータはintで取得して，必ずint16t_tに格納しますもし，intに入れると4byte = 32bitなので，シフト演算でまとめた時に負の値がおかしくなります

加速度の変換

double acc_x = mpu9250.get_acc_x()/16384.0*9.81;

mpu9250から取得した加速度はそのままだと使えません
なのでこれをm/s²に変換します mpu9250のデフォルトのレンジは，+-2gです
また，分解能は16bit，65536段階，つまり，-32768~32768です
(いや，0を考えると-32767~32768かな？)
例えば，16384という値が来れば 16384/16384 = 1g * 9.8 = 9.8m/s²となります

つまり，出てきた値を16384で割って，9.8をかければ加速度に変換できます
(今回はデフォルトのレンジが+-2gなのでこの計算で出来ます
もし，+-4gなら，1gの値が32768/4=8192なので，8192で割って，9.8をかけたら加速度になります)

角加速度の変換

double gyr_z = mpu9250.get_gyr_z()/130.072

これも同様に変換します
デフォルトが+-250dps = degree/secなので，32768/250 = 131.072の値で割ります
こうすることで，角加速度が出てきます

(ただ，これの単位は度なので，後でラジアンが必要になりました
その場合以下のようにしました

double gyr_z = mpu9250.get_gyr_z()/130.072*0.01745

rad = deg * (pi/180)なので，pi/180 = 0.01745を掛けました)

Raspberry PiとMPU9250の配線方法

f:id:masanari7430:20191007150636j:plain 図のICの名前は違いますが，この方法で結線しました

実験結果

youtu.be accを見ると9.8が出ていて，重力加速度が認識出来ています

まとめ

解説を書くのが長かった
最近，研究の方でも解説を書いているけど，これを書かないと1年後の自分が絶対覚えていないので仕方がないと思いながら書いています
でも，やっていて思ったのがこんなことしていてもいいのかなということです
最近，IPhoneのカメラだけでARを表示してさらにオクルージョンしてるのをみたり，Oculus Questがハンドトラッキングをカメラだけでしているのを見ると，この後の人生で自分が価値のあるものを作れるのかと思います
来年から働きますがこれでいいのかなとりあえずご飯食べてきます

へっぽこ元ロボコニスト

ロボコンに燃え尽きた自分が日々の出来事を書くだけのブログです twitter:@mozyanari_tech