int ms_open() {
dmpReady=1;
initialized = 0;
for (int i=0;i<DIM;i++){
lastval[i]=10;
}
// initialize device
printf("Initializing MPU...\n");
if (mpu_init(NULL) != 0) {
printf("MPU init failed!\n");
return -1;
}
printf("Setting MPU sensors...\n");
if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to set sensors!\n");
return -1;
}
printf("Setting GYRO sensitivity...\n");
if (mpu_set_gyro_fsr(2000)!=0) {
printf("Failed to set gyro sensitivity!\n");
return -1;
}
printf("Setting ACCEL sensitivity...\n");
if (mpu_set_accel_fsr(2)!=0) {
printf("Failed to set accel sensitivity!\n");
return -1;
}
// verify connection
printf("Powering up MPU...\n");
mpu_get_power_state(&devStatus);
printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);
//fifo config
printf("Setting MPU fifo...\n");
if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to initialize MPU fifo!\n");
return -1;
}
// load and configure the DMP
printf("Loading DMP firmware...\n");
if (dmp_load_motion_driver_firmware()!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
printf("Activating DMP...\n");
if (mpu_set_dmp_state(1)!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
//dmp_set_orientation()
//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Configuring DMP...\n");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
printf("Failed to enable DMP features!\n");
return -1;
}
printf("Setting DMP fifo rate...\n");
if (dmp_set_fifo_rate(rate)!=0) {
printf("Failed to set dmp fifo rate!\n");
return -1;
}
printf("Resetting fifo queue...\n");
if (mpu_reset_fifo()!=0) {
printf("Failed to reset fifo!\n");
return -1;
}
printf("Checking... ");
do {
delay_ms(1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate
r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
} while (r!=0 || fifoCount<5); //packtets!!!
printf("Done.\n");
initialized = 1;
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;
}
uint8_t MPU_Init(void)
{
GPIO_InitTypeDef gpio;
NVIC_InitTypeDef nvic;
EXTI_InitTypeDef exti;
int res=0;
IIC_Init();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
gpio.GPIO_Pin = GPIO_Pin_5;
gpio.GPIO_Mode = GPIO_Mode_IN;
gpio.GPIO_OType = GPIO_OType_PP;
gpio.GPIO_PuPd = GPIO_PuPd_UP;
gpio.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOB, &gpio);
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB,GPIO_PinSource5);
exti.EXTI_Line = EXTI_Line5;
exti.EXTI_Mode = EXTI_Mode_Interrupt;
exti.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿中断
exti.EXTI_LineCmd = ENABLE;
EXTI_Init(&exti);
nvic.NVIC_IRQChannel = EXTI9_5_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = ITP_MPU_EXTI9_5_PREEMPTION;
nvic.NVIC_IRQChannelSubPriority = ITP_MPU_EXTI9_5_SUB;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=mpu_set_int_level(1);
if(res)return 8;
res=dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
if(res)return 9;
res=run_self_test(); //自检
if(res)return 10;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 11;
}
else
return 12;
return 0;
}
// Private
int8_t IMUTask::MPUSetup()
{
// TODO: should check here to see if IMU is already setup and just init the lib
int8_t result = 0;
struct int_param_s int_param;
int_param.cb = IMU_interrupt;
int_param.pin = MPU_INT;
int_param.arg = FALLING;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned long timestamp;
result = mpu_init(&int_param);
debug::log("IMUTask: mpu_init returned " + String(result));
if (result != 0) {
return -1;
}
/* Wake up all sensors. */
result = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
debug::log("IMUTask: mpu_set_sensors returned " + String(result));
result = mpu_set_sample_rate(IMU_DEFAULT_MPU_HZ);
debug::log("IMUTask: mpu_set_sample_rate returned " + String(result));
debug::log("Pre laod");
result = dmp_load_motion_driver_firmware();
debug::log("IMUTask: dmp_load_motion_driver_firmware returned " + String(result));
if (result != 0) {
return -2;
}
result = dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation) );
debug::log("IMUTask: dmp_set_orientation returned " + String(result));
if (result != 0) {
return -3;
}
/*
* Known Bug -
* DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
* specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
* a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
* there will be a 25Hz interrupt from the MPU device.
*
* There is a known issue in which if you do not enable DMP_FEATURE_TAP
* then the interrupts will be at 200Hz even if fifo rate
* is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
*/
unsigned short dmp_features = DMP_FEATURE_TAP | // Don't remove this, see above
DMP_FEATURE_ANDROID_ORIENT; // we use this for the screen
// Disableing the rest for now as we dont yet have a use for them
// DMP_FEATURE_SEND_RAW_ACCEL |
// DMP_FEATURE_SEND_RAW_GYRO;
result = dmp_enable_feature(dmp_features);
if (result != 0) {
debug::log("IMUTask: mp_enable_feature returned " + String(result));
return -4;
}
result = dmp_set_fifo_rate(IMU_DEFAULT_MPU_HZ);
if (result != 0) {
debug::log("IMUTask: dmp_set_fifo_rate returned " + String(result));
return -5;
}
dmp_register_tap_cb(IMU_tap_cb);
dmp_register_android_orient_cb(IMU_android_orient_cb);
dmp_set_interrupt_mode(DMP_INT_GESTURE);
return 0;
}
boolean MPU9150Lib::init(int mpuRate)
{
uint8_t devStatus;
struct int_param_s int_param;
int result;
long accelOffset[3];
// get calibration data if it's there
m_lastDMPYaw = 0;
m_lastYaw = 0;
if (calLibRead(&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;
}
bool MPUManager::__StartDevice( bool p_bSelfTest )
{
if( 0 != mpu_init() )
{
printf("mpu_init failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
/* Wake up all sensors. */
if( 0 != mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_set_sensors failed\n");
return false;
}
/* Push both gyro and accel data into the FIFO. */
if( 0 != mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_configure_fifo failed\n");
return false;
}
if( 0 != mpu_set_sample_rate(m_usSampleRate) )
{
printf("mpu_set_sample_rate failed\n");
return false;
}
if( 0!= dmp_load_motion_driver_firmware() )
{
printf("dmp_load_motion_driver_firmware failed\n");
return false;
}
if( 0 != dmp_enable_feature(m_usDefaultFeatures) )
{
printf("dmp_enable_feature failed\n");
return false;
}
if( 0 != dmp_set_fifo_rate(m_usSampleRate) )
{
printf("dmp_set_fifo_rate failed\n");
return false;
}
if( 0 != mpu_get_gyro_sens( &m_fCurGyroSensitivity ) )
{
printf("mpu_get_gyro_sens failed\n");
return false;
}
else
{
printf("Current Gyro Sensitivity: %.2f\n", m_fCurGyroSensitivity);
}
if( 0 != mpu_get_accel_sens(&m_usCurAccelSensitivity) )
{
printf("mpu_get_accel_sens failed\n");
return false;
}
else
{
printf("Current Accel Sensitivity: %d\n", m_usCurAccelSensitivity);
}
m_usPackageLength = __dmp_get_packet_length();
unsigned short usfr = 0;
dmp_get_fifo_rate( &usfr );
printf("DMP start success\npackage length = %d\nFIFO rate = %d\n", m_usPackageLength, usfr);
if( p_bSelfTest )
{
if( __RunSelfTest() )
{
printf("Self test passed\n");
}
}
if( 0 != m_sGyroOffset[0] )
{
if( !__mpu_set_user_x_gyro_offset(m_sGyroOffset[0]) )
{
printf("set_user_x_gyro_offset %d failed\n", m_sGyroOffset[0]);
}
}
if( 0 != m_sGyroOffset[1] )
{
if( !__mpu_set_user_y_gyro_offset(m_sGyroOffset[1]) )
{
printf("set_user_y_gyro_offset %d failed\n", m_sGyroOffset[1]);
}
}
if( 0 != m_sGyroOffset[2] )
{
if( !__mpu_set_user_z_gyro_offset(m_sGyroOffset[2]) )
{
printf("set_user_z_gyro_offset %d failed\n", m_sGyroOffset[2]);
}
}
if( 0 != m_sAccelOffset[0] )
{
if( !__mpu_set_x_accel_offset(m_sAccelOffset[0]) )
{
printf("mpu_set_x_accel_offset %d failed\n", m_sAccelOffset[0]);
}
}
if( 0 != m_sAccelOffset[1] )
{
if( !__mpu_set_y_accel_offset(m_sAccelOffset[1]) )
{
printf("mpu_set_y_accel_offset %d failed\n", m_sAccelOffset[1]);
}
}
if( 0 != m_sAccelOffset[2] )
{
if( !__mpu_set_z_accel_offset(m_sAccelOffset[2]) )
{
printf("mpu_set_z_accel_offset %d failed\n", m_sAccelOffset[2]);
}
}
if( 0 != mpu_set_dmp_state(1) )
{
printf("mpu_set_dmp_state 1 failed\n");
return false;
}
if( 0 == mpu_set_lpf(42) )
{
printf("mpu_set_lpf failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
return true;
}
