//// MPU9150 IMU ////
int initialize_imu(int sample_rate, signed char orientation[9]){
printf("Initializing IMU\n");
//set up gpio interrupt pin connected to imu
if(gpio_export(INTERRUPT_PIN)){
printf("can't export gpio %d \n", INTERRUPT_PIN);
return (-1);
}
gpio_set_dir(INTERRUPT_PIN, INPUT_PIN);
gpio_set_edge(INTERRUPT_PIN, "falling"); // Can be rising, falling or both
linux_set_i2c_bus(1);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mpu_set_sample_rate(sample_rate);
// compass run at 100hz max.
if(sample_rate > 100){
// best to sample at a fraction of the gyro/accel
mpu_set_compass_sample_rate(sample_rate/2);
}
else{
mpu_set_compass_sample_rate(sample_rate);
}
mpu_set_lpf(188); //as little filtering as possible
dmp_load_motion_driver_firmware(sample_rate);
dmp_set_orientation(inv_orientation_matrix_to_scalar(orientation));
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(sample_rate);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
if(loadGyroCalibration()){
printf("\nGyro Calibration File Doesn't Exist Yet\n");
printf("Use calibrate_gyro example to create one\n");
printf("Using 0 offset for now\n");
};
// set the imu_interrupt_thread to highest priority since this is used
// for real-time control with time-sensitive IMU data
set_imu_interrupt_func(&null_func);
pthread_t imu_interrupt_thread;
struct sched_param params;
pthread_create(&imu_interrupt_thread, NULL, imu_interrupt_handler, (void*) NULL);
params.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_setschedparam(imu_interrupt_thread, SCHED_FIFO, ¶ms);
return 0;
}
int mpulib_init(int i2c_bus, int sample_rate, int mix_factor) {
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) {
printf("Invalid I2C bus %d\n", i2c_bus);
return -1;
}
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
int compass_sample_rate = sample_rate;
if( compass_sample_rate > MAX_COMPASS_SAMPLE_RATE ) {
compass_sample_rate = MAX_COMPASS_SAMPLE_RATE;
}
if (mpu_set_compass_sample_rate(compass_sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
int INVMPU9150::init(int i2cBus, int frequency) {
// Set linux i2c bus
linux_set_i2c_bus(i2cBus);
// The gyro orientation
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
// Init MPU
if (mpu_init(NULL)) {
return MPU9150_INIT_MPU_INIT_ERROR;
}
// Set sensors
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_SET_SENSORS_ERROR;
}
// Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)]
float gyroSsfDegreePerSecond;
if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) {
return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR;
}
gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI;
// Get accel sensitivity scale factor
if (mpu_get_accel_sens(& accelSsf)) {
return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR;
}
// Configure FIFO
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR;
}
// Set sample rate for gyro/acc
if (mpu_set_sample_rate(frequency)) {
return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR;
}
// Load motion driver firmare (DMP)
if (dmp_load_motion_driver_firmware()) {
return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR;
}
// Set gyro orientation
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR;
}
// Enable DMP features
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR;
}
// Set sample rate for DMP
if (dmp_set_fifo_rate(frequency)) {
return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR;
}
// Enable DMP
if (mpu_set_dmp_state(1)) {
return MPU9150_INIT_DMP_SET_STATE_ERROR;
}
return MPU9150_OK;
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#endif
printf(".");
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
if (mpu_set_sample_rate(200)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_compass_sample_rate(50)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < 0 || i2c_bus > 3)
return -1;
if (sample_rate < 2 || sample_rate > 50)
return -1;
if (mix_factor < 0 || mix_factor > 100)
return -1;
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#endif
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_compass_sample_rate(sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
/* New functions to enable / disable 6axis on the fly */
int enableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure enabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion enabled\n");
return 0;
}
int disableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure disabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion disabled\n");
return 0;
}
void mpu9150_exit()
{
// turn off the DMP on exit
if (mpu_set_dmp_state(0))
printf("mpu_set_dmp_state(0) failed\n");
// TODO: Should turn off the sensors too
}
void mpu9150_set_accel_cal(caldata_t *cal)
{
int i;
long bias[3];
if (!cal) {
use_accel_cal = 0;
return;
}
memcpy(&accel_cal_data, cal, sizeof(caldata_t));
for (i = 0; i < 3; i++) {
if (accel_cal_data.range[i] < 1)
accel_cal_data.range[i] = 1;
else if (accel_cal_data.range[i] > ACCEL_SENSOR_RANGE)
accel_cal_data.range[i] = ACCEL_SENSOR_RANGE;
bias[i] = -accel_cal_data.offset[i];
}
if (debug_on) {
printf("\naccel cal (range : offset)\n");
for (i = 0; i < 3; i++)
printf("%d : %d\n", accel_cal_data.range[i], accel_cal_data.offset[i]);
}
mpu_set_accel_bias(bias);
use_accel_cal = 1;
}
int mpu9250_init(int i2c_bus, int sample_rate)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) {
printf("Invalid I2C bus %d\n", i2c_bus);
return -1;
}
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
//*******Work around in mpu_init werden sensoren gelöscht, müssen aber vorher != 0 sein!!
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
//**************
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
// if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
// printf("\nmpu_configure_fifo() failed\n");
// return -1;
// }
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
// if (dmp_load_motion_driver_firmware()) {
// printf("\ndmp_load_motion_driver_firmware() failed\n");
// return -1;
// }
//
// if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
// printf("\ndmp_set_orientation() failed\n");
// return -1;
// }
//
// if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
// | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
// printf("\ndmp_enable_feature() failed\n");
// return -1;
// }
//
// if (dmp_set_fifo_rate(sample_rate)) {
// printf("\ndmp_set_fifo_rate() failed\n");
// return -1;
// }
//
// if (mpu_set_dmp_state(1)) {
// printf("\nmpu_set_dmp_state(1) failed\n");
// return -1;
// }
printf(" done\n\n");
return 0;
}
