static int
configure(int type, int c)
{
mpu9150_init();
mpu9150_start();
return 1;
}
void auto_init_mpu9150(void)
{
assert(MPU9150_NUM == MPU9150_INFO_NUM);
for (unsigned int i = 0; i < MPU9150_NUM; i++) {
LOG_DEBUG("[auto_init_saul] initializing mpu9150 #%u\n", i);
if (mpu9150_init(&mpu9150_devs[i], &mpu9150_params[i]) < 0) {
LOG_ERROR("[auto_init_saul] error initializing mpu9150 #%u\n", i);
continue;
}
saul_entries[(i * 3)].dev = &(mpu9150_devs[i]);
saul_entries[(i * 3)].name = mpu9150_saul_info[i].name;
saul_entries[(i * 3)].driver = &mpu9150_saul_acc_driver;
saul_entries[(i * 3) + 1].dev = &(mpu9150_devs[i]);
saul_entries[(i * 3) + 1].name = mpu9150_saul_info[i].name;
saul_entries[(i * 3) + 1].driver = &mpu9150_saul_gyro_driver;
saul_entries[(i * 3) + 2].dev = &(mpu9150_devs[i]);
saul_entries[(i * 3) + 2].name = mpu9150_saul_info[i].name;
saul_entries[(i * 3) + 2].driver = &mpu9150_saul_mag_driver;
saul_reg_add(&(saul_entries[(i * 3)]));
saul_reg_add(&(saul_entries[(i * 3) + 1]));
saul_reg_add(&(saul_entries[(i * 3) + 2]));
}
}
void mpu_init(void) {
ret_code_t ret_code;
// Initiate MPU9150 driver with TWI instance handler
ret_code = mpu9150_init(&m_twi_instance);
APP_ERROR_CHECK(ret_code); // Check for errors in return value
// Setup and configure the MPU9150 with intial values
mpu9150_config_t p_mpu_config = MPU9150_DEFAULT_CONFIG(); // Load default values
p_mpu_config.smplrt_div = 19; // Change sampelrate. Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV). 19 gives a sample rate of 50Hz
p_mpu_config.accel_config.afs_sel = AFS_2G; // Set accelerometer full scale range to 2G
ret_code = mpu9150_config(&p_mpu_config); // Configure the MPU9150 with above values
APP_ERROR_CHECK(ret_code); // Check for errors in return value
// This is a way to configure the interrupt pin behaviour
// NOT YET IMPLEMENTED IN THIS EXAMPLE. YOU CAN DO IT YOURSELF BUT YOU WILL HAVE TO SET UP THE NRF51 INTERRUPT PINS AND IRQ ROUTINES YOURSELF
// mpu9150_int_pin_cfg_t p_int_pin_cfg = MPU9150_DEFAULT_INT_PIN_CONFIG(); // Default configurations
// p_int_pin_cfg.int_rd_clear = 1; // When this bit is equal to 1, interrupt status bits are cleared on any read operation
// ret_code = mpu9150_int_cfg_pin(&p_int_pin_cfg); // Configure pin behaviour
// APP_ERROR_CHECK(ret_code); // Check for errors in return value
//
// // Enable the MPU interrupts
// mpu9150_int_enable_t p_int_enable = MPU9150_DEFAULT_INT_ENABLE_CONFIG();
// p_int_enable.data_rdy_en = 1; // Trigger interrupt everytime new sensor values are available
// ret_code = mpu9150_int_enable(&p_int_enable); // Configure interrupts
// APP_ERROR_CHECK(ret_code); // Check for errors in return value
}
int main (int argc, char **argv) {
t_mpu9150 accel;
mpu9150_open(&accel);
mpu9150_init(&accel);
mpu9150_open_mag(&accel);
mpu9150_init_mag(&accel);
mpu9150_start_mag(&accel);
usleep(10000);
while(1) {
mpu9150_read_data(&accel);
mpu9150_read_mag(&accel);
printf("\r\t%f\t%f\t%f\t|\t%f\t%f\t%f\t|\t%f\t%f\t%f\t",accel.acc[0],accel.acc[1],accel.acc[2],accel.gyr[0],accel.gyr[1],accel.gyr[2],accel.mag[0],accel.mag[1],accel.mag[2]);
usleep(10000);
}
return 0;
}
msg_t Thread_mpu9150_int(void* arg) {
(void) arg;
static const evhandler_t evhndl_mpu9150[] = {
mpu9150_int_event_handler
};
struct EventListener evl_mpu9150;
chRegSetThreadName("mpu9150_int");
mpu9150_init(mpu9150_driver.i2c_instance);
chEvtRegister(&mpu9150_int_event, &evl_mpu9150, 0);
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
return -1;
}
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(void)
{
mpu9150_t dev;
mpu9150_results_t measurement;
int32_t temperature;
int result;
puts("MPU-9150 test application\n");
printf("+------------Initializing------------+\n");
result = mpu9150_init(&dev, TEST_I2C, TEST_HW_ADDR, TEST_COMP_ADDR);
if (result == -1) {
puts("[Error] The given i2c is not enabled");
return 1;
}
else if (result == -2) {
puts("[Error] The compass did not answer correctly on the given address");
return 1;
}
mpu9150_set_sample_rate(&dev, 200);
if (dev.conf.sample_rate != 200) {
puts("[Error] The sample rate was not set correctly");
return 1;
}
mpu9150_set_compass_sample_rate(&dev, 100);
if (dev.conf.compass_sample_rate != 100) {
puts("[Error] The compass sample rate was not set correctly");
return 1;
}
printf("Initialization successful\n\n");
printf("+------------Configuration------------+\n");
printf("Sample rate: %"PRIu16" Hz\n", dev.conf.sample_rate);
printf("Compass sample rate: %"PRIu8" Hz\n", dev.conf.compass_sample_rate);
printf("Gyro full-scale range: 2000 DPS\n");
printf("Accel full-scale range: 2 G\n");
printf("Compass X axis factory adjustment: %"PRIu8"\n", dev.conf.compass_x_adj);
printf("Compass Y axis factory adjustment: %"PRIu8"\n", dev.conf.compass_y_adj);
printf("Compass Z axis factory adjustment: %"PRIu8"\n", dev.conf.compass_z_adj);
printf("\n+--------Starting Measurements--------+\n");
while (1) {
/* Get accel data in milli g */
mpu9150_read_accel(&dev, &measurement);
printf("Accel data [milli g] - X: %"PRId16" Y: %"PRId16" Z: %"PRId16"\n",
measurement.x_axis, measurement.y_axis, measurement.z_axis);
/* Get gyro data in dps */
mpu9150_read_gyro(&dev, &measurement);
printf("Gyro data [dps] - X: %"PRId16" Y: %"PRId16" Z: %"PRId16"\n",
measurement.x_axis, measurement.y_axis, measurement.z_axis);
/* Get compass data in mikro Tesla */
mpu9150_read_compass(&dev, &measurement);
printf("Compass data [mikro T] - X: %"PRId16" Y: %"PRId16" Z: %"PRId16"\n",
measurement.x_axis, measurement.y_axis, measurement.z_axis);
/* Get temperature in milli degrees celsius */
mpu9150_read_temperature(&dev, &temperature);
printf("Temperature [milli deg] : %"PRId32"\n", temperature);
printf("\n+-------------------------------------+\n");
xtimer_usleep(SLEEP);
}
return 0;
}
int main(int argc, char **argv)
{
int opt;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
mag_mode = -1;
memset(calFile, 0, sizeof(calFile));
while ((opt = getopt(argc, argv, "b:s:y:amh")) != -1) {
switch (opt) {
case 'b':
i2c_bus = strtoul(optarg, NULL, 0);
if (errno == EINVAL)
usage(argv[0]);
if (i2c_bus < 0 || i2c_bus > 5)
usage(argv[0]);
break;
case 's':
sample_rate = strtoul(optarg, NULL, 0);
if (errno == EINVAL)
usage(argv[0]);
if (sample_rate < 2 || sample_rate > 50)
usage(argv[0]);
break;
case 'y':
if (strlen(optarg) >= sizeof(calFile)) {
printf("Choose a shorter path for the cal file\n");
usage(argv[0]);
}
strcpy(calFile, optarg);
break;
case 'a':
if (mag_mode != -1)
usage(argv[0]);
mag_mode = 0;
break;
case 'm':
if (mag_mode != -1)
usage(argv[0]);
mag_mode = 1;
break;
case 'h':
default:
usage(argv[0]);
break;
}
}
if (mag_mode == -1)
usage(argv[0]);
register_sig_handler();
if (mpu9150_init(i2c_bus, sample_rate, 0))
exit(1);
read_loop(sample_rate);
if (strlen(calFile) == 0) {
if (mag_mode)
strcpy(calFile, "magcal.txt");
else
strcpy(calFile, "accelcal.txt");
}
write_cal();
mpu9150_exit();
return 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;
}
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(void) {
static const evhandler_t evhndl_main[] = {
extdetail_WKUP_button_handler
};
struct EventListener el0;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
extdetail_init();
palSetPad(GPIOC, GPIOC_LED);
/*!
* GPIO Pins for generating pulses at data input detect and data output send.
* Used for measuring latency timing of data
*
* \sa board.h
*/
palClearPad( TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
palSetPadMode(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad( TIMEINPUT_PORT, TIMEINPUT_PIN);
palSetPadMode(TIMEINPUT_PORT, TIMEINPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL );
/*
* I2C2 I/O pins setup
*/
palSetPadMode(si_i2c_connections.i2c_sda_port , si_i2c_connections.i2c_sda_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST |PAL_STM32_PUDR_FLOATING );
palSetPadMode(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING);
palSetPad(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad );
static const ShellCommand commands[] = {
{"mem", cmd_mem},
{"threads", cmd_threads},
{NULL, NULL}
};
usbSerialShellStart(commands);
mpu9150_start(&I2CD2);
i2cStart(mpu9150_driver.i2c_instance, &si_i2c_config);
mpu9150_init(mpu9150_driver.i2c_instance);
/* Administrative threads */
chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL);
chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL);
/* MAC */
/*!
* Use a locally administered MAC address second LSbit of MSB of MAC should be 1
* Use unicast address LSbit of MSB of MAC should be 0
*/
data_udp_init();
chThdCreateStatic(wa_lwip_thread , sizeof(wa_lwip_thread) , NORMALPRIO + 2, lwip_thread , SENSOR_LWIP);
chThdCreateStatic(wa_data_udp_send_thread , sizeof(wa_data_udp_send_thread) , NORMALPRIO , data_udp_send_thread , NULL);
/* i2c MPU9150 */
chThdCreateStatic(waThread_mpu9150_int, sizeof(waThread_mpu9150_int) , NORMALPRIO , Thread_mpu9150_int, NULL);
/* SPI ADIS */
//chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL);
//chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);
/*! Activates the EXT driver 1. */
extStart(&EXTD1, &extcfg);
chEvtRegister(&extdetail_wkup_event, &el0, 0);
while (TRUE) {
chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
}
}
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;
}
