diff --git a/src/gps.c b/src/gps.c
--- a/src/gps.c
+++ b/src/gps.c
@@ -39,45 +39,84 @@ void gps_init()
     // Initialize serial port
    // done in poweron uart_init();
 
+	gps_poweron();
+	HAL_Delay(500);
 
-    // Disable GLONASS mode
-    uint8_t disable_glonass[20] = {0xB5, 0x62, 0x06, 0x3E, 0x0C, 0x00, 0x00, 0x00, 0x20, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01, 0x8F, 0xB2};
-
-    //gps_sendubx(disable_glonass, 20);
-    HAL_UART_Transmit(uart_gethandle(), disable_glonass, 20, 100);
+//	// uart1 ubx only
+//
+//	uint8_t setUBXuart1 = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25,
+//	0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9A, 0x79};
+//	HAL_UART_Transmit(uart_gethandle(), setUBXuart1, sizeof(setUBXuart1)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
+//
+//
+//	// uart0 ubx only
+//	uint8_t setUBXuart0 = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25,
+//	0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x99, 0x65};
+//	HAL_UART_Transmit(uart_gethandle(), setUBXuart0, sizeof(setUBXuart0)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
+//
+//
+//	// uart1 ubx only
+//
+//	uint8_t setUBXuart2 = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x02, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25,
+//	0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9B, 0x8D};
+//	HAL_UART_Transmit(uart_gethandle(), setUBXuart2, sizeof(setUBXuart2)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
 
-    // Enable power saving
-    uint8_t enable_powersave[10] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92};
-    //gps_sendubx(enable_powersave, 10);
-    HAL_UART_Transmit(uart_gethandle(), enable_powersave, 10, 100);
+	// Disable messages
+	uint8_t setGGA[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0XFF, 0X23};
+	HAL_UART_Transmit(uart_gethandle(), setGGA, sizeof(setGGA)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
+
+	uint8_t ackbuffer[10];
+	for(uint8_t i=0; i<10; i++)
+		ackbuffer[i] = 0xaa;
+	HAL_UART_Receive(uart_gethandle(), ackbuffer, 10, 100);
+
+	uint8_t setZDA[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X08, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X07, 0X5B};
+	HAL_UART_Transmit(uart_gethandle(), setZDA, sizeof(setZDA)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
 
+	uint8_t setGLL[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X01, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X2A};
+	HAL_UART_Transmit(uart_gethandle(), setGLL, sizeof(setGLL)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
 
-    // Set dynamic model 6 (<1g airborne platform)
-    uint8_t airborne_model[] = { 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };
-    //gps_sendubx(airborne_model, sizeof(airborne_model)/sizeof(uint8_t));
-    HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
+	uint8_t setGSA[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X02, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X01, 0X31};
+	HAL_UART_Transmit(uart_gethandle(), setGSA, sizeof(setGSA)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
+
+	uint8_t setGSV[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X03, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X02, 0X38};
+	HAL_UART_Transmit(uart_gethandle(), setGSV, sizeof(setGSV)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
+
+	uint8_t setRMC[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X04, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X03, 0X3F};
+	HAL_UART_Transmit(uart_gethandle(), setRMC, sizeof(setRMC)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
+
+	uint8_t setVTG[] = {0XB5, 0X62, 0X06, 0X01, 0X08, 0X00, 0XF0, 0X05, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X04, 0X46};
+	HAL_UART_Transmit(uart_gethandle(), setVTG, sizeof(setRMC)/sizeof(uint8_t), 100);
+	HAL_Delay(100);
 
 
-   // Disable messages
-   uint8_t setGLL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x2B};
-   HAL_UART_Transmit(uart_gethandle(), setGLL, sizeof(setGLL)/sizeof(uint8_t), 100);
-   HAL_Delay(1);
-
-   uint8_t setGSA[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x32};
-   HAL_UART_Transmit(uart_gethandle(), setGSA, sizeof(setGSA)/sizeof(uint8_t), 100);
-   HAL_Delay(1);
+//    // Disable GLONASS mode
+//    uint8_t disable_glonass[20] = {0xB5, 0x62, 0x06, 0x3E, 0x0C, 0x00, 0x00, 0x00, 0x20, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01, 0x8F, 0xB2};
+//
+//    //gps_sendubx(disable_glonass, 20);
+//    volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), disable_glonass, 20, 100);
+//
+//    // Enable power saving
+//    uint8_t enable_powersave[10] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92};
+//    //gps_sendubx(enable_powersave, 10);
+//    res = HAL_UART_Transmit(uart_gethandle(), enable_powersave, 10, 100);
+//
+//
+//    // Set dynamic model 6 (<1g airborne platform)
+//    uint8_t airborne_model[] = { 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };
+//    //gps_sendubx(airborne_model, sizeof(airborne_model)/sizeof(uint8_t));
+//    res = HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
 
-   uint8_t setGSV[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x39};
-   HAL_UART_Transmit(uart_gethandle(), setGSV, sizeof(setGSV)/sizeof(uint8_t), 100);
-   HAL_Delay(1);
 
-   uint8_t setRMC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x04, 0x40};
-   HAL_UART_Transmit(uart_gethandle(), setRMC, sizeof(setRMC)/sizeof(uint8_t), 100);
-   HAL_Delay(1);
-
-   uint8_t setVTG[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x46};
-   HAL_UART_Transmit(uart_gethandle(), setVTG, sizeof(setRMC)/sizeof(uint8_t), 100);
-   HAL_Delay(1);
 
 
 }
@@ -86,14 +125,25 @@ void gps_init()
 void gps_update_position()
 {
     // Request a NAV-POSLLH message from the GPS
-    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03,
-        0x0A};
+    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03, 0x0A};
+
+    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;
+    }
+
+
     //_gps_send_msg(request, 8);
     uint8_t flushed = uart_gethandle()->Instance->RDR;
-    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
+    volatile HAL_StatusTypeDef txres = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
 
     uint8_t buf[36];
-    HAL_UART_Receive(uart_gethandle(), buf, 36, 100);
+    for(uint8_t i =0; i<36; i++)
+    	buf[i] = 0xaa;
+    volatile HAL_StatusTypeDef result = HAL_UART_Receive(uart_gethandle(), buf, 36, 1000);
 
     //for(uint8_t i = 0; i < 36; i++)
     //    buf[i] = _gps_get_byte();
@@ -125,6 +175,16 @@ void gps_update_time(uint8_t* hour, uint
 {
     // Send a NAV-TIMEUTC message to the receiver
     uint8_t request[8] = {0xB5, 0x62, 0x01, 0x21, 0x00, 0x00, 0x22, 0x67};
+
+    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;
+    }
+
+
     uint8_t flushed = uart_gethandle()->Instance->RDR;
     volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
 
@@ -152,14 +212,17 @@ void gps_update_time(uint8_t* hour, uint
 void gps_check_lock(uint8_t* lock, uint8_t* sats)
 {
     // Construct the request to the GPS
-    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x06, 0x00, 0x00,
-        0x07, 0x16};
+    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x06, 0x00, 0x00, 0x07, 0x16};
     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[60];
-    HAL_UART_Receive(uart_gethandle(), buf, 60, 100);
+    for(uint8_t i=0; i<60; i++)
+    	buf[i] = 0xaa;
+    volatile HAL_StatusTypeDef res = HAL_UART_Receive(uart_gethandle(), buf, 60, 3000);
 
     // Verify the sync and header bits
 //    if( buf[0] != 0xB5 || buf[1] != 0x62 )
@@ -168,7 +231,9 @@ void gps_check_lock(uint8_t* lock, uint8
 //        led_set(LED_RED, 1);
 
     // Check 60 bytes minus SYNC and CHECKSUM (4 bytes)
-//    if( !_gps_verify_checksum(&buf[2], 56) ) led_set(LED_RED, 1);
+    if( !_gps_verify_checksum(&buf[2], 56) )
+        led_blink(2);
+
 
     // Return the value if GPSfixOK is set in 'flags'
     if( buf[17] & 0x01 )