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 ("\t");
//  Serial.print (ay,DEC);
//  Serial.print ("\t");
//  Serial.print (az,DEC);  
//  Serial.print ("\t");
// 
//  // Gyroscope
//  Serial.print (gx,DEC); 
//  Serial.print ("\t");
//  Serial.print (gy,DEC);
//  Serial.print ("\t");
//  Serial.print (gz,DEC);  
//  Serial.print ("\t");
  //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 ("\t");
  Serial.print (my,DEC);
  Serial.print ("\t");
  Serial.print (mz,DEC);  
  Serial.print ("\t");
  
  // End of line
  Serial.println("");
  //delay(100);    
}

 

使用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 ("\r\n");
          Serial.print ("Acc errer. \r\n");
          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 ("\r\n");
          Serial.print ("Mag errer. \r\n");
          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 ("\t");
    Serial.print (q2,DEC);
    Serial.print ("\t");
    Serial.print (q3,DEC);  
    Serial.print ("\t");
    Serial.print (q4,DEC); 
    Serial.print ("\t");
    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 ("\t");
    Serial.print (ay1,DEC);
    Serial.print ("\t");
    Serial.print (az1,DEC);  
    Serial.print ("\t");
    // Gyroscope
    Serial.print (gx1,DEC); 
    Serial.print ("\t");
    Serial.print (gy1,DEC);
    Serial.print ("\t");
    Serial.print (gz1,DEC);  
    Serial.print ("\t");
  
  
    // 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 ("\t");
//    Serial.print (my1,DEC);
//    Serial.print ("\t");
//    Serial.print (mz1,DEC);  
//    Serial.print ("\t");

    //run mahony algrithom
    long time = millis();
    double delta_t = (time - t_pre)/1000.0;
//    Serial.print (delta_t,DEC);  
//    Serial.print ("\t");
    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 ("\r\n");
          Serial.print ("Acc errer. \r\n");
          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 ("\r\n");
          Serial.print ("Mag errer. \r\n");
          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);    
  }