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)
{
// Initialize ROS
ros::init(argc, argv, "am_mpu9150_node");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1);
ros::Rate loop_rate(50);
sensor_msgs::Imu imu;
imu.header.frame_id = "imu";
geometry_msgs::Vector3Stamped euler;
euler.header.frame_id = "imu_euler";
// 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;
std::string accelFile;
std::string magFile;
// Parameters from ROS
ros::NodeHandle n_private("~");
int def_i2c_bus = 2;
n_private.param("i2cbus", i2c_bus, def_i2c_bus);
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS)
{
ROS_ERROR("am_mpu9150: Imu Bus problem");
return -1;
}
int def_sample_rate = 50;
n_private.param("samplerate", sample_rate, def_sample_rate);
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE)
{
ROS_ERROR("am_mpu9150: Sample rate problem");
return -1;
}
loop_rate = sample_rate;
int def_yaw_mix_factor = 0;
n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor);
if (yaw_mix_factor < 0 || yaw_mix_factor > 100)
{
ROS_ERROR("am_mpu9150: yawmixfactor problem");
return -1;
}
std::string defAccelFile = "./accelcal.txt";
n_private.param("accelfile", accelFile, defAccelFile);
std::string defMagFile = "./magcal.txt";
n_private.param("magfile", magFile, defMagFile);
unsigned long loop_delay;
mpudata_t mpu;
// Initialize the MPU-9150
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
{
ROS_ERROR("am_mpu9150::Failed init!");
exit(1);
}
//load_calibration(0, accelFile);
//load_calibration(1, magFile);
memset(&mpu, 0, sizeof(mpudata_t));
vector3d_t e;
quaternion_t q;
while (ros::ok())
{
std::stringstream ss;
if (mpu9150_read(&mpu) == 0)
{
// ROTATE The angles correctly...
quaternionToEuler(mpu.fusedQuat, e);
e[VEC3_Z] = -e[VEC3_Z];
eulerToQuaternion(e, q);
// FUSED
imu.header.stamp = ros::Time::now();
imu.orientation.x = q[QUAT_X];
imu.orientation.y = q[QUAT_Y];
imu.orientation.z = q[QUAT_Z];
imu.orientation.w = q[QUAT_W];
// GYRO
double gx = mpu.rawGyro[VEC3_X];
double gy = mpu.rawGyro[VEC3_Y];
double gz = mpu.rawGyro[VEC3_Z];
gx = gx * gyroScaleX;
gy = gy * gyroScaleY;
gz = gz * gyroScaleZ;
imu.angular_velocity.x = gx;
imu.angular_velocity.y = gy;
imu.angular_velocity.z = gz;
// ACCEL
imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
// EULER
euler.header.stamp = imu.header.stamp;
euler.vector.x = mpu.fusedEuler[VEC3_Y];
euler.vector.y = mpu.fusedEuler[VEC3_X];
euler.vector.z = -mpu.fusedEuler[VEC3_Z];
// Publish the messages
imu_pub.publish(imu);
imu_euler_pub.publish(euler);
//ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x);
}
ros::spinOnce();
loop_rate.sleep();
}
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;
}
