M5でCANするIMU

# はじめに

M5Stack Grayに内蔵されたIMUのデータを、MCP2515を使ったCAN BUSモジュールを使って送受信してみました。
CAN（Controller Area Network）は自動車などで利用されている、耐ノイズ性の高い通信規格です。最大1Mbps、CAN BUSモジュールからの配線はCANH/CANLの2本で済むので、実はホビーにも使いやすいものなんじゃないかと思っています。
今回はIMUということで比較的情報量の多いデータをCAN上に流し、送受信を試してみました。
※メッセージのIDやその中身は、自動車で使われるCANに準拠したものではありません！誤動作や故障の危険があるため、自動車その他機器にそのまま繋いで使うことはおやめください。


# 接続構成
## 全体構成
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/997dba5f-2414-0048-88b5-368dff0e958a.jpeg" width=100%>

- M5Stack Gray(IMU送信側)
    - SPI
- CAN BUSモジュール１
- CAN BUSモジュール２
    - SPI
- Arduino Mega
    - USB Serial
- PC(Processingで可視化)

という感じで接続します。

CAN BUSモジュールには、Amazonでよく見かける[こちらのもの](https://www.amazon.co.jp/gp/product/B01GF75R0O/ref=ppx_yo_dt_b_search_asin_title?ie=UTF8&psc=1)を2つ使いました。
CANコントローラにMCP2515、CANトランシーバにTJA1050を使っており、マイコンとはSPIで接続できる代物です。CANの120Ω終端抵抗もジャンパで有効化できます。
後述しますが、購入したものには8MHzの水晶発振器がついていました。ここの設定さえ間違えなければ、他のCAN BUSモジュールとの通信もできます。


## M5Stack(送信側)とCAN BUSモジュール

下記のように、SPI接続をします。

| M5Stack | CAN BUSモジュール1 |
| ------- | ------------- |
| 接続せず    | INT           |
| 18(SCK) | SCK           |
| 23(MO)  | SI            |
| 19(MI)  | SO            |
| 5       | CS            |
| G       | GND           |
| 3V3     | VCC           |

M5StackのSPIは3.3V系なので、CAN BUSモジュールにも3.3Vを供給します。また、今回M5Stack側からは送信のみなのでINTには何も接続しません。ちなみにCSは普通のデジタル出力ができるピンであればよいのですが、内蔵のLCDやIMUを生かし、スピーカーからけたたましいノイズを発生せずに使えるピンの選択肢となると限られてきます。
余談ですが、接続用のケーブルには接触不良のないものを使いましょう(私はここで小一時間消費しました。。)。


## Arduino Mega(受信側)とCAN BUSモジュール

下記のようにSPI接続をします。

| Arduino Mega | CAN BUSモジュール2 |
| ------------ | ------------- |
| 10           | INT           |
| 52           | SCK           |
| 51           | SI            |
| 50           | SO            |
| 9            | CS            |
| GND          | GND           |
| 5V           | VCC           |

たまたま手元にArduino Megaがあったので使ってみましたが、Unoなどでも問題ないと思います(SPIの接続は変わりますが)。送信側のM5StackではINT接続をしませんでしたが、受信側では割り込み対応のピンと接続します。これは、マイコン側で割り込み受信を可能にするためです。



# プログラム

CAN BUSモジュール制御用のプログラムには[Seeed-StudioのCAN_BUS_Shieldライブラリ](https://github.com/Seeed-Studio/CAN_BUS_Shield)を、M5Stack側のIMU制御プログラムには[M5StackのMPU9250サンプル](https://github.com/m5stack/M5Stack/blob/master/examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino)を、そしてProcessingには[こちらのプログラム](https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde)をそのまま利用しました。


## M5Stack側プログラム

M5Stackのサンプルプログラム(MPU9250BasicAHRS.ino)から改変したところを抜粋します。
このプログラムでは、IMU(MPU9250)から取得したQuaternionの4要素(float)をそれぞれ別のidを付けて、タイムスタンプとともにCANへ送ります。このやり方が正しいかは不明ですが、とりあえずCANへの高速データ送信のテストとして作ってみました。


    $ diff MPU9250BasicAHRS_CAN/MPU9250BasicAHRS_CAN.ino MPU9250BasicAHRS/MPU9250BasicAHRS.ino 
    24d23
    < #define CAN_OUT
    28,53d26
    < /*CAN settings*/
    < #include <mcp_can.h>
    < const int SPI_CS_PIN = 5;
    < MCP_CAN CAN(SPI_CS_PIN);
    < unsigned char stmp[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    < 

mcp_can.hを読み込みます。CAN_BUS_Shieldライブラリのインストールを忘れずに。
SPIデバイスセレクトのためのデジタルピンをSPI_CS_PINで設定します。M5Stackの場合LCDやスピーカーなど内蔵のデバイスと共有しているピンが多いので、チョイスを誤るとLCDがつかなくなったりスピーカーから凄い音が出たりとおかしなことになります。
stmp[8]のところでCANに送るためのメッセージバッファを用意しています。
※CANの仕様では、id11ビット、データ64ビット(8バイト)の構造となります。


    < void sendCanMsgFloat(unsigned long id, unsigned long timestamp, float val){
    <   union {
    <     float f;
    <     unsigned char b[4];
    <   } ftob;
    <   ftob.f = val;
    <   union {
    <     unsigned long l;
    <     unsigned char b[4];
    <   } ultob;
    <   ultob.l = timestamp;
    <   
    <   stmp[0] = ultob.b[3];
    <   stmp[1] = ultob.b[2];
    <   stmp[2] = ultob.b[1];
    <   stmp[3] = ultob.b[0];    
    <   stmp[4] = ftob.b[3];
    <   stmp[5] = ftob.b[2];
    <   stmp[6] = ftob.b[1];
    <   stmp[7] = ftob.b[0];
    55,57d27
    <   CAN.sendMsgBuf(id, 0, 8, stmp);
    <   delay(2);
    < }

実際にCANへデータを送る関数です。構造体を使って、floatとunsigned long(タイムスタンプを想定)をバイト形式に変換しています。Arduino上ではどちらも32ビットなので、CANへ送るデータは合計64ビットとなります。
なお、バイトオーダーの観点でArduinoの場合はリトルエンディアン、CAN BUSはビッグエンディアン(ネットワークバイトオーダー)ということで順序を入れ替えてみました。


    62,70d31
    <   
    < #ifdef CAN_OUT
    <   while (CAN_OK != CAN.begin(CAN_500KBPS, MCP_8MHz)) {// init can bus : baudrate = 500k
    <     Serial.println("CAN BUS Shield init fail");
    <     Serial.println(" Init CAN BUS Shield again");
    <     delay(100);
    <   }
    <   Serial.println("CAN BUS Shield init ok!");
    < #endif //CAN OUTPUT

CANデバイスの初期化を行っています。MCP_8MHzの部分は接続するCANモジュールの動作周波数に合わせて設定します。CAN_BUS_Shieldライブラリ上はMCP_16MHzもチョイスできるようです。この部分の設定を誤ると、他のCANデバイスとの通信が正常にできなくなります。(8MHzの場合1MbpsのCAN通信は厳しいと思います。)
CAN_500KBPS部分で、CAN BUSの通信速度を定義しています。


    428,429c389
    <       M5.Lcd.setCursor(0,  120);
    <       M5.Lcd.printf("  q0:% 5.2f qx:% 5.2f qy:% 5.2f qz:% 5.2f \r\n",(*getQ()), (*(getQ() + 1)), (*(getQ() + 2)), (*(getQ() + 3)));//print quaternion
    ---
    > 

M5StackのLCDにQuaternionを表示させるようにしてみました。


    458,464d417
    < #ifdef CAN_OUT
    <   sendCanMsgFloat(0x01, IMU.count, *getQ());
    <   sendCanMsgFloat(0x02, IMU.count, *(getQ() + 1));
    <   sendCanMsgFloat(0x03, IMU.count, *(getQ() + 2));
    <   sendCanMsgFloat(0x04, IMU.count, *(getQ() + 3));
    < #endif
    < 

先述した関数を使って、Quaternionの各要素を別idでタイムスタンプとともにCANへ送信します。

一応、opendiffでGUI表示したものも貼り付けておきます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/897cd12f-259f-6a39-25cf-655a89cdb029.png" width=100%>
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/3d0faea0-efa2-dd66-1d2e-9697d7039a11.png" width=100%>


## Arduino Mega側プログラム

Arduino Mega側では、CAN BUS上から取得したM5StackのIMUデータをデコードし、Serial通信でProcessing上のビューワへ中継します。CANからの受信部分はCAN_BUS_Shieldライブラリのサンプルプログラムreceive_check.inoをそのまま使っています。


    $ diff receive_check_processing/receive_check_processing.ino receive_check/receive_check.ino 
    21,42d20
    < /*for processing*/
    < uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
    < int qw, qx, qy, qz;
    < float q0, q1, q2, q3;
    < 
    < void printProcessing(){
    <   qw =int(q0 * 16384.0f);
    <   qx =int(q1 * 16384.0f);
    <   qy =int(q2 * 16384.0f);
    <   qz =int(q3 * 16384.0f);
    <     
    <   teapotPacket[2] = highByte(qw);
    <   teapotPacket[3] = lowByte(qw);
    <   teapotPacket[4] = highByte(qx);
    <   teapotPacket[5] = lowByte(qx);
    <   teapotPacket[6] = highByte(qy);
    <   teapotPacket[7] = lowByte(qy);
    <   teapotPacket[8] = highByte(qz);
    <   teapotPacket[9] = lowByte(qz);
    <   SERIAL.write(teapotPacket, 14);
    <   teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
    < }

デコードしたIMUのQuaternion情報をProcessing側のプログラムで利用できる形式に変換する関数です。[こちらの記事](https://homemadegarbage.com/motion-senser02)にあるものを利用させていただきました。



    65,101c43,49
    <         //SERIAL.println("-----------------------------");
    <         //SERIAL.print("Get data from ID: 0x");
    <         //SERIAL.println(canId, HEX);
    < 
    <         //for (int i = 0; i < len; i++) { // print the data
    <         //    SERIAL.print(buf[i], HEX);
    <         //    SERIAL.print("\t");
    <         //}
    <         union {
    <           float f;
    <           unsigned char b[4];
    <         } ftob;
    <         if(canId == 1){
    <           ftob.b[0] = buf[7];
    <           ftob.b[1] = buf[6];
    <           ftob.b[2] = buf[5];
    <           ftob.b[3] = buf[4];
    <           q0 = ftob.f;
    <         }else if(canId == 2){
    <           ftob.b[0] = buf[7];
    <           ftob.b[1] = buf[6];
    <           ftob.b[2] = buf[5];
    <           ftob.b[3] = buf[4];
    <           q1 = ftob.f;
    <         }else if(canId == 3){
    <           ftob.b[0] = buf[7];
    <           ftob.b[1] = buf[6];
    <           ftob.b[2] = buf[5];
    <           ftob.b[3] = buf[4];
    <           q2 = ftob.f;
    <         }else if(canId == 4){
    <           ftob.b[0] = buf[7];
    <           ftob.b[1] = buf[6];
    <           ftob.b[2] = buf[5];
    <           ftob.b[3] = buf[4];
    <           q3 = ftob.f;
    <           printProcessing();
    ---
    >         SERIAL.println("-----------------------------");
    >         SERIAL.print("Get data from ID: 0x");
    >         SERIAL.println(canId, HEX);
    > 
    >         for (int i = 0; i < len; i++) { // print the data
    >             SERIAL.print(buf[i], HEX);
    >             SERIAL.print("\t");
    103c51
    <         //SERIAL.println();
    ---
    >         SERIAL.println();

CANデバイスから受け取ったデータをデコードし、Quaternionの4要素にしています。とっても雑ですが。

ここでも一応、opendiffでGUI表示したものも貼り付けておきます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/ba5e9030-9d20-2c24-4a9d-d54bd6d7625e.png" width=100%>
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/f3859e12-d84c-4161-1f72-103a7a837f1c.png" width=100%>


## PC側プログラム

[MPUTeapot.pde](https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde)をそのままProcessingで動かします。Serialポートの指定は良しなに。


# 動作例

M5Stackの物理的な動きと、Processing上で表示された飛行機が連動して動いていることがわかります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/333764/3bf86038-f776-1da4-c87c-8f6d55c41f73.gif" width=100%>

# 終わりに

いかがでしたでしょうか。ホビーでCANを使ってみたい方の参考になれば幸いです。機会があれば、そのほかのデータをCANで送ったり、同時送受信してみた内容も記載してみたいと思います。