diff --git a/src/gps.c b/src/gps.c
--- a/src/gps.c
+++ b/src/gps.c
@@ -40,19 +40,45 @@ void gps_init()
    // done in poweron uart_init();
 
 
-//    // 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);
-//
-//    // Enable power saving
-//    uint8_t enable_powersave[10] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92};
-//    gps_sendubx(enable_powersave, 10);
-//
-//
-//    // 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));
+    // 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);
+
+    // 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);
+
+
+    // 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);
+
+
+   // 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);
+
+   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);
+
 
 }
 
@@ -98,14 +124,13 @@ void gps_update_position()
 void gps_update_time(uint8_t* hour, uint8_t* minute, uint8_t* second)
 {
     // Send a NAV-TIMEUTC message to the receiver
-    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x21, 0x00, 0x00,
-        0x22, 0x67};
+    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x21, 0x00, 0x00, 0x22, 0x67};
     uint8_t flushed = uart_gethandle()->Instance->RDR;
-    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
+    volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
 
     // Get the message back from the GPS
     uint8_t buf[28];
-    HAL_UART_Receive(uart_gethandle(), buf, 28, 100);
+    res = HAL_UART_Receive(uart_gethandle(), buf, 28, 500);
 
 //    // Verify the sync and header bits
 //    if( buf[0] != 0xB5 || buf[1] != 0x62 )
@@ -180,8 +205,10 @@ uint8_t gps_check_nav(void)
 
     // Clock in and verify the ACK/NACK
     uint8_t ack[10];
-    for(uint8_t i = 0; i < 10; i++)
-        ack[i] = _gps_get_byte();
+//    for(uint8_t i = 0; i < 10; i++)
+//        ack[i] = _gps_get_byte();
+
+    HAL_UART_Receive(uart_gethandle(), ack, 10, 100);
 
     // If we got a NACK, then return 0xFF
     if( ack[3] == 0x00 ) return 0xFF;