//

// GPS: communicate with ublox GPS module via ubx protocol

//

#include "stm32f0xx_hal.h"

#include "config.h"

#include "gpio.h"

#include "uart.h"

#include "gps.h"

volatile gps_data_t position;

uint8_t gpson = 0;

// Private methods

static void gps_ubx_checksum(uint8_t* data, uint8_t len, uint8_t* cka, uint8_t* ckb);

static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len);

// Poll for fix data from the GPS and update the internal structure

void gps_update_data(void)

{

    // Construct the request to the GPS

    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0xFF, 0xFF};

    volatile uint8_t check_a = 0;

    volatile uint8_t check_b = 0;

    for(uint8_t i = 2; i<6; i++)

    {

    	check_a += request[i];

    	check_b += check_a;

    }

    request[6] = check_a;

    request[7] = check_b;

    uint8_t flushed = uart_gethandle()->Instance->RDR;

    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    // Get the message back from the GPS

    uint8_t buf[100];

    for(uint8_t i=0; i<100; i++)

    	buf[i] = 0xaa;

    volatile HAL_StatusTypeDef res = HAL_UART_Receive(uart_gethandle(), buf, 100, 3000);

    // Check 60 bytes minus SYNC and CHECKSUM (4 bytes)

    if( !_gps_verify_checksum(&buf[2], 96) )

        led_blink(2);

    //volatile uint32_t gpstime_ms = (buf[6+0] << 24) | (buf[6+1] << 16) | buf[6+2] << 8) | (buf[6+3]);

    position.month = buf[6+6];

    position.day = buf[6+7];

    position.hour = buf[6+8];

    position.minute = buf[6+9];

    position.second = buf[6+10];

    position.valid = buf[6+11] & 0b1111;

    position.fixtype = buf[6+20];

    position.sats_in_solution = buf[6+23];

    position.longitude = (buf[6+24] << 0) | (buf[6+25] << 8) | (buf[6+26] << 16) | (buf[6+27] << 24); // degrees

    position.latitude =  (buf[6+28] << 0) | (buf[6+29] << 8) | (buf[6+30] << 16) | (buf[6+31] << 24); // degrees

    position.altitude = (buf[6+36] << 0) | (buf[6+37] << 8) | (buf[6+38] << 16) | (buf[6+39] << 24); // mm above sealevel

    position.altitude /= 1000; // mm => m

    position.speed = (buf[6+60] << 0) | (buf[6+61] << 8) | (buf[6+62] << 16) | (buf[6+63] << 24); // mm/second

    position.speed /= 1000; // mm/s -> m/s

    position.pdop = (buf[6+76] << 0) | (buf[6+77] << 8);

    position.pdop /= 100; // scale to dop units

    position.heading = (buf[6+84] << 0) | (buf[6+85] << 8) | (buf[6+86] << 16) | (buf[6+87] << 24); // mm above sealevel

    position.heading /= 100000; // 1e-5

//    // Return the value if GPSfixOK is set in 'flags'

//    if( buf[17] & 0x01 )

//        *lock = buf[16];

//    else

//        *lock = 0;

}

// TODO: Add data valid flag: invalidate data when GPS powered off

// Verify that the uBlox 6 GPS receiver is set to the <1g airborne navigaion mode.

uint8_t gps_check_nav(void)

{

    uint8_t request[8] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00,

        0x2A, 0x84};

    uint8_t flushed = uart_gethandle()->Instance->RDR;

    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    // Get the message back from the GPS

    uint8_t buf[44];

    HAL_UART_Receive(uart_gethandle(), buf, 44, 100);

//    // Verify sync and header bytes

//    if( buf[0] != 0xB5 || buf[1] != 0x62 )

//        led_set(LED_RED, 1);

//    if( buf[2] != 0x06 || buf[3] != 0x24 )

//        led_set(LED_RED, 1);

    // Check 40 bytes of message checksum

//    if( !_gps_verify_checksum(&buf[2], 40) ) led_set(LED_RED, 1);

    // Clock in and verify the ACK/NACK

    uint8_t ack[10];

//    for(uint8_t i = 0; i < 10; i++)

//        ack[i] = _gps_get_byte();

    HAL_UART_Receive(uart_gethandle(), ack, 10, 100);

#include "stm32f0xx_hal.h"

#include "uart.h"

#include "config.h"

#include "gpio.h"

UART_HandleTypeDef huart1;

DMA_HandleTypeDef hdma_usart1_rx;

DMA_HandleTypeDef hdma_usart1_tx;

uint8_t uart_initted = 0;

void uart_init(void)

{

    __GPIOB_CLK_ENABLE();

    __HAL_RCC_USART1_CLK_ENABLE();

    GPIO_InitTypeDef GPIO_InitStruct;

    // Init gpio pins for uart

    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;

    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;

    GPIO_InitStruct.Pull = GPIO_NOPULL; //GPIO_PULLUP;

    GPIO_InitStruct.Speed = GPIO_SPEED_LOW;

    GPIO_InitStruct.Alternate = GPIO_AF0_USART1;

    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    // Init UART periph

    huart1.Instance = USART1;

    huart1.Init.BaudRate = 9600;

    huart1.Init.WordLength = UART_WORDLENGTH_8B;

    huart1.Init.StopBits = UART_STOPBITS_1;

    huart1.Init.Parity = UART_PARITY_NONE;

    huart1.Init.Mode = UART_MODE_TX_RX;

    huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;

    huart1.Init.OverSampling = UART_OVERSAMPLING_16;

    huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;

    huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_RXOVERRUNDISABLE_INIT|UART_ADVFEATURE_DMADISABLEONERROR_INIT;

    huart1.AdvancedInit.OverrunDisable = UART_ADVFEATURE_OVERRUN_DISABLE;

    huart1.AdvancedInit.DMADisableonRxError = UART_ADVFEATURE_DMA_DISABLEONRXERROR;

    HAL_UART_Init(&huart1);

    HAL_Delay(100);

	uint8_t switch_baud[] = "$PUBX,41,1,0003,0001,115200,0*1E\r\n";

	HAL_UART_Transmit(uart_gethandle(), switch_baud, sizeof(switch_baud)/sizeof(uint8_t), 1000);

    HAL_UART_DeInit(&huart1);

    huart1.Init.BaudRate = 115200;

    HAL_UART_Init(&huart1);

//

//    __DMA1_CLK_ENABLE();

//

//  // Init UART DMA

//    hdma_usart1_rx.Instance = DMA1_Channel3;

//    hdma_usart1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;

//    hdma_usart1_rx.Init.PeriphInc = DMA_PINC_DISABLE;

//    hdma_usart1_rx.Init.MemInc = DMA_MINC_ENABLE;

//    hdma_usart1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;

//    hdma_usart1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;

//    hdma_usart1_rx.Init.Mode = DMA_CIRCULAR;

//    hdma_usart1_rx.Init.Priority = DMA_PRIORITY_LOW;

//    HAL_DMA_Init(&hdma_usart1_rx);

//

//    __HAL_LINKDMA(&huart1,hdmarx,hdma_usart1_rx);

//

//    hdma_usart1_tx.Instance = DMA1_Channel2;

//    hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;

//    hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;

//    hdma_usart1_tx.Init.MemInc = DMA_MINC_DISABLE;

//    hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;

//    hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;

//    hdma_usart1_tx.Init.Mode = DMA_NORMAL;

//    hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;

//    HAL_DMA_Init(&hdma_usart1_tx);

//

//    __HAL_LINKDMA(&huart1,hdmatx,hdma_usart1_tx);

//

////    HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 0, 0);

////    HAL_NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);

    HAL_NVIC_SetPriority(USART1_IRQn, 0, 0);

    //HAL_NVIC_EnableIRQ(USART1_IRQn);

    HAL_NVIC_DisableIRQ(USART1_IRQn);

    uart_initted = 1;

}

void uart_deinit(void)

{

    if(uart_initted == 1)

    {

        HAL_UART_DeInit(&huart1);

        uart_initted = 0;

    }

}

UART_HandleTypeDef* uart_gethandle(void)

{

    return &huart1;

} 

DMA_HandleTypeDef* uart_get_txdma_handle(void)

{

    return &hdma_usart1_tx;

}

DMA_HandleTypeDef* uart_get_rxdma_handle(void)

{

    return &hdma_usart1_rx;

}