概要
タイトル通りです。
SPIで使う方法は今後
参考
2年以上前に自分が書いたものも参考にしました
配線
|MPU9250 |expression |Arduino |
|---|---|---|--- |
|Vcc |5.0V | 5.0V |
|GND |GND |GND |
|SCL|clock|A5|
|SDA|Data Input|A4|
AD0をHIGH -> アドレスが0x69
AD0をLOW -> アドレスが0x68 (何も接続しない場合)
動作test
最初に レジスタアドレス0x6B の Bit6 の SLEEP を LOW にしないと動作しない。(デフォルトで0x6B-> 0x40 のため、最初はSLEEPがついている。
接続確認のために、Who_I_amレジスタ(アドレス0x75)を読む、0x71が読み込めたら成功
# include <Wire.h>
# define mpu_address 0x68
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("Test start");
write_mpu(0x6B,0x00);
}
void loop() {
Serial.println(read_mpu(0x75),HEX);
Serial.println("-----");
delay(1000);
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
ここでうまくいかない場合、接続が悪い可能性がある、I2C_scanner等での確認をしてほしい
設定データについて
|address |expression |デフォルト |
|---|---|---|---|
|0x19| sampling rate |0x00|
|0x1A|sampling rate|0x00|
|0x1B|ジャイロ感度・ローパスフィルタ・sampling rate|0x00|
|0x1C|加速度感度|0x00|
|0x1D|ローパスフィルタ|0x00|
- デフォルトでFCHOICE_B は00 です。Bは反転を表すので、デフォルトで FCHOICE = 11です。
- 0x19のSMPLRT_DIVはFCHOICE = 00の時のみ効果があり、SAMPLE_RATE= Internal_Sample_Rate / (1 + SMPLRT_DIV)となるようです。
感度設定
dps = degree per second
360[dps] = 2π [rad/s]
デフォルトは250dpsである。
250dpsの時
得られたデータが262(0x106)ならば、250/(2^15) * 262 = 2°/s の角加速度ということ
動作プログラム
これで、加速度・角速度の計測ができます
# include <Wire.h>
# define mpu_address 0x68
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("Test start");
write_mpu(0x6B, 0x00); //sleep 解除
//write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
}
void loop() {
/*
Serial.println(read_mpu(0x19));
Serial.println(read_mpu(0x1B));
Serial.println(read_mpu(0x1D));
Serial.println(read_mpu(0x1A));
Serial.println("-----");
*/
byte gx_H = read_mpu(0x43);
byte gx_L = read_mpu(0x44);
byte gy_H = read_mpu(0x45);
byte gy_L = read_mpu(0x46);
byte gz_H = read_mpu(0x47);
byte gz_L = read_mpu(0x48);
int gx = gx_H << 8 | gx_L;
int gy = gy_H << 8 | gy_L;
int gz = gz_H << 8 | gz_L;
float gx_dps = gx / 131.0; //degree / s へ変換
float gy_dps = gy / 131.0;
float gz_dps = gz / 131.0;
byte ax_H = read_mpu(0x3B);
byte ax_L = read_mpu(0x3C);
byte ay_H = read_mpu(0x3D);
byte ay_L = read_mpu(0x3E);
byte az_H = read_mpu(0x3F);
byte az_L = read_mpu(0x40);
int ax = ax_H << 8 | ax_L;
int ay = ay_H << 8 | ay_L;
int az = az_H << 8 | az_L;
/*
Serial.print(gx);
Serial.print(" ");
Serial.print(gy);
Serial.print(" ");
Serial.print(gz);
Serial.println(" ");
*/
Serial.print(gx_dps);
Serial.print(" ");
Serial.print(gy_dps);
Serial.print(" ");
Serial.print(gz_dps);
Serial.println(" ");
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
byte read_mpu_multiByte(byte add, int num) {
byte k[10];
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
return k;
}
磁気センサーを使う
ようやく本題に入ります。
レジスタ55(0x37)のBYPASS_ENを1にすることでバイパスモードに入ります。そうすることで、
Interface Bypass Mux によって、AK8963と接続されます。データシート再度載せます
動作確認
Who_I_am(アドレス0x00)を読んでみましょう
# include <Wire.h>
# define mpu_address 0x68
# define mag_address 0x0C
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("Test start");
write_mpu(0x6B, 0x00); //sleep 解除
//write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
write_mpu(0x37,0x02); //AK8963接続
}
void loop() {
byte gx_H = read_mpu(0x43);
byte gx_L = read_mpu(0x44);
byte gy_H = read_mpu(0x45);
byte gy_L = read_mpu(0x46);
byte gz_H = read_mpu(0x47);
byte gz_L = read_mpu(0x48);
int gx = gx_H << 8 | gx_L;
int gy = gy_H << 8 | gy_L;
int gz = gz_H << 8 | gz_L;
float gx_dps = gx / 131.0; //degree / s へ変換
float gy_dps = gy / 131.0;
float gz_dps = gz / 131.0;
byte ax_H = read_mpu(0x3B);
byte ax_L = read_mpu(0x3C);
byte ay_H = read_mpu(0x3D);
byte ay_L = read_mpu(0x3E);
byte az_H = read_mpu(0x3F);
byte az_L = read_mpu(0x40);
int ax = ax_H << 8 | ax_L;
int ay = ay_H << 8 | ay_L;
int az = az_H << 8 | az_L;
/*
Serial.print(gx);
Serial.print(" ");
Serial.print(gy);
Serial.print(" ");
Serial.print(gz);
Serial.println(" ");
*/
/*
Serial.print(gx_dps);
Serial.print(" ");
Serial.print(gy_dps);
Serial.print(" ");
Serial.print(gz_dps);
Serial.println(" ");
*/
Serial.println(read_mag(0x00),HEX);
Serial.println("-----");
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void write_mag(byte add, byte data) {
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mag(byte add) {
byte k;
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
byte read_mpu_multiByte(byte add, int num) {
byte k[10];
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
return k;
}
0x48が読めれば成功です!
磁気データ読み込み
起動にはCNTL1(0x0A)のモードを変更する必要があります(デフォルトでパワーダウンモード)
重要な注意点は ST2レジスタ(0x09)の読み込みを行わないと、データが更新されないことです。
バースト読み込みで0x03から、0x09まで読み込むことを想定して作られているということです。
ポインタとか使おうかと思いましたが、わかりにくいので、全部グローバル変数にしちゃいました・・・
# include <Wire.h>
# define mpu_address 0x68
# define mag_address 0x0C
volatile int mag_x;
volatile int mag_y;
volatile int mag_z;
void setup()
{
Wire.begin();
Serial.begin(9600);
Serial.println("Test start");
write_mpu(0x6B, 0x00); //sleep 解除
//write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
write_mpu(0x37,0x02); //AK8963接続
write_mag(0x0A,0x06); //連続測定mode2
}
void loop() {
byte gx_H = read_mpu(0x43);
byte gx_L = read_mpu(0x44);
byte gy_H = read_mpu(0x45);
byte gy_L = read_mpu(0x46);
byte gz_H = read_mpu(0x47);
byte gz_L = read_mpu(0x48);
int gx = gx_H << 8 | gx_L;
int gy = gy_H << 8 | gy_L;
int gz = gz_H << 8 | gz_L;
float gx_dps = gx / 131.0; //degree / s へ変換
float gy_dps = gy / 131.0;
float gz_dps = gz / 131.0;
byte ax_H = read_mpu(0x3B);
byte ax_L = read_mpu(0x3C);
byte ay_H = read_mpu(0x3D);
byte ay_L = read_mpu(0x3E);
byte az_H = read_mpu(0x3F);
byte az_L = read_mpu(0x40);
int ax = ax_H << 8 | ax_L;
int ay = ay_H << 8 | ay_L;
int az = az_H << 8 | az_L;
byte magx_L = read_mag(0x03);
byte magx_H = read_mag(0x04);
//磁気データ計測
read_mag_multiByte(0x03,7); //7じゃないと動かない
/*
Serial.print(gx);
Serial.print(" ");
Serial.print(gy);
Serial.print(" ");
Serial.print(gz);
Serial.println(" ");
*/
/*
Serial.print(gx_dps);
Serial.print(" ");
Serial.print(gy_dps);
Serial.print(" ");
Serial.print(gz_dps);
Serial.println(" ");
*/
Serial.print(mag_x);
Serial.print(" ");
Serial.print(mag_y);
Serial.print(" ");
Serial.print(mag_z);
Serial.println(" ");
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void write_mag(byte add, byte data) {
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mag(byte add) {
byte k;
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void read_mag_multiByte(byte add, int num) {
byte k[10];
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
mag_x = k[1]<<8 | k[0];
mag_y = k[3]<<8 | k[2];
mag_z = k[5]<<8 | k[4];
}
ひとまず、こんな感じでデータが取れます。このグラフ表示は、Arduino IDEのシリアルプロッタです(Ctrl+Shift+Lで起動)
Magwick Filterを使った、9軸姿勢検出
こちらを参考にしました。
加速度・角速度の読み込みをバーストに変更しました。
ひとまず、やった感じ。ジャイロのオフセットかな。deg/sへの変換でした。
ちょっと動かすと、すごく振動する・・・
感度テスト
250dpsらしいですが、ちょっと感度に不安を感じたので、ジャイロgzを積分し、一回転したとき360度になることを確認するプログラムを作りました。
# include <Wire.h>
# include <FlexiTimer2.h>
# define mpu_address 0x68
# define mag_address 0x0C
# include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
volatile int mag_x;
volatile int mag_y;
volatile int mag_z;
volatile int gx ;
volatile int gy ;
volatile int gz ;
volatile int ax ;
volatile int ay ;
volatile int az ;
volatile float gx_dps ;
volatile float gy_dps ;
volatile float gz_dps ;
volatile unsigned long time_now = 0;
volatile boolean control_flag = false;
volatile float degree=0;
void control() {
if (time_now % 100 == 0) {
control_flag = true;
}
time_now++;
}
void setup()
{
Wire.begin();
Serial.begin(250000);
//Serial.println("Test start");
Serial.println("ROLL PITCH YAW");
write_mpu(0x6B, 0x00); //sleep 解除
write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
write_mpu(0x37, 0x02); //AK8963接続
write_mag(0x0A, 0x06); //連続測定mode2
FlexiTimer2::set(1, 1.0 / 10000, control); // 0.1msごとに
FlexiTimer2::start();
MadgwickFilter.begin(100); //100Hz
}
void loop() {
if (control_flag == true ) {
control_flag = false;
//6軸データ計測
read_mpu_multiByte(0x3B, 14); //7じゃないと動かない
//磁気データ計測
read_mag_multiByte(0x03, 7); //7じゃないと動かない
// Serial.print(gx);
// Serial.print(" ");
// Serial.print(gy);
// Serial.print(" ");
// Serial.print(gz);
// Serial.println(" ");
//
/*
Serial.print(gx_dps);
Serial.print(" ");
Serial.print(gy_dps);
Serial.print(" ");
Serial.print(gz_dps);
Serial.println(" ");
*/
/*
Serial.print(mag_x);
Serial.print(" ");
Serial.print(mag_y);
Serial.print(" ");
Serial.print(mag_z);
Serial.println(" ");
*/
MadgwickFilter.update(gx, gy, gz, ax, ay, az, mag_x, mag_y, mag_z);
float ROLL = MadgwickFilter.getRoll();
float PITCH = MadgwickFilter.getPitch();
float YAW = MadgwickFilter.getYaw();
// Serial.print(ROLL);
// Serial.print(" ");
// Serial.print(PITCH);
// Serial.print(" ");
// Serial.print(YAW);
// Serial.println(" ");
degree+= gz_dps*0.01;
Serial.println(degree);
}
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void read_mpu_multiByte(byte add, int num) {
byte k[14];
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
ax = k[0] << 8 | k[1];
ay = k[2] << 8 | k[3];
az = k[4] << 8 | k[5];
gx = (k[8] << 8 | k[9])-149;
gy = (k[10] << 8 | k[11]) -96;
gz = (k[12] << 8 | k[13])-115;
gx_dps = gx/131.0;
gy_dps = gy/131.0;
gz_dps = gz/131.0;
}
void write_mag(byte add, byte data) {
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mag(byte add) {
byte k;
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void read_mag_multiByte(byte add, int num) {
byte k[10];
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
mag_x = k[1] << 8 | k[0];
mag_y = k[3] << 8 | k[2];
mag_z = k[5] << 8 | k[4];
}
ちゃんと半回転で180になりました
完成
# include <Wire.h>
# include <FlexiTimer2.h>
# define mpu_address 0x68
# define mag_address 0x0C
# include <MadgwickAHRS.h>
Madgwick MadgwickFilter;
volatile int mag_x;
volatile int mag_y;
volatile int mag_z;
volatile int gx ;
volatile int gy ;
volatile int gz ;
volatile int ax ;
volatile int ay ;
volatile int az ;
volatile float gx_dps ;
volatile float gy_dps ;
volatile float gz_dps ;
volatile unsigned long time_now = 0;
volatile boolean control_flag = false;
volatile float degree=0;
void control() {
if (time_now % 100 == 0) {
control_flag = true;
}
time_now++;
}
void setup()
{
Wire.begin();
Serial.begin(250000);
//Serial.println("Test start");
Serial.println("ROLL PITCH YAW");
write_mpu(0x6B, 0x00); //sleep 解除
write_mpu(0x1A, 0x02); //DLPF_CFG = 2, 92Hz
write_mpu(0x37, 0x02); //AK8963接続
write_mag(0x0A, 0x06); //連続測定mode2
FlexiTimer2::set(1, 1.0 / 10000, control); // 0.1msごとに
FlexiTimer2::start();
MadgwickFilter.begin(100); //100Hz
}
void loop() {
if (control_flag == true ) {
control_flag = false;
//6軸データ計測
read_mpu_multiByte(0x3B, 14); //7じゃないと動かない
//磁気データ計測
read_mag_multiByte(0x03, 7); //7じゃないと動かない
// Serial.print(gx);
// Serial.print(" ");
// Serial.print(gy);
// Serial.print(" ");
// Serial.print(gz);
// Serial.println(" ");
//
/*
Serial.print(gx_dps);
Serial.print(" ");
Serial.print(gy_dps);
Serial.print(" ");
Serial.print(gz_dps);
Serial.println(" ");
*/
/*
Serial.print(mag_x);
Serial.print(" ");
Serial.print(mag_y);
Serial.print(" ");
Serial.print(mag_z);
Serial.println(" ");
*/
MadgwickFilter.update(gx_dps, gy_dps, gz_dps, ax, ay, az, mag_x, mag_y, mag_z);
float ROLL = MadgwickFilter.getRoll();
float PITCH = MadgwickFilter.getPitch();
float YAW = MadgwickFilter.getYaw();
Serial.print(ROLL);
Serial.print(" ");
Serial.print(PITCH);
Serial.print(" ");
Serial.print(YAW);
Serial.println(" ");
}
}
void write_mpu(byte add, byte data) {
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mpu(byte add) {
byte k;
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void read_mpu_multiByte(byte add, int num) {
byte k[14];
Wire.beginTransmission(mpu_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mpu_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
ax = k[0] << 8 | k[1];
ay = k[2] << 8 | k[3];
az = k[4] << 8 | k[5];
gx = (k[8] << 8 | k[9])-149;
gy = (k[10] << 8 | k[11]) -96;
gz = (k[12] << 8 | k[13])-115;
gx_dps = gx/131.0;
gy_dps = gy/131.0;
gz_dps = gz/131.0;
}
void write_mag(byte add, byte data) {
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.write(data);
Wire.endTransmission();
}
byte read_mag(byte add) {
byte k;
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, 1);
while (Wire.available()) {
k = Wire.read();
}
return k;
}
void read_mag_multiByte(byte add, int num) {
byte k[10];
Wire.beginTransmission(mag_address);
Wire.write(add);
Wire.endTransmission();
Wire.requestFrom(mag_address, num);
int i = 0;
while (Wire.available()) {
k[i] = Wire.read();
i++;
}
mag_x = k[1] << 8 | k[0];
mag_y = k[3] << 8 | k[2];
mag_z = k[5] << 8 | k[4];
}
ちゃんと動くものができました。
ちなみに角度の初期値は
"MagwickAHRS.cpp"内の
Madgwick::Madgwick() {
beta = betaDef;
q0 = 0.0f;
q1 = 0.0f;
q2 = 1.0f;
q3 = 0.0f;
invSampleFreq = 1.0f / sampleFreqDef;
anglesComputed = 0;
}
の"q0~q3"を変更すればできます。ただ、private変数のため外部で直接変更することはできません。
誰かの参考になることを願います。ではまた。