diff --git a/Source/gps.c b/Source/gps.c
--- a/Source/gps.c
+++ b/Source/gps.c
@@ -77,12 +77,12 @@ void gps_update_data(void)
     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.speed /= 10; // mm/s -> cm/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 = (buf[6+64] << 0) | (buf[6+65] << 8) | (buf[6+66] << 16) | (buf[6+67] << 24);
     position.heading /= 100000; // 1e-5
 
 //    // Return the value if GPSfixOK is set in 'flags'
@@ -93,44 +93,6 @@ void gps_update_data(void)
 
 }
 
-// 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);
-
-    // If we got a NACK, then return 0xFF
-    if( ack[3] == 0x00 ) return 0xFF;
-
-    // Return the navigation mode and let the caller analyse it
-    return buf[8];
-}
-
 
 // Verify the checksum for the given data and length.
 static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len)
@@ -202,22 +164,21 @@ void gps_poweron(void)
 
 //    // 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);
+//    HAL_UART_Transmit(uart_gethandle(), disable_glonass, sizeof(disable_glonass)/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);
-//    res = HAL_UART_Transmit(uart_gethandle(), enable_powersave, 10, 100);
+//    HAL_UART_Transmit(uart_gethandle(), enable_powersave, sizeof(enable_powersave)/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));
-//    res = HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
-
-
+//    HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
+//
+//