使用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); }
使用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); }