int init(){ open_bus(); unsigned char whoami=0; i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &whoami); if(!silent_flag) { printf("WHO_AM_I: %x\n", whoami); } struct int_param_s int_param; if(!silent_flag) { printf("MPU init: %i\n", mpu_init(&int_param)); printf("MPU sensor init: %i\n", mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)); printf("MPU configure fifo: %i\n", mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)); printf("DMP firmware: %i\n ",dmp_load_motion_driver_firmware()); printf("DMP orientation: %i\n ",dmp_set_orientation( inv_orientation_matrix_to_scalar(gyro_orientation))); } unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL; if(!silent_flag) { printf("DMP feature enable: %i\n", dmp_enable_feature(dmp_features)); printf("DMP set fifo rate: %i\n", dmp_set_fifo_rate(DEFAULT_MPU_HZ)); printf("DMP enable %i\n", mpu_set_dmp_state(1)); } if(!no_interrupt_flag) { mpu_set_int_level(1); // Interrupt is low when firing dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Fire interrupt on new FIFO value } return 0; }
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; }
inv_error_t MPU9250_DMP::setIntLevel(unsigned char active_low) { return mpu_set_int_level(active_low); }
/* * Set Up Interrupt for IMU */ int setupDMPInt() { //Variables int status; XScuGic_Config* IntcConfig; XGpioPs_Config* GpioConfig; unsigned char data; //MPU and DMP //Configure MPU Interrupt Pin status = imuReadIntConfig(&data); //Read Register if (status != XST_SUCCESS) { return status; } //Modify Bits data &= (~INT_LEVEL_BIT); //Clear bit --> active high data &= (~INT_OPEN_BIT); //Clear bit --> push-pull data &= (~INT_RD_CLEAR_BIT); //Clear bit --> interrupt status bits are cleared only by reading INT_STATUS (Register 58) data |= LATCH_INT_EN_BIT; //Set bit --> INT pin is held high until the interrupt is cleared //Configure Int Pin status = imuConfigureInt(&data); if (status != XST_SUCCESS) { return status; } //Set Interrupt level status = mpu_set_int_level(0); //Set Interrupt for "active high" (0) if (status != XST_SUCCESS) { myprintf("mpu_int.c: Could not set interrupt level.\r\n"); return status; } //Enable Latched Interrupt status = imuSetIntLatched(1); if (status != XST_SUCCESS) { myprintf("mpu_int.c: Could not set latched interrupt.\r\n"); return status; } //Set DMP Interrupt Mode status = imuSetDmpIntMode(DMP_INT_CONTINUOUS); //Interrupt when one FIFO period has elapsed if (status != XST_SUCCESS) { myprintf("mpu_int.c: Could not set interrupt mode.\r\n"); return status; } //Initialize Xil Exceptions Xil_ExceptionInit(); //Initialize GPIO GpioConfig = XGpioPs_LookupConfig(GPIO_DEVICE_ID); status = XGpioPs_CfgInitialize(&Gpio, GpioConfig, GpioConfig->BaseAddr); if (status != XST_SUCCESS) { myprintf("mpu_utils.c: Error initializing GPIO Config.\r\n"); return status; } //Initialize GIC IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID); status = XScuGic_CfgInitialize(&Intc, IntcConfig, IntcConfig->CpuBaseAddress); if (status != XST_SUCCESS) { myprintf("mpu_utils.c: Error initializing SCU GIC Config.\r\n"); return status; } //Connect interrupt controller interrupt handler to HW interrupt handling logic in PS Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT, (Xil_ExceptionHandler) XScuGic_InterruptHandler, &Intc); //Connect driver handler (GIC) called when interrupt occurs to HW defined above XScuGic_Connect(&Intc, GPIO_INT_ID, (Xil_ExceptionHandler) myXGpioPs_IntrHandler, (void*) &Gpio); //Use my own Intr Handler //Enable Interrupt for Pin XGpioPs_SetIntrTypePin(&Gpio, GPIO_INT_PIN, XGPIOPS_IRQ_TYPE_EDGE_RISING); //Set Callback Handler for GPIO Interrupts XGpioPs_SetCallbackHandler(&Gpio, (void *) &Gpio, (XGpioPs_Handler) ImuIntrHandler); //Enable GPIO Interrupt for Pin XGpioPs_IntrEnablePin(&Gpio, GPIO_INT_PIN); if (!XGpioPs_IntrGetEnabledPin(&Gpio, GPIO_INT_PIN)) { myprintf("mpu_int.c: Interrupt not enabled.\r\n"); } //Enable Interrupts for GPIO XScuGic_Enable(&Intc, GPIO_INT_ID); //Enable Interrupts in Processor Xil_ExceptionEnableMask(XIL_EXCEPTION_IRQ); //Free memory return XST_SUCCESS; }