Arduinoで方位角と角速度と加速度を記録する

はじめに

　筆者は簡単なCanSatを勉強する機会があって、Arduinoを使ってCanSatの動きを記録したり観測しました。せっかくArduinoに触ったのでまとめてみたく、復習がてら参考になることを目標に記事を書いてみようと思います。センサーの値の読み取りと記録が主題なのでCanSatだけではなくロボットや定点観測などにも通用すると思います。
　皆さんの参考になればうれしいです。

対象となる読者

ArudinoでI2C接続のセンサーに興味がある人。

完成形

以下のような感じになりました。
完成版スケッチ in GitHub

環境

• Arduino UNO Rev3
• 磁気センサー　GY-271 QMC5883L
• 角速度、加速度センサー　GY-521 MPU-6000
• Micro SD Card Readerはよくわからなかった。ゴメンナサイ。SPI接続の完成形の写真みたいなもの。
• Micro SD カード
• Arduino IDE 1.8.10

開発の流れ

　下記のような流れで作りました。

1.　仕様を決める。どんなデータをとってどこへ記録するかを決める。
2.　使うセンサー、部品を決めて買う。
3.　センサーや部品のマニュアルやレジスターマップを読み込む。←英語力必要
4.　データシートやレジスターマップを基にプログラムを組む。
5.　テストと修正または改良の繰り返し
6.　完成

仕様

• リアルタイムで磁気センサー、角速度センサー、加速度センサーからを読み取り、方位角$[°]$、角速度$[°/s]$、加速度$[m/s^2]$に変換する。
• 各センサーから読み取り変換した値をCSVファイルに記録する。
• センサーの値を読み取り、記録するタイミングはプッシュボタンスイッチで制御
• プログラムのステータスを赤LEDで表示。ロギング中はLED点灯、エラーは点滅。
• 使用する部品は環境項を参照

I2Cについて

　$I^2C$とは周辺機器とシリアル通信するインターフェースのことです。I2CはArduinoに搭載されており、いろいろなセンサーも対応しています。I2Cはその仕様上ふたつの信号線と電源のVCCとGNDで通信でき、同一の信号線上に複数の機器をつなぐことができます。このことから、低コストで簡単に機器を扱うことができます。
　今回はArduinoにI2Cを取り扱うAPIがあるのでそれを使い、センサーを制御します。

I2Cを使ってみる

Arduinoとセンサーをつなぐ

　基本的にArduinoとセンサーのSDA,SCL,VCC,GND同士をつなげばいいです。ここで問題となるのはプルアップ抵抗の必要性と電圧です。I2C通信にはプルアップ抵抗が必要です。また、用いるセンサーに適正電圧があるのでそれも確認してください。しかし、ArduinoはWireを使うときに強制的に内部のプルアップ抵抗を使うようなのでいらないみたいです。また、電圧はセンサーの基盤の上に電圧レギュレータが搭載していて3.3-5Vまで大丈夫そうなのでそのままつなぎました。
　詳しくは完成形項の図を参照
　

I2C通信の流れ

　I2Cにはマスターとスレーブがあり、スレーブにはスレーブアドレスというものが割り振られています。この両者はマスターはスレーブを制御し、スレーブはマスターから制御されるという関係です。マスターがArduinoにあたり、センサーがスレーブになります。スレーブアドレスはマスターからスレーブを区別するためにあり、センサーのデータシートなどに必ず記載されているはずです。
　I2C通信はセンサーのスレーブアドレスを指定し、レジスターと呼ばれる記憶装置に読み書きして制御します。つまり、値を得たいセンサーのスレーブアドレスを指定してからセンサーの値が入っているレジスターを絶対アドレス指定し、読み込むと値が得られます。逆に、スレーブアドレスを指定してからセンサーの挙動を制御するレジスターを絶対アドレス指定し、書き込むとセンサーが書き込まれた値をもとに動きます。
　今回はI2CをWireと呼ばれるArduinoが提供するAPIを利用します。Wireを利用するためには#include <Wire.h>をコードに追加してください。すると、Wireと呼ばれる変数が既に宣言されているのでWire.something();といった感じで利用します。
参考リンク：Wire Library

Wireの初期化

1.　Wireを利用するためにはWire.begin();を呼び出します。
2.　SCLのクロック周波数を設定します。Wire.setClock(clock_in_Hz);値は扱うセンサーのデータシートを参照してください。（今回はclock_in_Hz=400kHz）

initialize_Wire.cpp

Wire.begin();               // 1. `Wire`を利用するためには`Wire.begin();`を呼び出します。
Wire.setClock(clock_in_Hz); // 2. SCLのクロック周波数を設定します。`Wire.setClock(clock_in_Hz);`値は扱うセンサーのデータシートを参照してください。（今回はclock_in_Hz=400kHz）

値を書き込む

1.　通信の開始、スレーブアドレスを指定。
2.　書き込むレジスターの始点となるアドレスを書き込む。
3.　データを書き込む。
4.　通信の終了。

write_in_a_i2c_device.cpp

Wire.beginTransmission(slaveAdress);  // 1. 通信の開始、スレーブアドレスを指定。
Wire.write(registerAdress);           // 2. 書き込むレジスターの始点となるアドレスを書き込む。
Wire.write(data, size);               // 3. データを書き込む。データを一つだけ書き込むときはWire.write(data, 1);またはWire.write(data);
Wire.endTransmission();               // 4. 通信の終了。

値を読み込む

1.　通信の開始、スレーブアドレスを指定。
2.　読み出すレジスターの始点となるアドレスを書き込む。
3.　バスをアクティブにしたまま一旦通信の終了。
4.　データを指定したサイズの分だけ要求。
5.　要求したデータがそろうまで待つ。
6.　データを読み込む。
7.　通信の終了。

read_from_a_i2c_device.cpp

Wire.beginTransmission(slaveAdress);       // 1. 通信の開始、スレーブアドレスを指定。
Wire.write(registerAdress);                // 2. 読み出すレジスターの始点となるアドレスを書き込む。
Wire.endTransmission(false);               // 3. バスをアクティブにしたまま一旦通信の終了。
Wire.requestFrom(slaveAdress, size);       // 4. データを指定したサイズの分だけ要求。データを一つだけ読み込むときはsize=1
while ((size_t)Wire.available() < size);   // 5. 要求したデータがそろうまで待つ。
for (size_t i = 0; i < size; i++)          // 6. データを読み込む。
{
  output[i] = Wire.read();
}
Wire.endTransmission();                    // 7. 通信の終了。

I2C通信するコード

　上記を踏まえてクラスにまとめてみました。
　このクラスのvoid initialize(uint32_t clockHz);を最初に呼び出し、他のメソッドを使います。void registerDump(uint8_t slaveAdress, size_t size);はセンサーのレジスターをアドレス0からsizeの分だけSerialに出力します。デバッギングに使ってください。

I2C.h

#ifndef _I2C_H
#define _I2C_H

#include <Arduino.h>
#include <Wire.h>

class I2C
{
  private:
    uint32_t _clock;
    boolean _initialized;

  public:
    I2C();
    // it calls initialized() inside
    I2C(uint32_t clockHz);

    //
    // must call it first
    void initialize(uint32_t clockHz);

    uint32_t getClockHz() { return _clock; }
    boolean isInitialized() { return _initialized; }
    boolean isSlaveConnected(uint8_t slaveAdress)
    {
      Wire.beginTransmission(slaveAdress);
      return !Wire.endTransmission();
    }

    //
    // writer
    size_t writeArray(uint8_t slaveAdress,  uint8_t registerAdress, uint8_t *data, size_t size);
    size_t write1Byte(uint8_t slaveAdress, uint8_t registerAdress, uint8_t data)
    {
      return writeArray(slaveAdress, registerAdress, &data, 1);
    }

    //
    // reader
    void readArray(uint8_t slaveAdress,  uint8_t registerAdress, size_t size, uint8_t *output);
    uint8_t read1Byte(uint8_t slaveAdress,  uint8_t registerAdress)
    {
      uint8_t result;
      readArray(slaveAdress, registerAdress, 1, &result);
      return result;
    }

    //
    // for debugging
    void registerDump(uint8_t slaveAdress, size_t size);
};

#endif

I2C.cpp

#include "I2C.h"

I2C::I2C()
{
  _clock = 0;
  _initialized = false;
}

I2C::I2C(uint32_t clockHz)
{
  initialize(clockHz);
}

void I2C::initialize(uint32_t clockHz)
{
  _clock = clockHz;
  Wire.begin();
  Wire.setClock(_clock); _initialized = true;
}

size_t I2C::writeArray(uint8_t slaveAdress,  uint8_t registerAdress, uint8_t *data, size_t size)
{
  size_t result = 0;
  Wire.beginTransmission(slaveAdress);
  Wire.write(registerAdress);
  result = Wire.write(data, size);
  Wire.endTransmission();
  return result;
}

void I2C::readArray(uint8_t slaveAdress,  uint8_t registerAdress, size_t size, uint8_t *output)
{
  Wire.beginTransmission(slaveAdress);
  Wire.write(registerAdress);
  Wire.endTransmission(false);
  Wire.requestFrom(slaveAdress, size);
  while ((size_t)Wire.available() < size);
  for (size_t i = 0; i < size; i++)
  {
    output[i] = Wire.read();
  }
  Wire.endTransmission(true);
}

void I2C::registerDump(uint8_t slaveAdress, size_t size)
{
  uint8_t *data;
  data = new uint8_t[size];
  readArray(slaveAdress, 0, size, data);
  Serial.print("register dump\n");
  for (size_t i = 0; i < size; i++)
  {
    Serial.print("0x");
    Serial.print(i, HEX);
    Serial.print(" 0x");
    Serial.println(data[i], HEX);
  }
  delete data;
}

サンプルプログラム

　上記のクラスを用いてArduinoにI2Cで接続されたスレーブを探し出し、そのスレーブについてレジスターを16バイト分だけSerialに出力します。
I2C Scannerを参考にしました。

main.ino

#include "I2C.h"

I2C i2c;

void setup() {
  i2c.initialize(400000);
  Serial.begin(9600);
}

void loop() {
  boolean wasSlaveFound = false;
  for(uint8_t address = 1; address < 127; address++)
  {
    if (i2c.isSlaveConnected(address))
    {
      Serial.print("Slave was found at 0x");
      Serial.println(address, HEX);
      i2c.registerDump(address, 16);
      wasSlaveFound = true;
    }
  }
  if (!wasSlaveFound)
  {
    Serial.println("Slave was not found.");
  }
  delay(10000);
}

磁気センサーQMC5883Lをつなげた場合。
スレーブアドレスが0xDとわかった。

Result

Slave was found at 0xD
register dump
0x0 0xFF
0x1 0xFB
0x2 0x8C
0x3 0xF1
0x4 0x4F
0x5 0xF2
0x6 0x0
0x7 0x16
0x8 0xFD
0x9 0xD
0xA 0x0
0xB 0x1
0xC 0x1
0xD 0x0
0xE 0x0
0xF 0x0

磁気センサーを使ってみる

　センサーを完成形の図の通りかArduinoとセンサーのSDA,SCL,VCC,GND同士をつなげてください。

データシートを読み解く

　今回使う磁気センサーはGY-271 QMC5883Lを使います。データシートは以下のリンクにありました。
GY-271 QMC5883L Data Sheets
　ひとつ注意事項としては類似品のHMC5883Lに気を付けてください。QMC5883LはHMC5883Lほとんど一緒ですがスレーブアドレスが違うのでうまくいきません。区別が付かない場合、第一回にあるようなスレーブをスキャンするプログラムでスキャンしてみてください。スレーブアドレスが0xDだったらQMC5883Lです。
　上記のリンクではAPIも配布されていますが、センサーを直接扱ってみたいので自作APIを作ってみたいと思います。

	値
SCL クロック周波数	最大400kHz
スレーブアドレス	0xD

以下　レジスターマップ

GY-271 QMC5883L Data Sheetsより引用
見てわかると思いますがアドレス0x00-0x05までのレジスターが目的のデータと分かります。つまり、アドレス0x00-0x05を指定して読みだしてビット演算すればセンサーの生のデータが得られます。このデータをデータシートに記載されているセンサー特性（感度など）を基に変換補正すれば目的のデータが得ることができます。

値を読んでみよう

　早速、磁気を測定したいですが正しく動いているのかわかりにくいのでセンサーに搭載されている温度センサーを最初に読みたいと思います。I2C通信するコードは前述したようなコードなので、クラスにまとめたI2CクラスI2C.hを利用します。
　また、定数QMC5883_ADDRESS,REG_~はスレーブアドレスとレジスターアドレスを示します。

温度の読み取り

　データシートによると

アドレス	値
07	TOUT [7:0]
08	TOUT [15:8]

とあります。これの意味するところはセンサーが出力する温度の値は2Byteの整数で、アドレス07 TOUT [7:0]は上位バイト
、アドレス08 TOUT [15:8]は下位バイトということです。また、MSB(Most Significant Byte)とLSB(Least Significant Byte)はそれぞれ最上位バイト、最下位バイトということを示します。
　これを踏まえたコード示します。

read_temperature

float getTemperatureRaw()
{
  uint8_t data[2];
  _i2c->readArray(QMC5883_ADDRESS, REG_TEMPERATURE_L, 2, data); 
  return data[0] | data[1] << 8;
}

　得られた値はとんでもない数値を示していると思いますが、センサーを指などで温めてみると値はだんだん大きくなっていると思います。この温度から、セルシウス温度に変換しますがセンサーひとつひとつ違うので、温度計で測りながら調整してみてください。

センサーのセットアップ

　たぶんこのセンサーは設定をしなくても動くと思いますが、正確な値を欲しい時やセンサーの挙動が分からないと不便なことが多いことでしょうから設定することをお勧めします。
　データシートよりアドレス09,0Bのレジスターを利用します。また、今回はセンサーが定期的に測定するような設定にしてあります。
　void configure(...)はセンサーの感度などを測定して後で利用するために変数_sensitivityとして保存しています。
　Errorはエラーコードとエラーメッセージを保存するクラスです。

setup

Error initialize(I2C *i2c,
                 MagneticSensorDataRate rate,　// 1秒間あたりどのくらい測定するか
                 MagneticSensorSample sample,　// サンプリング
                 MagneticSensorRange range)    // 測定範囲
{
  if (!i2c)
  {
    return Error(FATAL_ERROR, F("FATAL_ERROR : I2C pointer is NULL!\n"));
  }
  if (!i2c->isInitialized())
  {
    return Error(FATAL_ERROR, F("FATAL_ERROR : I2C was not initialized!\n"));
  }
  _i2c = i2c;

  // begin wiring to magnetic sensor
  if (!_i2c->isSlaveConnected(QMC5883_ADDRESS))
  {
    return Error(FATAL_ERROR, F("FATAL_ERROR : QMC5883 is not available!\n"));;
  }
  if (_i2c->read1Byte(QMC5883_ADDRESS, QMC5883_ADDRESS) != 0b11111111)
  {
    return Error(FATAL_ERROR, F("FATAL_ERROR : QMC58837's register is not available!\n "));
  }

  // configure sensor
  configure(rate, sample, range);

  return Error(SUCCEEDED, F("SUCCEEDED : QMC5883 was initialized correctly.\n"));;
}

void configure(MagneticSensorDataRate rate,
                    MagneticSensorSample sample,
                    MagneticSensorRange range)
{
  _i2c->write1Byte(QMC5883_ADDRESS, REG_SETRESET, 1);
  _i2c->write1Byte(QMC5883_ADDRESS, REG_CONFIG_1, sample << 6 | range << 4 | rate << 2 | 0b01);
  if (range == RANGE_2GA)
    _sensitivity = SENSITIVITY_2G_LSB_PER_GAUSE;
   else
    _sensitivity = SENSITIVITY_8G_LSB_PER_GAUSE;
}

磁束密度の読み取り

　getXYZRaw(int *x, int *y, int *z)の_offset_xなどはセンサーにはズレがあるので補正しています。
　getXYZNnT()ではgetXYZRaw(int *x, int *y, int *z)で得られた値を_sensitivityでわって磁束密度へ変換しています。

read_magnetic_flux_density

void getXYZRaw(int *x, int *y, int *z)
{
  uint8_t data[6];
  _i2c->readArray(QMC5883_ADDRESS, REG_OUT_X_L, 6, data);
  *x = (data[0] | data[1] << 8) + _offset_x;
  *y = (data[2] | data[3] << 8) + _offset_y;
  *z = (data[4] | data[5] << 8) + _offset_z;
}

XYZ getXYZnT()
{
  XYZ xyz;
  int x, y, z;  
  getXYZRaw(&x, &y, &z);
  xyz.x = x;
  xyz.y = y;
  xyz.z = z;
  xyz.x = xyz.x / _sensitivity;
  xyz.y = xyz.y / _sensitivity;
  xyz.z = xyz.z / _sensitivity;
  return  xyz;
}

方位角への変換

　方位角はセンサーの基盤に書いてあるX軸方向を0度としています。また、真北が0度になるように偏角を足して補正してあります。Angleはラジアンです。

get_true_azimuth

Angle getTrueAzimuthXY()
{
  int x, y, z;
  getXYZRaw(&x, &y, &z);
  float angle = atan2(y, x) + MAGNETIC_DECLINATION;
  if (angle < 0)
    angle = angle + 2* PI;
  if (angle > 2 * PI)
    angle = angle - 2* PI;
  return Angle(angle); 
}

つづく

　私の気力と時間が許す限りつづきを書くつもりです。