Exemple #1
0
/* 
 * 函数名:main
 * 描述  : 主函数
 * 输入  :无
 * 输出  : 无
 */
int main(void)
{
	/* 配置系统时钟为 72M */ 
	SystemInit(); 
	LED_GPIO_Config();
	Nvic_Init();
	I2C_GPIO_Config();
	Delay(0xFFFF);
	InitMPU6050();
	Delay(0xFFFF);
  Init_HMC5883L();
	Delay(0xFFFF);
	Tim3_Init(2500);
	usart1_config();
	
  while(1)
  {
	    if(sentFlag)
			{
				 sentFlag = 0;								 
			   UART1_ReportIMU();
			}
	}
	  
}
Exemple #2
0
//----------------------------------------
static  void Task_Led1(void* p_arg)//负责计算姿态
{
   	GPIO_InitTypeDef GPIO_InitStructure;
   	(void) p_arg;
   	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC , ENABLE); 
   	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;				                 //LED1
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(GPIOC, &GPIO_InitStructure);	    
	while (1)
	{		
		
		
		//LED_N_STATUS(1,0);		
		UART1_ReportIMU(&EulerAngle_Current);//1.5ms
		//LED_N_STATUS(1,0);
		OSTimeDlyHMSM(0, 0, 0, 40);
	}
}
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;
}