//文件名称：main_ins.c
//功能描述：主函数，硬件初始化，解算函数

#include "System_Config.h"


#ifdef __GNUC__
  #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
  #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif


uint16_t cmd;
char D_data[4];
extern int calflag;

int main(void)
{       
  int i=0;
  int initaccgg[3]={0};
  int accgg[3]={0};
  int  magicdataa[4]={0};
  float compss[3]={0};
  float agg[3]={0};
  
  Driver_Init();  
  
  printf("AHRS hardware config OK \r\n");
  MPU6050GyroInit();	
  MPU6050AccInit(); 
  InitCmp();
  MPU6050ReadAcc(initaccgg);        
  cmd = UART_Res();
  
  if(cmd==5)//0x05，标定开始命令，通过
  {
    while(1)
    {
      for(i=0;i<16;i++)
      {
              
         MPU6050ReadAcc(accgg);
        
         agg[0]+=accgg[0];
         agg[1]+=accgg[1];
         agg[2]+=accgg[2];
      }
      agg[0]*=0.0625;
      agg[1]*=0.0625;
      agg[2]*=0.0625;
      ReadCmpOut(&magicdataa[0],&magicdataa[1], &magicdataa[2]);
      
      compss[0]=magicdataa[0]; 
      compss[1]=magicdataa[1];
      compss[2]=magicdataa[2];
      printf("%.3f %.3f %.3f %.3f %.3f %.3f\r\n",agg[0],agg[1],agg[2],compss[0],compss[1],compss[2]);
    }
  }
  
  if(cmd==35)//0x23，数据输出命令，通过
  {
    //caflag=1;
    printf("AHRS Start \r\n");
    mpu_calibration();       
    Timer_Config();   
    NVIC_Config();
    while(1)
    {
      if(calflag==1)
      {
        cal();
        calflag=0;
      }
      else
      {
      }
    }
  }
}     
  

PUTCHAR_PROTOTYPE
{

  USART_SendData(USART1, (uint8_t) ch);
  while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET)
  {}
  return ch;
}