/**************************************************************************************************
* Init MCU
**************************************************************************************************/
void Init_MCU() {

  // Set PORTB8/7 to Open Drain output mode
  GPIO_Config(&GPIOB_BASE, _GPIO_PINMASK_8, _GPIO_CFG_SPEED_50MHZ | _GPIO_CFG_OTYPE_OD | _GPIO_CFG_MODE_OUTPUT);
  GPIO_Config(&GPIOB_BASE, _GPIO_PINMASK_7, _GPIO_CFG_SPEED_50MHZ | _GPIO_CFG_OTYPE_OD | _GPIO_CFG_MODE_OUTPUT);

  GPIO_Digital_Output(&GPIOD_ODR, _GPIO_PINMASK_12);                            // LED GREEN
  GPIO_Digital_Output(&GPIOD_ODR, _GPIO_PINMASK_13);                            // LED ORANGE
  GPIO_Digital_Output(&GPIOD_ODR, _GPIO_PINMASK_14);                            // LED RED
  GPIO_Digital_Output(&GPIOD_BASE, _GPIO_PINMASK_15);                           // LED BLUE
  GPIO_Digital_Output(&GPIOC_ODR, _GPIO_PINMASK_5);                             // RF_STB
  GPIO_Digital_Output(&GPIOE_ODR, _GPIO_PINMASK_4);                             // RF_RST
  GPIO_Digital_Output(&GPIOA_IDR, _GPIO_PINMASK_0);                             // Button

  GPIO_Alternate_Function_Enable(&_GPIO_MODULE_USART2_PA23);
  UART2_Init_Advanced(19200, _UART_8_BIT_DATA, _UART_NOPARITY, _UART_ONE_STOPBIT, &_GPIO_MODULE_USART2_PA23);
  //****************************************************************************
/*UART3_Init_Advanced(9600, _UART_8_BIT_DATA, _UART_NOPARITY, _UART_ONE_STOPBIT, &_GPIO_MODULE_USART3_PB10_11);
  Delay_ms(100);
  USART3_CR1bits.RXNEIE = 1;       // enable uart rx interrupt
  NVIC_IntEnable(IVT_INT_USART3);  // enable interrupt vector
  Delay_ms(100);                        // Wait for UART module to stabilize*/
  //****************************************************************************
  UART6_Init_Advanced(19200, _UART_8_BIT_DATA, _UART_NOPARITY, _UART_ONE_STOPBIT, &_GPIO_MODULE_USART6_PC67 );
  Delay_ms(100);
  
  // Enable Usart Receiver interrupt:
  USART6_CR1bits.RXNEIE = 1;         // enable uart rx interrupt
  NVIC_IntEnable(IVT_INT_USART6);    // enable interrupt vector
  Enableinterrupts();
}
void main() {

  Start_TP();

  Init_GPIO();
  Init_SDIO();
  Init_Ext_Mem();
  Init_FAT();
  RTC_Init();
  
  I2C2_Init_Advanced(400000, &_GPIO_MODULE_I2C2_PF01);
  UART2_Init_Advanced(9600, _UART_8_BIT_DATA, _UART_NOPARITY, _UART_ONE_STOPBIT, &_GPIO_MODULE_USART2_PD5_PA3);
  MPU9150A_FSY = 0;
  MPU9150A_Init();
  MPU9150A_Detect();
  MAG_Detect();
  tmrTicks = 0;
  initTimer2();
  Timer2_On();
  MPU9150A_Read(); //initial read
  delay_ms(10);

  while (1) {
    DisableInterrupts();
    Check_TP();
    EnableInterrupts();
    //DrawScreen(&Boot);
    //DrawScreen(&Speedometer_graphics);
    Run_logger();

  }
}