int init_mpu(int sample_rate, int yaw_mix_factor)
{
int ret;
verbose = 1;
i2c_bus = DEFAULT_I2C_BUS;
// sample_rate = DEFAULT_SAMPLE_RATE_HZ;
// yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
mpu9150_set_debug(verbose);
if ((ret = mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor)) != 0) {
return ret;
}
set_cal(0, NULL);
set_cal(1, NULL);
return ret;
}
int main(int argc, char **argv)
{
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;
MQTT_init();
while ((opt = getopt(argc, argv, "b:s:y:a:m:vh")) != -1) {
switch (opt) {
case 'b':
i2c_bus = strtoul(optarg, NULL, 0);
if (errno == EINVAL)
usage(argv[0]);
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS)
usage(argv[0]);
break;
case 's':
sample_rate = strtoul(optarg, NULL, 0);
if (errno == EINVAL)
usage(argv[0]);
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE)
usage(argv[0]);
break;
case 'y':
yaw_mix_factor = strtoul(optarg, NULL, 0);
if (errno == EINVAL)
usage(argv[0]);
if (yaw_mix_factor < 0 || yaw_mix_factor > 100)
usage(argv[0]);
break;
case 'a':
len = 1 + strlen(optarg);
accel_cal_file = (char *)malloc(len);
if (!accel_cal_file) {
perror("malloc");
exit(1);
}
strcpy(accel_cal_file, optarg);
break;
case 'm':
len = 1 + strlen(optarg);
mag_cal_file = (char *)malloc(len);
if (!mag_cal_file) {
perror("malloc");
exit(1);
}
strcpy(mag_cal_file, optarg);
break;
case 'v':
verbose = 1;
break;
case 'h':
default:
usage(argv[0]);
break;
}
}
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);
read_loop(sample_rate);
mpu9150_exit();
MQTTAsync_destroy(&client);
return 0;
}
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;
}