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