void read_loop(unsigned int sample_rate)
{
unsigned long loop_delay;
mpudata_t mpu; /*
typedef struct {
short rawGyro[3];
short rawAccel[3];
long rawQuat[4];
unsigned long dmpTimestamp;
short rawMag[3];
unsigned long magTimestamp;
short Temp[3];
short calibratedAccel[3];
short calibratedMag[3];
quaternion_t fusedQuat;
vector3d_t fusedEuler;
float lastDMPYaw;
float lastYaw;
} mpudata_t;*/
memset(&mpu, 0, sizeof(mpudata_t));
if (sample_rate == 0)
return;
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
while (!done) {
while(msg_cnt<MPU_MSG_NUM && !done){
if (mpu9150_read(&mpu) == 0) {
mpu_add_msg(&mpu);
// print_fused_euler_angles(&mpu);
print_fused_quaternions(&mpu);
// print_calibrated_accel(&mpu);
// print_calibrated_mag(&mpu);
}
linux_delay_ms(loop_delay);
}
msg_cnt=0;
publish(mpu_msg);
//publish(msg);
}
printf("\n\n");
}
void read_loop(unsigned int sample_rate)
{
int i, change;
unsigned long loop_delay;
mpudata_t mpu;
if (sample_rate == 0)
return;
memset(&mpu, 0, sizeof(mpudata_t));
for (i = 0; i < 3; i++) {
minVal[i] = 0x7fff;
maxVal[i] = 0x8000;
}
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
while (!done) {
change = 0;
if (mag_mode) {
if (mpu9150_read_mag(&mpu) == 0) {
for (i = 0; i < 3; i++) {
if (mpu.rawMag[i] < minVal[i]) {
minVal[i] = mpu.rawMag[i];
change = 1;
}
if (mpu.rawMag[i] > maxVal[i]) {
maxVal[i] = mpu.rawMag[i];
change = 1;
}
}
}
}
else {
if (mpu9150_read_dmp(&mpu) == 0) {
for (i = 0; i < 3; i++) {
if (mpu.rawAccel[i] < minVal[i]) {
minVal[i] = mpu.rawAccel[i];
change = 1;
}
if (mpu.rawAccel[i] > maxVal[i]) {
maxVal[i] = mpu.rawAccel[i];
change = 1;
}
}
}
}
if (change) {
if (mag_mode)
print_mag(&mpu);
else
print_accel(&mpu);
}
linux_delay_ms(loop_delay);
}
printf("\n\n");
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mpu9150_node");
ros::NodeHandle n;
//creates publisher of IMU message
ros::Publisher pub = n.advertise<sensor_msgs::Imu>("imu/data_raw", 1000);
//creates publisher of Magnetic FIeld message
ros::Publisher pubM = n.advertise<sensor_msgs::MagneticField>("imu/mag", 1000);
ros::Rate loop_rate(10);
/* Init the sensor the values are hardcoded at the local_defaults.h file */
int opt, len;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
int verbose = 0;
char *mag_cal_file = NULL;
char *accel_cal_file = NULL;
unsigned long loop_delay;
//creates object of mpudata_t
mpudata_t mpu;
// Initialize the MPU-9150
register_sig_handler();
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
exit(1);
set_cal(0, accel_cal_file);
set_cal(1, mag_cal_file);
if (accel_cal_file)
free(accel_cal_file);
if (mag_cal_file)
free(mag_cal_file);
if (sample_rate == 0)
return -1;
// ROS loop config
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering MPU read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
while (ros::ok())
{
//creates objects of each message type
//IMU Message
sensor_msgs::Imu msgIMU;
std_msgs::String header;
geometry_msgs::Quaternion orientation;
geometry_msgs::Vector3 angular_velocity;
geometry_msgs::Vector3 linear_acceleration;
//magnetometer message
sensor_msgs::MagneticField msgMAG;
geometry_msgs::Vector3 magnetic_field;
//modified to output in the format of IMU message
if (mpu9150_read(&mpu) == 0) {
//IMU Message
//sets up header for IMU message
msgIMU.header.seq = count;
msgIMU.header.stamp.sec = ros::Time::now().toSec();
msgIMU.header.frame_id = "/base_link";
//adds data to the sensor message
//orientation
msgIMU.orientation.x = mpu.fusedQuat[QUAT_X];
msgIMU.orientation.y = mpu.fusedQuat[QUAT_Y];
msgIMU.orientation.z = mpu.fusedQuat[QUAT_Z];
msgIMU.orientation.w = mpu.fusedQuat[QUAT_W];
//msgIMU.orientation_covariance[0] =
//angular velocity
msgIMU.angular_velocity.x = mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE;
msgIMU.angular_velocity.y = mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE;
msgIMU.angular_velocity.z = mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
//msgIMU.angular_velocity_covariance[] =
//linear acceleration
msgIMU.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
msgIMU.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
msgIMU.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
//msgIMU.linear_acceleration_covariance[] =
//Magnetometer Message
//sets up header
msgMAG.header.seq = count;
msgMAG.header.stamp.sec = ros::Time::now().toSec();
msgMAG.header.frame_id = "/base_link";
//adds data to magnetic field message
msgMAG.magnetic_field.x = mpu.calibratedMag[VEC3_X];
msgMAG.magnetic_field.y = mpu.calibratedMag[VEC3_Y];
msgMAG.magnetic_field.z = mpu.calibratedMag[VEC3_Z];
//fills the list with zeros as per message spec when no covariance is known
//msgMAG.magnetic_field_covariance[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
}
//publish both messages
pub.publish(msgIMU);
pubM.publish(msgMAG);
ros::spinOnce();
loop_rate.sleep();
++count;
}
mpu9150_exit();
return 0;
}
