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) { printk(KERN_DEBUG "\naccel cal (range : offset)\n"); for (i = 0; i < 3; i++) printk(KERN_DEBUG "%d : %d\n", accel_cal_data.range[i], accel_cal_data.offset[i]); } mpu_set_accel_bias(bias); use_accel_cal = 1; }
boolean MPU9150Lib::init(int mpuRate, int magMix) { struct int_param_s int_param; int result; long accelOffset[3]; m_magMix = magMix; m_lastDMPYaw = 0; m_lastYaw = 0; // get calibration data if it's there if (calSensRead(&m_calData)) { // use calibration data if it's there and wanted m_useMagCalibration &= m_calData.magValid; m_useAccelCalibration &= m_calData.accelValid; // Process calibration data for runtime if (m_useMagCalibration) { m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2); m_magXRange = m_calData.magMaxX - m_magXOffset; m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2); m_magYRange = m_calData.magMaxY - m_magYOffset; m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2); m_magZRange = m_calData.magMaxZ - m_magZOffset; } if (m_useAccelCalibration) { accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2; accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2; accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2; mpu_set_accel_bias(accelOffset); m_accelXRange = m_calData.accelMaxX + accelOffset[0]; m_accelYRange = m_calData.accelMaxY + accelOffset[1]; m_accelZRange = m_calData.accelMaxZ + accelOffset[2]; } } #ifdef MPULIB_DEBUG if (m_useMagCalibration) Serial.println("Using mag cal"); if (m_useAccelCalibration) Serial.println("Using accel cal"); #endif mpu_init_structures(); // Not using interrupts so set up this structure to keep the driver happy int_param.cb = NULL; int_param.pin = 0; int_param.lp_exit = 0; int_param.active_low = 1; result = mpu_init(&int_param); if (result != 0) { #ifdef MPULIB_DEBUG Serial.print("mpu_init failed with code: "); Serial.println(result); #endif return false; } mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // enable all of the sensors mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // get accel and gyro data in the FIFO also mpu_set_sample_rate(mpuRate); // set the update rate mpu_set_compass_sample_rate(mpuRate); // set the compass update rate to match #ifdef MPULIB_DEBUG Serial.println("Loading firmware"); #endif if ((result = dmp_load_motion_driver_firmware()) != 0) { // try to load the DMP firmware #ifdef MPULIB_DEBUG Serial.print("Failed to load dmp firmware: "); Serial.println(result); #endif return false; } dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct 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(mpuRate); if (mpu_set_dmp_state(1) != 0) { #ifdef MPULIB_DEBUG Serial.println("mpu_set_dmp_state failed"); #endif return false; } return true; }
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; }