Esempio n. 1
0
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;
}
Esempio n. 2
0
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;
}
Esempio n. 3
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;
}