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