Raspberry Pi4 SPIでセンサ(L3GD20)から取得した値をROSでPublishする

2020年12月12日

この記事は、Micro Mouse Advent Calendar 2020の12日目の記事です。
昨日の記事は、kerikun11さんの「ESP32でマイクロマウスを作るメリットとデメリット」でした。
マイクロマウスに使用されているマイコンは、STマイクロエレクトロニクス株式会社のSTM32やルネサスエレクトロニクス株式会社のRXなどが主流ですが、マイクロマウスでは少数派のESP32を使っている理由や、気になっていたメリット・デメリットについて言及があり大変興味深い内容でした。

はじめに

今回の記事では、Raspberry Pi4に搭載されているSPIでセンサから値を取得してROSにPublishするということをしていきたいと思います。OSは、Ubuntu18.04LTS、ROS(melodic)、使用言語はC/C++です。

準備編

Raspberry Pi4とL3GD20を配線をしていきます。L3GD20は秋月電子の L3GD20H使用３軸ジャイロセンサーモジュールキットを使用しました。
配線は以下の表の通り行いました。

機能名	Raspberry Pi4(ピン番号)	L3GD20モジュール(ピン番号)
VDD(3.3V)	+3.3V(1,17)	+V(1)
SCLK	GPIO11/SPI0_CLK(23)	SCL(2)
MOSI	GPIO10/SPI0_MOSI(19)	SDA(3)
MISO	GPIO9/SPI0_MISO(21)	SAO(4)
CS	GPIO8/SPI0_CE0_N(24)	CS(5)
GND	Ground(6,9,14,25, 30, 34, 39)	GND(8)

続いて、L3GD20のデータシートを読んで、SPIの通信モードと設定に必要なレジスタの情報などを確認しました。

実装編

プログラムを書いていきたいと思います。
初めに、Raspberry Pi4でSPIデバイスを認識しているのかどうかの確認を行います。

ls -l /dev/spidev0.*

デバイスが認識されているのかどうかということと、実行権限がどのようになっているのかのチェックを同時に行いました。権限が通常ユーザーにない場合は、chmodなどで権限を与えてあげればOKです。公式サイトからインストールしたUbuntuでは有効になっていました。有効になっていない場合は、/boot/config~を編集して有効にしてあげればOKです。ROSのインストールについては、公式サイトの通りインストールを行えば大丈夫でした。

今回、C/C++でSPIの機能を使うにあたって、デバイスをオープンしてspidevにアクセスをしていくという方法を用いることにしました。設定は、関数ioctlを用いて行いました。(Raspberry PiのSPI周りについて、私が参考にしたサイトを参考文献に載せたので、興味のある方は参照していただければと思います。)
実装したプログラムを以下に示します。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>

geometry_msgs::Twist cmd_gyro;
int fd;
uint8_t bits = 8;
uint32_t speed = 1000000;
void init(void);
float getXAxis(void);
float getYAxis(void);
float getZAxis(void);

int main(int argc, char** argv) {
  ros::init(argc, argv, "gyro_publisher");
  ros::NodeHandle nh;

  fd = open("/dev/spidev0.0", O_RDWR | O_SYNC);

  if(fd < 0){
    ROS_INFO("error");
  }

  // set spi mode
  // write mode
  ioctl(fd, SPI_IOC_WR_MODE, SPI_MODE_3);
  // read mode
  ioctl(fd, SPI_IOC_RD_MODE, SPI_MODE_3);

  // set communication bits
  ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
  ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);

  // set communication speed
  ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
  ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);

  init();

  // publish
  ros::Publisher gyro_cmd_pub = nh.advertise("gyro_vel", 1000);

  ros::Rate loop_rate(100);
  while (ros::ok()) {
    cmd_gyro.angular.x = getXAxis();
    cmd_gyro.angular.y = getYAxis();
    cmd_gyro.angular.z = getZAxis();
    gyro_cmd_pub.publish(cmd_gyro);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

void communication(uint8_t *tx, uint8_t *rx, uint8_t length)
{
  struct spi_ioc_transfer tr = {
    .tx_buf = (unsigned long)tx,
    .rx_buf = (unsigned long)rx,
    .len = length,
    .speed_hz = speed,
    .delay_usecs = 0,
    .bits_per_word = bits,
  };
  ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
}

void writeByte(uint8_t reg, uint8_t data)
{
  uint8_t tx_buff[2];
  uint8_t rx_buff[2];

  tx_buff[0] = reg & 0x7F;
  tx_buff[1] = data;

  communication(tx_buff, rx_buff, 2);
}

uint8_t readByte(uint8_t reg)
{
  uint8_t tx_buff[2];
  uint8_t rx_buff[2];

  tx_buff[0] = reg |0x80;
  tx_buff[1] = 0x00;

  communication(tx_buff, rx_buff, 2);
  return rx_buff[1];
}

void init(void)
{
  // power on
  usleep(20000);
  writeByte(0x20, 0x0f);

  // setup 2000deg/sec
  usleep(20000);
  writeByte(0x23, 0x20);
}

float getXAxis(void){
  int16_t data = (int16_t)( (readByte(0x29) << 8)| readByte(0x28));
  float ret = data * 0.070f * 3.1415926535f / 180.0f;
  return ret;
}

float getYAxis(void){
  int16_t data = (int16_t)( (readByte(0x2B) << 8)| readByte(0x2A));
  float ret = data * 0.070f * 3.1415926535f / 180.0f;
  return ret;
}

float getZAxis(void){
  int16_t data = (int16_t)( (readByte(0x2D) << 8)| readByte(0x2C));
  float ret = data * 0.070f * 3.1415926535f / 180.0f;
  return ret;
}

spidev0.0のデバイスをオープンして、SPIの初期設定をした後、L3GD20の初期設定をして、データを取得してPublishするということをしています。今回は、ROSのデータ構造体の一つであるTwistにデータを入れてトピックとしてあげました。rostopic echoで確認するときに見やすいため採用をしたという形になります。

さいごに

Raspberry Pi4には、OSが搭載されながらも、SPIやI2Cを用いてセンサ値の取得などもできるため勉強にはもってこいのデバイスだなぁとしみじみ思いました。ただし、c/c++を用いてSPIなどの機能を使用したいと思ったときに、調べたり、エラー対処をするのが少し大変かなという実感を持ちました。

参考文献

Raspberry Pi公式サイト　SPI