void Read_Compass()
{
compass.readMag();
magnetom_x = SENSOR_SIGN[6] * compass.m.x;
magnetom_y = SENSOR_SIGN[7] * compass.m.y;
magnetom_z = SENSOR_SIGN[8] * compass.m.z;
}
/**
* @brief provides imu readings in a 50 Hz rate.
*
*/
void loop() {
if((millis()-timer)>=20) { // Main loop runs at 50Hz
timer=millis();
//Read data from the hardware
gyro.read();
compass.readAcc();
compass.readMag();
//Assign read data to the ros messages
imu_msg.angular_velocity.x=gyro.g.x;
imu_msg.angular_velocity.y=gyro.g.y;
imu_msg.angular_velocity.z=gyro.g.z;
imu_msg.linear_acceleration.x=compass.a.x;
imu_msg.linear_acceleration.y=compass.a.y;
imu_msg.linear_acceleration.z=compass.a.z;
mag_msg.magnetic_field.x=compass.m.x;
mag_msg.magnetic_field.y=compass.m.y;
mag_msg.magnetic_field.z=compass.m.z;
//Publish the data to the ros message system
imu_pub.publish( &imu_msg );
mag_pub.publish( &mag_msg);
nh.spinOnce();
}
nh.spinOnce();
}