int mympu_open(unsigned int rate) {
mpu_select_device(0);
mpu_init_structures();
ret = mpu_init(NULL);
#ifdef MPU_DEBUG
if (ret) return 10+ret;
#endif
ret = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 20+ret;
#endif
ret = mpu_set_gyro_fsr(FSR);
#ifdef MPU_DEBUG
if (ret) return 30+ret;
#endif
ret = mpu_set_accel_fsr(2);
#ifdef MPU_DEBUG
if (ret) return 40+ret;
#endif
mpu_get_power_state((unsigned char *)&ret);
#ifdef MPU_DEBUG
if (!ret) return 50+ret;
#endif
ret = mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 60+ret;
#endif
dmp_select_device(0);
dmp_init_structures();
ret = dmp_load_motion_driver_firmware();
#ifdef MPU_DEBUG
if (ret) return 80+ret;
#endif
ret = dmp_set_fifo_rate(rate);
#ifdef MPU_DEBUG
if (ret) return 90+ret;
#endif
ret = mpu_set_dmp_state(1);
#ifdef MPU_DEBUG
if (ret) return 100+ret;
#endif
ret = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
// ret = dmp_enable_feature(DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
#ifdef MPU_DEBUG
if (ret) return 110+ret;
#endif
return 0;
}
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;
}
