#include "led.h"
#include "delay.h"
#include "key.h"
#include "sys.h"
#include "lcd.h"
#include "usart.h"
#include "mpu6050.h"
#include "usmart.h"   
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

int main(void)
{
        float pitch,roll,yaw;                
  short aacx,aacy,aacz;                  
        short gyrox,gyroy,gyroz;                                          
        NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);         
        uart_init(9600);                
        delay_init();       
        MPU_Init();                                       

                        MPU_Get_Accelerometer(&aacx,&aacy,&aacz);                       
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);       
        while(mpu_dmp_init())
        {
    printf("error%d ",mpu_dmp_init());
                delay_ms(200);
        }  
                while(1)
        {
       
                if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
                {
                       
                        MPU_Get_Accelerometer(&aacx,&aacy,&aacz);       
                        delay_ms(1);
                        MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);       
       
                printf("aacx:%d ",aacx);
                printf("aacy:%d ",aacy);
                printf("aacz:%d ",aacz);
                printf("gx:%d ",gyrox);
                printf("gy:%d ",gyroy);
                printf("gz:%d ",gyroz);
                printf("pitch:%f ",pitch);
                printf("roll:%f ",roll);
                printf("yaw:%f ",yaw);
                delay_ms(1000);
                }
               
        }        
}