Update MPU9255_node.cpp

mdleiton · Feb 1, 2022 · b3a9d23 · b3a9d23
1 parent 7408874
commit b3a9d23
Showing 1 changed file with 35 additions and 18 deletions.
diff --git a/src/MPU9255_node.cpp b/src/MPU9255_node.cpp
@@ -10,26 +10,31 @@
 int main(int argc, char **argv){
 
   ros::init(argc, argv, "IMU_pub");
-  
+
   ros::NodeHandle n;
   ros::Publisher pub_imu = n.advertise<sensor_msgs::Imu>("imu/data_raw", 2);
   ros::Publisher pub_mag = n.advertise<sensor_msgs::MagneticField>("imu/mag", 2);
-  
-	int fd; 
+
+        int fd,fe;
   wiringPiSetupSys();
   fd = wiringPiI2CSetup(0x68);
-	
-	if (fd == -1) {
+
+        if (fd == -1) {
     printf("no i2c device found\n");
     return -1;
-	}
-  int16_t InBuffer[9] = {0}; 
+        }
+  int16_t InBuffer[9] = {0};
   static int32_t OutBuffer[3] = {0};
 
+
+   wiringPiI2CWriteReg8(fd, 0x37, 0x22); //Enable the Magnetometer
+   fe = wiringPiI2CSetup(0x0C); //Start the compass
+   wiringPiI2CWriteReg8(fe, 0x0A, 0x01); //Trigger first Mag reading
+
   while (ros::ok()){
-    //http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html 
+    //http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
     //http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html
-    sensor_msgs::Imu data_imu;    
+    sensor_msgs::Imu data_imu;
     sensor_msgs::MagneticField data_mag;
 
     data_mag.header.stamp = ros::Time::now();
@@ -42,30 +47,40 @@ int main(int argc, char **argv){
     //datos acelerómetro
     InBuffer[0]=  (wiringPiI2CReadReg8 (fd, 0x3B)<<8)|wiringPiI2CReadReg8 (fd, 0x3C);
     InBuffer[1]=  (wiringPiI2CReadReg8 (fd, 0x3D)<<8)|wiringPiI2CReadReg8 (fd, 0x3E);
-    InBuffer[2]=  (wiringPiI2CReadReg8 (fd, 0x3F)<<8)|wiringPiI2CReadReg8 (fd, 0x40);   
-    
+    InBuffer[2]=  (wiringPiI2CReadReg8 (fd, 0x3F)<<8)|wiringPiI2CReadReg8 (fd, 0x40);
+
     data_imu.linear_acceleration.x = InBuffer[0]*conversion_acce;
     data_imu.linear_acceleration.y = InBuffer[1]*conversion_acce;
     data_imu.linear_acceleration.z = InBuffer[2]*conversion_acce;
 
      //datos giroscopio
     InBuffer[3]=  (wiringPiI2CReadReg8 (fd, 0x43)<<8)|wiringPiI2CReadReg8 (fd, 0x44);
     InBuffer[4]=  (wiringPiI2CReadReg8 (fd, 0x45)<<8)|wiringPiI2CReadReg8 (fd, 0x46);
-    InBuffer[5]=  (wiringPiI2CReadReg8 (fd, 0x47)<<8)|wiringPiI2CReadReg8 (fd, 0x48); 
+    InBuffer[5]=  (wiringPiI2CReadReg8 (fd, 0x47)<<8)|wiringPiI2CReadReg8 (fd, 0x48);
 
     data_imu.angular_velocity.x = InBuffer[3]*conversion_gyro;
     data_imu.angular_velocity.y = InBuffer[4]*conversion_gyro;
-    data_imu.angular_velocity.z = InBuffer[5]*conversion_gyro; 
+    data_imu.angular_velocity.z = InBuffer[5]*conversion_gyro;
 
     //datos magnetómetro
-    InBuffer[6]=  (wiringPiI2CReadReg8 (fd, 0x04)<<8)|wiringPiI2CReadReg8 (fd, 0x03);
-    InBuffer[7]=  (wiringPiI2CReadReg8 (fd, 0x06)<<8)|wiringPiI2CReadReg8 (fd, 0x05);
-    InBuffer[8]=  (wiringPiI2CReadReg8 (fd, 0x08)<<8)|wiringPiI2CReadReg8 (fd, 0x07);
-
+
+    uint8_t ST1;
+    wiringPiI2CWriteReg8(fe, 0x0A, 0x01);
+
+    do //wait until data has arrived
+    {
+     ST1=wiringPiI2CReadReg8(fe, 0x02);
+    } while (!(ST1 & 0x01));
+
+    wiringPiI2CWriteReg8(fe, 0x0A, 0x01);
+    InBuffer[6]=  (wiringPiI2CReadReg8 (fe, 0x04)<<8)|wiringPiI2CReadReg8 (fe, 0x03);
+    InBuffer[7]=  (wiringPiI2CReadReg8 (fe, 0x06)<<8)|wiringPiI2CReadReg8 (fe, 0x05);
+    InBuffer[8]=  (wiringPiI2CReadReg8 (fe, 0x08)<<8)|wiringPiI2CReadReg8 (fe, 0x07);
+
     data_mag.magnetic_field.x = InBuffer[6];
     data_mag.magnetic_field.y = InBuffer[7];
     data_mag.magnetic_field.z = InBuffer[8];
-    
+
     pub_imu.publish(data_imu);
 
     pub_mag.publish(data_mag);
@@ -74,3 +89,5 @@ int main(int argc, char **argv){
     }
   return 0;
  }
+
+