はじめに
夏休みが後3日ほどで終わってしまうので割と焦りながら進めています（特に後期が忙しいとかではなく、なんとなく自分の気分的に夏の工作だったので）。
MPU9250の制御プログラムを生で書いていくと読みにくくなりそうだったので、最低限必要なデータが取り出せるようにライブラリ化しました。
ソースコード
Arduino公式のライブラリに登録するほど良いソースコードではないのでコピペの際は自己責任で。
次の画像のようにArduinoのライブラリが保存されているフォルダにコピペすれば通常のライブラリと同様に使えます。
Macのデフォルトだと「書類」>「Arduino」> 「libraries」です。
.hファイル
#ifndef MPU9250_KTORA_H
#define MPU9250_KTORA_H
//ヘッダ
#include <Arduino.h>
#include <Wire.h>
//加速度 設定値アドレス
#define RANGE_ACC_2G 0x00 //2G
#define RANGE_ACC_4G 0x08 //4G
#define RANGE_ACC_8G 0x10 //8G
#define RANGE_ACC_16G 0x18 //16G
//角速度 設定値アドレス
#define RANGE_GYRO_250DPS 0x00 //250DPS
#define RANGE_GYRO_500DPS 0x08 //500DPS
#define RANGE_GYRO_1000DPS 0x10 //1000DPS
#define RANGE_GYRO_2000DPS 0x18 //2000DPS
//磁気センサ モード 未実装（いつか作るかも）
//クラス宣言
class MPU9250_ktora{
//private
private:
//Wireオブジェクト
TwoWire *Wire_use;
//測定範囲 計算用
float AccelRange; //加速度
float GyroRange; //角速度
//生データ保存用
//加速度
int16_t Data_accX;
int16_t Data_accY;
int16_t Data_accZ;
//温度
int16_t Data_temp;
//角速度
int16_t Data_gyrX;
int16_t Data_gyrY;
int16_t Data_gyrZ;
//磁気
int16_t Data_magX;
int16_t Data_magY;
int16_t Data_magZ;
//オフセット
float Offset_accX;
float Offset_accY;
float Offset_accZ;
float Offset_GyrX;
float Offset_GyrY;
float Offset_GyrZ;
float Offset_MagX;
float Offset_MagY;
float Offset_MagZ;
//I2Cデバイスへのデータ送信(書き込み)
//アドレス,レジスタ,データ
void i2cWriteData(uint8_t Dev_Address, uint8_t Reg_Address, uint8_t Data);
//I2Cデバイスからのデータ受信(読み込み)
//アドレス,レジスタ,要求バイト数,格納変数
void i2cReadData(uint8_t Dev_Address, uint8_t Reg_Address, uint8_t Byte_Num, uint8_t *Data);
//public
public:
//コンストラクタ 初期化
MPU9250_ktora():
Wire_use(NULL),
AccelRange(0.0),
GyroRange(0.0),
Data_accX(0),
Data_accY(0),
Data_accZ(0),
Data_temp(0),
Data_gyrX(0),
Data_gyrY(0),
Data_gyrZ(0),
Data_magX(0),
Data_magY(0),
Data_magZ(0),
Offset_accX(0),
Offset_accY(0),
Offset_accZ(0),
Offset_GyrX(0),
Offset_GyrY(0),
Offset_GyrZ(0),
Offset_MagX(0),
Offset_MagY(0),
Offset_MagZ(0) {};
//Wireオブジェクトの取得
void set_Wire(TwoWire *wire);
//MPU9250のセットアップ
void setup_Mpu9250(uint8_t acc_range = RANGE_ACC_8G, uint8_t gyro_range = RANGE_GYRO_1000DPS);
//データの更新
void update_Data_Mpu9250();
//オフセットの計算
void calc_Offset();
//オフセットの設定
void set_Offset(float of_calcAX, float of_calcAY, float of_calcAZ, float of_calcGX, float of_calcGY, float of_calcGZ, float of_calcMX, float of_calcMY, float of_calcMZ);
//x軸加速度
float get_AccX();
//y軸加速度
float get_AccY();
//z軸加速度
float get_AccZ();
//温度
float get_Temp();
//x軸角速度
float get_GyrX();
//y軸角速度
float get_GyrY();
//z軸角速度
float get_GyrZ();
//x軸磁気
float get_MagX();
//y軸磁気
float get_MagY();
//z軸磁気
float get_MagZ();
};
#endif
.cppファイル
#include "MPU9250_ktora.h"
//MPU9250
#define MPU9250_ADDRESS 0x68 //MPU9250 スレーブアドレス
#define PWR_MGMT_1 0x6B //スリープモード 解除用アドレス
//測定範囲 設定アドレス
#define RANGE_ACC 0x1c //加速度
#define RANGE_GYRO 0x1b //角速度
//各レジスタのアドレス
#define ACC_ADDRESS 0x3b //加速度(6byte)
#define TEMP_ADDRESS 0x65 //温度(2byte)
#define GYRO_ADDRESS 0x43 //角速度(6byte)
//AK8963
#define INT_PIN_CFG 0x37 //磁気センサ(AK8963) バイパスモード設定アドレス
#define AK8963_ADDRESS 0x0c //AK8963 スレーブアドレス
#define CNTL1 0x0A //磁気センサ モード設定(データシート参照)
#define ST1 0x02 //フラグアドレス
#define MAG_ADDRESS 0x03 //磁気アドレス(6byte)
//I2Cデバイスへのデータ送信(書き込み)
//アドレス,レジスタ,データ
void MPU9250_ktora::i2cWriteData(uint8_t Dev_Address, uint8_t Reg_Address, uint8_t Data){
Wire_use -> beginTransmission(Dev_Address); //I2Cデバイスのアドレスの指定
Wire_use -> write(Reg_Address); //I2Cデバイスの書き込みレジスタの指定
Wire_use -> write(Data); //データの指定
Wire_use -> endTransmission(); //送信完了
}
//I2Cデバイスからのデータ受信(読み込み)
//アドレス,レジスタ,要求バイト数,格納変数
void MPU9250_ktora::i2cReadData(uint8_t Dev_Address, uint8_t Reg_Address, uint8_t Byte_Num, uint8_t *Data){
Wire_use -> beginTransmission(Dev_Address); //I2Cデバイスのアドレスの指定
Wire_use -> write(Reg_Address); //I2Cデバイスの書き込みレジスタの指定
Wire_use -> endTransmission(); //送信完了
Wire_use -> requestFrom(Dev_Address, Byte_Num); //I2Cデバイスのアドレス,読み込みサイズの指定
uint8_t i = 0; //要素番号
while(Wire_use -> available()){
Data[i++] = Wire_use -> read(); //データの読み込み
}
}
//Wireオブジェクトの取得
void MPU9250_ktora::set_Wire(TwoWire *wire){
Wire_use = wire;
if (Wire_use == NULL){
Wire_use = &Wire;
Wire_use -> begin();
}
}
//MPU9250のセットアップ
void MPU9250_ktora::setup_Mpu9250(uint8_t acc_range, uint8_t gyro_range){
//MPU9250
//スリープモード解除
i2cWriteData(MPU9250_ADDRESS,PWR_MGMT_1,0x00); //スリープモード解除用アドレスに書き込み
//加速度センサのレンジを設定
i2cWriteData(MPU9250_ADDRESS,RANGE_ACC,acc_range); //レンジ設定アドレスに書き込み
//計算用データ選定
switch(acc_range){
case RANGE_ACC_2G:
AccelRange = 2.0;
break;
case RANGE_ACC_4G:
AccelRange = 4.0;
break;
case RANGE_ACC_8G:
AccelRange = 8.0;
break;
case RANGE_ACC_16G:
AccelRange = 16.0;
break;
default:
break;
}
//ジャイロセンサのレンジを設定
i2cWriteData(MPU9250_ADDRESS,RANGE_GYRO,gyro_range); //レンジ設定アドレスに書き込み
//計算用データ選定
switch(acc_range){
case RANGE_GYRO_250DPS:
GyroRange = 250.0;
break;
case RANGE_GYRO_500DPS:
GyroRange = 500.0;
break;
case RANGE_GYRO_1000DPS:
GyroRange = 1000.0;
break;
case RANGE_GYRO_2000DPS:
GyroRange = 2000.0;
break;
default:
break;
}
//磁気センサ(AK8963)
i2cWriteData(MPU9250_ADDRESS,INT_PIN_CFG,0x02); //磁気センサ(AK8963)アクセス
i2cWriteData(AK8963_ADDRESS,CNTL1,0x16); //分解能設定(設定値はデータシート参照)
}
//データの更新
void MPU9250_ktora::update_Data_Mpu9250(){
//MPU9250
uint8_t Data_ATG[14]; //データ(加速度,温度,ジャイロ)格納用配列
uint8_t Data_MAG[7]; //データ(磁気)格納用
//加速度,温度,ジャイロの読み込み
i2cReadData(MPU9250_ADDRESS,ACC_ADDRESS,14,Data_ATG); //加速度のアドレスから14byte読み込む(温度,ジャイロも読み込む)
//磁気の読み込み
uint8_t ST1_check = 0; //フラグ
i2cReadData(AK8963_ADDRESS,ST1,1,&ST1_check); //ST1レジスタで読み出し準備ができているか確認
//準備ができていたら磁気のデータを読み出す
if(ST1_check & 0x01){
//ST2レジスタ(7byte目)まで読み込まないとデータ保護が解除されない
i2cReadData(AK8963_ADDRESS,MAG_ADDRESS,7,Data_MAG); //磁気アドレスから7byte読み込む
}
//2byteで1つのデータなので足し合わせる
//読み込みデータを16bitに変換
//加速度
Data_accX = (Data_ATG[0] << 8) | Data_ATG[1]; //x軸加速度
Data_accY = (Data_ATG[2] << 8) | Data_ATG[3]; //y軸加速度
Data_accZ = (Data_ATG[4] << 8) | Data_ATG[5]; //z軸加速度
//チップ温度
Data_temp = (Data_ATG[6] << 8) | Data_ATG[7]; //チップ温度
//ジャイロ
Data_gyrX = (Data_ATG[8] << 8) | Data_ATG[9]; //x軸ジャイロ
Data_gyrY = (Data_ATG[10] << 8) | Data_ATG[11]; //y軸ジャイロ
Data_gyrZ = (Data_ATG[12] << 8) | Data_ATG[13]; //z軸ジャイロ
//磁気(軸が異なる)
Data_magX = (Data_MAG[3] << 8) | Data_MAG[2]; //x軸磁気
Data_magY = (Data_MAG[1] << 8) | Data_MAG[0]; //y軸磁気
Data_magZ = -((Data_MAG[5] << 8) | Data_MAG[4]); //z軸磁気
}
//オフセットの計算
void MPU9250_ktora::calc_Offset(){
float of_calcAX = 0;
float of_calcAY = 0;
float of_calcAZ = 0;
float of_calcGX = 0;
float of_calcGY = 0;
float of_calcGZ = 0;
//100回の計算の平均をオフセットとする
for(uint8_t i = 0; i < 100; i++){
//データの更新
update_Data_Mpu9250();
of_calcAX += get_AccX();
of_calcAY += get_AccY();
of_calcAZ += get_AccZ() - 1;
of_calcGX += get_GyrX();
of_calcGY += get_GyrY();
of_calcGZ += get_GyrZ() - 1;
}
Offset_accX = of_calcAX / 100;
Offset_accY = of_calcAY / 100;
Offset_accZ = of_calcAZ / 100;
Offset_GyrX = of_calcGX / 100;
Offset_GyrY = of_calcGY / 100;
Offset_GyrZ = of_calcGZ / 100;
}
//オフセットの設定
void MPU9250_ktora::set_Offset(float of_calcAX, float of_calcAY, float of_calcAZ, float of_calcGX, float of_calcGY, float of_calcGZ, float of_calcMX, float of_calcMY, float of_calcMZ){
//値をそのまま格納
Offset_accX = of_calcAX;
Offset_accY = of_calcAY;
Offset_accZ = of_calcAZ;
Offset_GyrX = of_calcGX;
Offset_GyrY = of_calcGY;
Offset_GyrZ = of_calcGZ;
Offset_MagX = of_calcMX;
Offset_MagY = of_calcMY;
Offset_MagZ = of_calcMZ;
}
//データの出力及び単位変換
//float(32768 ~ -32767)で割ってレンジに割り当てる
//x軸加速度
float MPU9250_ktora::get_AccX(){
float AccX = Data_accX * AccelRange / 32768.0; //加速度(G)に変換
return AccX - Offset_accX;
}
//y軸加速度
float MPU9250_ktora::get_AccY(){
float AccY = Data_accY * AccelRange / 32768.0; //加速度(G)に変換
return AccY - Offset_accY;
}
//z軸加速度
float MPU9250_ktora::get_AccZ(){
float AccZ = Data_accZ * AccelRange / 32768.0; //加速度(G)に変換
return AccZ - Offset_accZ;
}
//温度
float MPU9250_ktora::get_Temp(){
float Temp = (Data_temp / 333.87) + 21.0; //セルシウス度(°C)に変換(データシートより)
return Temp;
}
//x軸角速度
float MPU9250_ktora::get_GyrX(){
float GyrX = Data_gyrX * GyroRange / 32768.0; //角速度(deg/s)に変換
return GyrX - Offset_GyrX;
}
//y軸角速度
float MPU9250_ktora::get_GyrY(){
float GyrY = Data_gyrY * GyroRange / 32768.0; //角速度(deg/s)に変換
return GyrY - Offset_GyrY;
}
//z軸角速度
float MPU9250_ktora::get_GyrZ(){
float GyrZ = Data_gyrZ * GyroRange / 32768.0; //角速度(deg/s)に変換
return GyrZ - Offset_GyrZ;
}
//x軸磁気
float MPU9250_ktora::get_MagX(){
float MagX = Data_magX * 4800.0 / 32768.0; //テスラ[uT]に変換
return MagX - Offset_MagX;
}
//y軸磁気
float MPU9250_ktora::get_MagY(){
float MagY = Data_magY * 4800.0 / 32768.0; //テスラ[uT]に変換
return MagY - Offset_MagY;
}
//z軸磁気
float MPU9250_ktora::get_MagZ(){
float MagZ = Data_magZ * 4800.0 / 32768.0; //テスラ[uT]に変換
return MagZ - Offset_MagZ;
}
使い方
使い方は簡単で次のようにして呼び出すとそれぞれのデータが出力されます。
#include <MPU9250_ktora.h>
MPU9250_ktora mpu9250; //オブジェクト生成
void setup(){
Serial.begin(9600); //シリアル通信を開始する
Wire.begin(); //I2C通信を開始する
mpu9250.set_Wire(&Wire); //Wireのコピー
mpu9250.setup_Mpu9250(); //mpu9250の設定
}
void loop(){
mpu9250.update_Data_Mpu9250(); //データの更新
//シリアルモニタ出力
Serial.print("AccelX=");
Serial.print(mpu9250.get_AccX());
Serial.print(" AccelY=");
Serial.print(mpu9250.get_AccY());
Serial.print(" AccelZ=");
Serial.print(mpu9250.get_AccZ());
Serial.println("");
Serial.print("Temperature=");
Serial.print(mpu9250.get_Temp());
Serial.println("");
Serial.print("GyroX=");
Serial.print(mpu9250.get_GyrX());
Serial.print(" GyroY=");
Serial.print(mpu9250.get_GyrY());
Serial.print(" GyroZ=");
Serial.print(mpu9250.get_GyrZ());
Serial.println("");
Serial.print("MagneticX=");
Serial.print(mpu9250.get_MagX());
Serial.print(" MagneticY=");
Serial.print(mpu9250.get_MagY());
Serial.print(" MagneticZ=");
Serial.print(mpu9250.get_MagZ());
Serial.println("");
delay(2000);
}
データは次の画像のようにシリアルモニタに出力されます。
オフセット（加速度と角速度のみ磁気センサについては未実装）を自動で設定する場合（複数回の平均値を計算）には、センサを水平面に置いてsetup()内に次の行を追加します。
mpu9250.calc_Offset(); //オフセットの計算
手動で設定する場合にはsetup()内に次の行を追加します。
mpu9250.set_Offset(加速度x,y,z,角速度x,y,z,磁気x,y,z); //オフセットの設定
最後に
今週中にはPID制御の方に移っていきたいです。
通信まではサクッと進める予定だったのですが、なかなかゆっくりになってしまっています…。
コメント