Arduino 接入 MPU9250六轴
生活随笔
收集整理的這篇文章主要介紹了
Arduino 接入 MPU9250六轴
小編覺得挺不錯的,現在分享給大家,幫大家做個參考.
使用MPU9250獲得加速，角速度及地磁計數據。
接線使用Arduino上的SCL接Mpu9250的SCL，SDA接SDA，3.3v或5v供電。
使用I2C連接，通過MPU9250不同地址，我們可以獲得不同的數據，即加速度和角速度地址為0x68。
而地磁計地址為0x0C。
下面是代碼：
#include <Wire.h>
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())
Data[index++]=Wire.read();
}
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
Wire.endTransmission();
}
// Initializations
void setup()
{
// Arduino initializations
Wire.begin();
Serial.begin(115200);
// Configure gyroscope range
I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
// Configure accelerometers range
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G);
// Set by pass mode for the magnetometers
I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
// Request first magnetometer single measurement
I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
}
long int cpt=0;
// Main loop, read and display data
void loop()
{
// ____________________________________
// ::: accelerometer and gyroscope :::
// Read accelerometer and gyroscope
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Create 16 bits values from 8 bits data
// Accelerometer
int16_t ax=-(Buf[0]<<8 | Buf[1]);
int16_t ay=-(Buf[2]<<8 | Buf[3]);
int16_t az=Buf[4]<<8 | Buf[5];
// Gyroscope
int16_t gx=-(Buf[8]<<8 | Buf[9]);
int16_t gy=-(Buf[10]<<8 | Buf[11]);
int16_t gz=Buf[12]<<8 | Buf[13];
// Display values
// Accelerometer
// Serial.print (ax,DEC);
// Serial.print (" ");
// Serial.print (ay,DEC);
// Serial.print (" ");
// Serial.print (az,DEC);
// Serial.print (" ");
//
// // Gyroscope
// Serial.print (gx,DEC);
// Serial.print (" ");
// Serial.print (gy,DEC);
// Serial.print (" ");
// Serial.print (gz,DEC);
// Serial.print (" ");
//delay(100);
// Read magnetometer data
I2CwriteByte(MAG_ADDRESS,0x37,0x02);
uint8_t Mag[7];
I2Cread(MAG_ADDRESS,0x03,7,Mag);
// Magnetometer
int16_t mx=-(Mag[0]<<8 | Mag[1]);
int16_t my=-(Mag[2]<<8 | Mag[3]);
int16_t mz=-(Mag[4]<<8 | Mag[5]);
// Magnetometer
Serial.print (mx,DEC);
Serial.print (" ");
Serial.print (my,DEC);
Serial.print (" ");
Serial.print (mz,DEC);
Serial.print (" ");
// End of line
Serial.println("");
//delay(100);
}
View Code
使用mahony算法，地磁計融合與不融合地磁計算四元數。
使用mahony方法進行計算。
只使用acc與gyro計算姿態，主要的計算步驟如下：
第一：將acc單位化。
第二：預測慣性向量，根據現有的四元數作預測，q為四元數，v為預測慣性向量。
第三：誤差計算，e為實際測量acc向預測v的叉乘，結果為一個旋轉誤差。
第四：PI補償
即當eInt為，計算eInt為下：
第五：計算四元數導， w為角速度：
第六：更新：
當使用地磁計進行數據融合時，重新計算。
第一：單位化acc與mag。
第二：計算h ，地磁參考方向，其中為q的共軛四元數。
第三：計算重力與地磁估計方向
第四：計算誤差
第五：與不使用地磁計相同，計算累計誤差，根據PI進行計算Gyro，
即當eInt為，計算eInt為下：
第五：計算四元數導， w為角速度：
第六：更新：
#include <Wire.h>
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
float Kp = 1;
float Ki = 0.0;
double eInt1=0.0;
double eInt2=0.0;
double eInt3=0.0;
double q1=1.0;
double q2=0.0;
double q3=0.0;
double q4=0.0;
double delta_t = 1.0/256.0;
long t_pre;
// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())
Data[index++]=Wire.read();
}
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
Wire.endTransmission();
}
// Initializations
void setup()
{
// Arduino initializations
Wire.begin();
Serial.begin(115200);
// Configure gyroscope range
I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
// Configure accelerometers range
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G);
// Set by pass mode for the magnetometers
I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
// Request first magnetometer single measurement
I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
}
double quaternionProd1(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r1;
}
double quaternionProd2(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r2;
}
double quaternionProd3(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r3;
}
double quaternionProd4(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r4;
}
void MahonyUpDate(double delta_t,double ax, double ay,double az, double gx, double gy, double gz, double mx, double my, double mz)
{
double a_norm = sqrt(ax*ax+ay*ay+az*az);
if(a_norm==0)
{
Serial.print ("
");
Serial.print ("Acc errer.
");
return;
}
ax = ax/a_norm;
ay = ay/a_norm;
az = az/a_norm;
double m_norm = sqrt(mx*mx+my*my+mz*mz);
if(m_norm==0)
{
Serial.print ("
");
Serial.print ("Mag errer.
");
return;
}
mx = mx/m_norm;
my = my/m_norm;
mz = mz/m_norm;
double mq1,mq2,mq3,mq4;
double h1,h2,h3,h4;
double qdot1,qdot2,qdot3,qdot4;
//Magwick mag
mq1 = quaternionProd1(q1,q2,q3,q4,0,mx,my,mz);
mq2 = quaternionProd2(q1,q2,q3,q4,0,mx,my,mz);
mq3 = quaternionProd3(q1,q2,q3,q4,0,mx,my,mz);
mq4 = quaternionProd4(q1,q2,q3,q4,0,mx,my,mz);
h1 = quaternionProd1(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h2 = quaternionProd2(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h3 = quaternionProd3(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h4 = quaternionProd4(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
float b2 = sqrt( h2*h2 +h3*h3);
float b4 = h4;
float v1 = 2*(q2*q4-q1*q3);
float v2 = 2*(q1*q2+q3*q4);
float v3 = 2*(q1*q1-q2*q2-q3*q3+q4*q4);
float w1 = 2*b2*(0.5-q3*q3-q4*q4)+2*b4*(q2*q4-q1*q3);
float w2 = 2*b2*(q2*q3 - q1*q4)+2*b4*(q1*q2+q3*q4);
float w3 = 2*(q1*q3 + q2*q4)+2*b4*(0.5-q2*q2-q3*q3);
float e1 = ay*v3 - az*v2 + my*w3 - mz*w2;
float e2 = -ax*v3 + az*v1 - mx*w3 + mz*w1;
float e3 = ax*v2 - ay*v1 + mx*w2 - my*w1;
// //Magwick acc
// float v1 = 2*(q2*q4-q1*q3);
// float v2 = 2*(q1*q2+q3*q4);
// float v3 = q1*q1-q2*q2-q3*q3+q4*q4;
// float e1 = ay*v3 - az*v2;
// float e2 = -ax*v3 + az*v1;
// float e3 = ax*v2 - ay*v1;
//errer
eInt1 = eInt1 + e1*delta_t;
eInt2 = eInt2 + e2*delta_t;
eInt2 = eInt2 + e2*delta_t;
//PI
gx = gx + Kp*e1 +Ki*eInt1;
gy = gy + Kp*e2 +Ki*eInt2;
gz = gz + Kp*e3 +Ki*eInt3;
//q dot
qdot1 = 0.5* quaternionProd1(q1,q2,q3,q4, 0,gx,gy,gz);
qdot2 = 0.5* quaternionProd2(q1,q2,q3,q4, 0,gx,gy,gz);
qdot3 = 0.5* quaternionProd3(q1,q2,q3,q4, 0,gx,gy,gz);
qdot4 = 0.5* quaternionProd4(q1,q2,q3,q4, 0,gx,gy,gz);
//quaternion calculate
q1 = q1 + qdot1*delta_t;
q2 = q2 + qdot2*delta_t;
q3 = q3 + qdot3*delta_t;
q4 = q4 + qdot4*delta_t;
//unify
double norm_q = sqrt(q1*q1+q2*q2+q3*q3+q4*q4);
q1 = q1/norm_q;
q2 = q2/norm_q;
q3 = q3/norm_q;
q4 = q4/norm_q;
Serial.print (q1,DEC);
Serial.print (" ");
Serial.print (q2,DEC);
Serial.print (" ");
Serial.print (q3,DEC);
Serial.print (" ");
Serial.print (q4,DEC);
Serial.print (" ");
Serial.println("");
}
void loop()
{
// ____________________________________
// ::: accelerometer and gyroscope :::
// Read accelerometer and gyroscope
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Create 16 bits values from 8 bits data
// Accelerometer
int16_t ax=-(Buf[0]<<8 | Buf[1]);
int16_t ay=-(Buf[2]<<8 | Buf[3]);
int16_t az=Buf[4]<<8 | Buf[5];
// Gyroscope
int16_t gx=-(Buf[8]<<8 | Buf[9]);
int16_t gy=-(Buf[10]<<8 | Buf[11]);
int16_t gz=Buf[12]<<8 | Buf[13];
double ax1 = ax/2048.0;
double ay1 = ay/2048.0;
double az1 = az/2048.0;
double gx1 = gx*3.14/16.4/180;
double gy1 = gy*3.14/16.4/180;
double gz1 = gz*3.14/16.4/180;
// Display values
// Accelerometer
Serial.print (ax1,DEC);
Serial.print (" ");
Serial.print (ay1,DEC);
Serial.print (" ");
Serial.print (az1,DEC);
Serial.print (" ");
// Gyroscope
Serial.print (gx1,DEC);
Serial.print (" ");
Serial.print (gy1,DEC);
Serial.print (" ");
Serial.print (gz1,DEC);
Serial.print (" ");
// Read magnetometer data
I2CwriteByte(MAG_ADDRESS,0x37,0x02);
uint8_t Mag[7];
I2Cread(MAG_ADDRESS,0x03,7,Mag);
// Magnetometer
int16_t mx=-(Mag[3]<<8 | Mag[2]);
int16_t my=-(Mag[1]<<8 | Mag[0]);
int16_t mz=-(Mag[5]<<8 | Mag[4]);
double mx1 = mx*0.6;
double my1 = my*0.6;
double mz1 = mz*0.6;
// Magnetometer
// Serial.print (mx1,DEC);
// Serial.print (" ");
// Serial.print (my1,DEC);
// Serial.print (" ");
// Serial.print (mz1,DEC);
// Serial.print (" ");
//run mahony algrithom
long time = millis();
double delta_t = (time - t_pre)/1000.0;
// Serial.print (delta_t,DEC);
// Serial.print (" ");
MahonyUpDate(delta_t,ax1,ay1,az1,gx1,gy1,gz1,mx1,my1,mz1);
t_pre = time;
// End of line
//delay(100);
}
配合ros，使用arduino。
#include <Wire.h>
#include <ros.h>
#include <sensor_msgs/Imu.h>
ros::NodeHandle nh;
sensor_msgs::Imu imu_sensor;
ros::Publisher imu_sensor_pub("imu", &imu_sensor);
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
float Kp = 5;
float Ki = 0.0;
double eInt1=0.0;
double eInt2=0.0;
double eInt3=0.0;
double q1=1.0;
double q2=0.0;
double q3=0.0;
double q4=0.0;
long t_pre;
// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())
Data[index++]=Wire.read();
}
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
Wire.endTransmission();
}
// Initializations
void setup()
{
// Arduino initializations
Wire.begin();
nh.initNode();
nh.advertise(imu_sensor_pub);
I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G);
I2CwriteByte(MPU9250_ADDRESS,0x37,0x02);
I2CwriteByte(MAG_ADDRESS,0x0A,0x16);
//ros initialize
}
double quaternionProd1(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r1;
}
double quaternionProd2(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r2;
}
double quaternionProd3(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r3;
}
double quaternionProd4(double a1,double a2,double a3,double a4,double b1,double b2,double b3,double b4)
{
double r1,r2,r3,r4;
r1 = a1*b1-a2*b2-a3*b3-a4*b4;
r2 = a1*b2+a2*b1+a3*b4-a4*b3;
r3 = a1*b3-a2*b4+a3*b1+a4*b2;
r4 = a1*b4+a2*b3-a3*b2+a4*b1;
return r4;
}
void MahonyUpDate(double delta_t,double ax, double ay,double az, double gx, double gy, double gz, double mx, double my, double mz)
{
double a_norm = sqrt(ax*ax+ay*ay+az*az);
if(a_norm==0)
{
Serial.print ("
");
Serial.print ("Acc errer.
");
return;
}
ax = ax/a_norm;
ay = ay/a_norm;
az = az/a_norm;
double m_norm = sqrt(mx*mx+my*my+mz*mz);
if(m_norm==0)
{
Serial.print ("
");
Serial.print ("Mag errer.
");
return;
}
mx = mx/m_norm;
my = my/m_norm;
mz = mz/m_norm;
double mq1,mq2,mq3,mq4;
double h1,h2,h3,h4;
double qdot1,qdot2,qdot3,qdot4;
//Magwick mag
mq1 = quaternionProd1(q1,q2,q3,q4,0,mx,my,mz);
mq2 = quaternionProd2(q1,q2,q3,q4,0,mx,my,mz);
mq3 = quaternionProd3(q1,q2,q3,q4,0,mx,my,mz);
mq4 = quaternionProd4(q1,q2,q3,q4,0,mx,my,mz);
h1 = quaternionProd1(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h2 = quaternionProd2(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h3 = quaternionProd3(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
h4 = quaternionProd4(mq1,mq2,mq3,mq4,q1,-q2,-q3,-q4);
float b2 = sqrt( h2*h2 +h3*h3);
float b4 = h4;
float v1 = 2*(q2*q4-q1*q3);
float v2 = 2*(q1*q2+q3*q4);
float v3 = 2*(q1*q1-q2*q2-q3*q3+q4*q4);
float w1 = 2*b2*(0.5-q3*q3-q4*q4)+2*b4*(q2*q4-q1*q3);
float w2 = 2*b2*(q2*q3 - q1*q4)+2*b4*(q1*q2+q3*q4);
float w3 = 2*(q1*q3 + q2*q4)+2*b4*(0.5-q2*q2-q3*q3);
float e1 = ay*v3 - az*v2 + my*w3 - mz*w2;
float e2 = -ax*v3 + az*v1 - mx*w3 + mz*w1;
float e3 = ax*v2 - ay*v1 + mx*w2 - my*w1;
// //Magwick acc
// float v1 = 2*(q2*q4-q1*q3);
// float v2 = 2*(q1*q2+q3*q4);
// float v3 = q1*q1-q2*q2-q3*q3+q4*q4;
// float e1 = ay*v3 - az*v2;
// float e2 = -ax*v3 + az*v1;
// float e3 = ax*v2 - ay*v1;
//errer
eInt1 = eInt1 + e1*delta_t;
eInt2 = eInt2 + e2*delta_t;
eInt2 = eInt2 + e2*delta_t;
//PI
gx = gx + Kp*e1 +Ki*eInt1;
gy = gy + Kp*e2 +Ki*eInt2;
gz = gz + Kp*e3 +Ki*eInt3;
//q dot
qdot1 = 0.5* quaternionProd1(q1,q2,q3,q4, 0,gx,gy,gz);
qdot2 = 0.5* quaternionProd2(q1,q2,q3,q4, 0,gx,gy,gz);
qdot3 = 0.5* quaternionProd3(q1,q2,q3,q4, 0,gx,gy,gz);
qdot4 = 0.5* quaternionProd4(q1,q2,q3,q4, 0,gx,gy,gz);
//quaternion calculate
q1 = q1 + qdot1*delta_t;
q2 = q2 + qdot2*delta_t;
q3 = q3 + qdot3*delta_t;
q4 = q4 + qdot4*delta_t;
//unify
double norm_q = sqrt(q1*q1+q2*q2+q3*q3+q4*q4);
q1 = q1/norm_q;
q2 = q2/norm_q;
q3 = q3/norm_q;
q4 = q4/norm_q;
imu_sensor.orientation.x = q1;
imu_sensor.orientation.y = q2;
imu_sensor.orientation.z = q3;
imu_sensor.orientation.w = q4;
}
void loop()
{
// ____________________________________
// ::: accelerometer and gyroscope :::
// Read accelerometer and gyroscope
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
// Create 16 bits values from 8 bits data
// Accelerometer
int16_t ax=-(Buf[0]<<8 | Buf[1]);
int16_t ay=-(Buf[2]<<8 | Buf[3]);
int16_t az=Buf[4]<<8 | Buf[5];
// Gyroscope
int16_t gx=-(Buf[8]<<8 | Buf[9]);
int16_t gy=-(Buf[10]<<8 | Buf[11]);
int16_t gz=Buf[12]<<8 | Buf[13];
double ax1 = ax/2048.0;
double ay1 = ay/2048.0;
double az1 = az/2048.0;
double gx1 = gx*3.14/16.4/180;
double gy1 = gy*3.14/16.4/180;
double gz1 = gz*3.14/16.4/180;
imu_sensor.header.frame_id = "base_link";
imu_sensor.angular_velocity.y = gy1;
imu_sensor.angular_velocity.x = gx1;
imu_sensor.angular_velocity.z = gz1;
imu_sensor.linear_acceleration.x = ax1;
imu_sensor.linear_acceleration.y = ay1;
imu_sensor.linear_acceleration.z = az1;
float angular_vel[9]={1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};
float orientation[9]={1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6};
for(int i=0; i<9; i++){
imu_sensor.orientation_covariance[i] = angular_vel[i];
imu_sensor.angular_velocity_covariance[i] = orientation[i];
}
// Read magnetometer data
I2CwriteByte(MAG_ADDRESS,0x37,0x02);
uint8_t Mag[7];
I2Cread(MAG_ADDRESS,0x03,7,Mag);
// Magnetometer
int16_t mx=-(Mag[3]<<8 | Mag[2]);
int16_t my=-(Mag[1]<<8 | Mag[0]);
int16_t mz=-(Mag[5]<<8 | Mag[4]);
double mx1 = mx*0.6;
double my1 = my*0.6;
double mz1 = mz*0.6;
//run mahony algrithom
long time = millis();
double delta_t = (time - t_pre)/1000.0;
MahonyUpDate(delta_t,ax1,ay1,az1,gx1,gy1,gz1,mx1,my1,mz1);
t_pre = time;
// End of line
imu_sensor_pub.publish(&imu_sensor);
nh.spinOnce();
delay(10);
}
View Code
總結
以上是生活随笔為你收集整理的Arduino 接入 MPU9250六轴的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 大数据传输,文件传输的专业解决方案！
- 下一篇： 枣类制品好评90字