/************************************************************************** 函数功能:MPU6050内置DMP的初始化 入口参数:无 返回 值:无 作 者:平衡小车之家 **************************************************************************/ void DMP_Init(void) { u8 temp[1]={0}; i2cRead(0x68,0x75,1,temp); printf("mpu_set_sensor complete ......\r\n"); if(temp[0]!=0x68)NVIC_SystemReset(); if(!mpu_init()) { if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_set_sensor complete ......\r\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) printf("mpu_configure_fifo complete ......\r\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) printf("mpu_set_sample_rate complete ......\r\n"); if(!dmp_load_motion_driver_firmware()) printf("dmp_load_motion_driver_firmware complete ......\r\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) printf("dmp_set_orientation complete ......\r\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) printf("dmp_enable_feature complete ......\r\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) printf("dmp_set_fifo_rate complete ......\r\n"); run_self_test(); if(!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\r\n"); } }
void init_mpu6050(struct eeprom *e) { int result=0; die_if(eeprom_open("/dev/i2c/0", 0x68, EEPROM_TYPE_8BIT_ADDR, e) < 0, "unable to open MPU6050 device file "); result=mpu_init(); if(!result) { printf("mpu initialization complete......\n "); //mpu initialization complete if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor printf("mpu_set_sensor complete ......\n"); else printf("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo printf("mpu_configure_fifo complete ......\n"); else printf("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(50)) //mpu_set_sample_rate printf("mpu_set_sample_rate complete ......\n"); else printf("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare printf("dmp_load_motion_driver_firmware complete ......\n"); else printf("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation printf("dmp_set_orientation complete ......\n"); else printf("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature printf("dmp_enable_feature complete ......\n"); else printf("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(50)) //dmp_set_fifo_rate printf("dmp_set_fifo_rate complete ......\n"); else printf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) printf("mpu_set_dmp_state complete ......\n"); else printf("mpu_set_dmp_state come across error ......\n"); } else { printf("mpu INIT INIT INIT error ......\r\n"); } }
/** * @brief mpu6050_init * @param void * @retval 成功为0 * @note 初始化mpu6050 频率为100*1000HZ */ int16_t mpu6050_init(void) { I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000); int result=0; result=mpu_init(); if(!result) { PrintChar("mpu initialization complete......\n "); //mpu initialization complete if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor PrintChar("mpu_set_sensor complete ......\n"); else PrintChar("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo PrintChar("mpu_configure_fifo complete ......\n"); else PrintChar("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate PrintChar("mpu_set_sample_rate complete ......\n"); else PrintChar("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare PrintChar("dmp_load_motion_driver_firmware complete ......\n"); else PrintChar("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation PrintChar("dmp_set_orientation complete ......\n"); else PrintChar("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature PrintChar("dmp_enable_feature complete ......\n"); else PrintChar("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate PrintChar("dmp_set_fifo_rate complete ......\n"); else PrintChar("dmp_set_fifo_rate come across error ......\n"); run_self_test(); //自检 if(!mpu_set_dmp_state(1)) PrintChar("mpu_set_dmp_state complete ......\n"); else PrintChar("mpu_set_dmp_state come across error ......\n"); } return 0; }
int main(int argc, char** argv) { if (argc > 1) { if (strcmp(argv[1], "--help") == 0) { printf(USAGE_MSG); return 0; } else if (strcmp(argv[1], "--version") == 0) { printf(VERSION_MSG); return 0; } else if (strcmp(argv[1], "--selftest") == 0) { self_test = true; } else { printf("unrecognized option: %s\n", argv[1]); printf(HELP_MSG); return 1; } } init_log(); if (!self_test) { userlog(LOG_INFO, "started (v." VERSION ")"); } else { userlog(LOG_INFO, "started (self-test mode) (v." VERSION ")"); } setvbuf(stdin, NULL, _IONBF, 0); setvbuf(stdout, NULL, _IONBF, 0); roots = array_create(20); if (init_inotify() && roots != NULL) { set_inotify_callback(&inotify_callback); if (!self_test) { main_loop(); } else { run_self_test(); } unregister_roots(); } else { printf("GIVEUP\n"); } close_inotify(); array_delete(roots); userlog(LOG_INFO, "finished"); closelog(); return 0; }
/**********************************MPU6050*************************************/ void MPU6050_INIT() { mpu_init(); mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//#(INV_XYZ_GYRO | INV_XYZ_ACCEL)在inv_mpu.h定义 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); mpu_set_sample_rate(DEFAULT_MPU_HZ);//#本文件开头定义 dmp_load_motion_driver_firmware(); dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//#本文定义 dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL;//#全部在(inv_mpu_dmp_motion_driver.h)中定义 dmp_enable_feature(dmp_features); dmp_set_fifo_rate(DEFAULT_MPU_HZ); run_self_test(); mpu_set_dmp_state(1); }
/** **************************************************************************************** * @brief start mpu6050 dmp * @description * This function is used to initialize mpu6050 dmp. ***************************************************************************************** */ void mpu_start() { int result = mpu_init(NULL); while(result != MPU_OK) { result = mpu_init(NULL); } if(result == MPU_OK) { printf("mpu initialization complete......\n "); //mpu_set_sensor if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) printf("mpu_set_sensor complete ......\n"); else printf("mpu_set_sensor come across error ......\n"); if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) //mpu_configure_fifo printf("mpu_configure_fifo complete ......\n"); else printf("mpu_configure_fifo come across error ......\n"); if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK) //mpu_set_sample_rate printf("mpu_set_sample_rate complete ......\n"); else printf("mpu_set_sample_rate error ......\n"); if(dmp_load_motion_driver_firmware() == MPU_OK) //dmp_load_motion_driver_firmvare printf("dmp_load_motion_driver_firmware complete ......\n"); else printf("dmp_load_motion_driver_firmware come across error ......\n"); if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK) //dmp_set_orientation printf("dmp_set_orientation complete ......\n"); else printf("dmp_set_orientation come across error ......\n"); if(dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL) == MPU_OK) //dmp_enable_feature printf("dmp_enable_feature complete ......\n"); else printf("dmp_enable_feature come across error ......\n"); if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK) //dmp_set_fifo_rate printf("dmp_set_fifo_rate complete ......\n"); else printf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); if(mpu_set_dmp_state(1) == MPU_OK) printf("mpu_set_dmp_state complete ......\n"); else printf("mpu_set_dmp_state come across error ......\n"); } }
void dmp_init() { Timer4_init(); while (1 == mpu_init()); //mpu_set_sensor while (1 == mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)); //mpu_configure_fifo while (1 == mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)); //mpu_set_sample_rate while (1 == mpu_set_sample_rate(DEFAULT_MPU_HZ)); //dmp_load_motion_driver_firmvare while (1 == dmp_load_motion_driver_firmware()); //dmp_set_orientation while (1 == dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))); //dmp_enable_feature while (1 == dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)); //dmp_set_fifo_rate while (1 == dmp_set_fifo_rate(DEFAULT_MPU_HZ)); run_self_test(); while (1 == mpu_set_dmp_state(1)); rt_kprintf("start mpu6050\n"); rt_kprintf("start ahrs\n"); }
int MPU_DMP_Init() { int result = 0; //mpu_init(); printf("mpu initialization complete......\n "); //mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("mpu_set_sensor complete ......\n"); } else { printf("mpu_set_sensor come across error ......\n"); result |=0x0001; } //mpu_configure_fifo if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) { printf("mpu_configure_fifo complete ......\n"); } else { printf("mpu_configure_fifo come across error ......\n"); result |=0x0002; } //mpu_set_sample_rate if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) { printf("mpu_set_sample_rate complete ......\n"); } else { printf("mpu_set_sample_rate error ......\n"); result |=0x0004; } //dmp_load_motion_driver_firmvare if(!dmp_load_motion_driver_firmware()) { printf("dmp_load_motion_driver_firmware complete ......\n"); } else { printf("dmp_load_motion_driver_firmware come across error ......\n"); result |=0x0008; } //dmp_set_orientation if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) { printf("dmp_set_orientation complete ......\n"); } else { printf("dmp_set_orientation come across error ......\n"); result |=0x0010; } //dmp_enable_feature if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | //DMP_FEATURE_SEND_RAW_GYRO | // DMP_FEATURE_ANDROID_ORIENT | //By Damm Stagner DMP_FEATURE_GYRO_CAL)) { printf("dmp_enable_feature complete ......\n"); } else { printf("dmp_enable_feature come across error ......\n"); result |=0x0020; } //dmp_set_fifo_rate if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) { printf("dmp_set_fifo_rate complete ......\n"); } else { printf("dmp_set_fifo_rate come across error ......\n"); result |=0x0040; } run_self_test(); if(!mpu_set_dmp_state(1)) { printf("mpu_set_dmp_state complete ......\n"); } else { printf("mpu_set_dmp_state come across error ......\n"); result |=0x0080; } return result; }
int mpu6050_init() { /* ** Configures I2C to Master mode to generate start ** condition on I2C bus and to transmit data at a ** speed of 100khz */ SetupI2C(); rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050"); rt_hw_interrupt_mask(SYS_INT_I2C0INT); int result = mpu_init(); if(!result) { rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) rt_kprintf("mpu_set_sensor complete ......\n"); else rt_kprintf("mpu_set_sensor come across error ......\n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo rt_kprintf("mpu_configure_fifo complete ......\n"); else rt_kprintf("mpu_configure_fifo come across error ......\n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate rt_kprintf("mpu_set_sample_rate complete ......\n"); else rt_kprintf("mpu_set_sample_rate error ......\n"); if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare rt_kprintf("dmp_load_motion_driver_firmware complete ......\n"); else rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation rt_kprintf("dmp_set_orientation complete ......\n"); else rt_kprintf("dmp_set_orientation come across error ......\n"); if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature rt_kprintf("dmp_enable_feature complete ......\n"); else rt_kprintf("dmp_enable_feature come across error ......\n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate rt_kprintf("dmp_set_fifo_rate complete ......\n"); else rt_kprintf("dmp_set_fifo_rate come across error ......\n"); run_self_test(); if(!mpu_set_dmp_state(1)) rt_kprintf("mpu_set_dmp_state complete ......\n"); else rt_kprintf("mpu_set_dmp_state come across error ......\n"); } while(1) { dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); /* Gyro and accel data are written to the FIFO by the DMP in chip * frame and hardware units. This behavior is convenient because it * keeps the gyro and accel outputs of dmp_read_fifo and * mpu_read_fifo consistent. */ /* if (sensors & INV_XYZ_GYRO ) send_packet(PACKET_TYPE_GYRO, gyro); if (sensors & INV_XYZ_ACCEL) send_packet(PACKET_TYPE_ACCEL, accel); */ /* Unlike gyro and accel, quaternions are written to the FIFO in * the body frame, q30. The orientation is set by the scalar passed * to dmp_set_orientation during initialization. */ if (sensors & INV_WXYZ_QUAT ) { q0=quat[0] / q30; q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30; Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw); UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100); delay_ms(10); } } 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; }