//// 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; }