/**************************************************************************
函数功能：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;
}
