diff --git a/inc/gps.h b/inc/gps.h
--- a/inc/gps.h
+++ b/inc/gps.h
@@ -6,10 +6,10 @@
 
 typedef struct _gps_data
 {
-    int32_t pdop;
-    int32_t sats_in_solution;
-    uint32_t speed;
-    //! int32_t heading;
+    uint32_t pdop;
+    uint8_t sats_in_solution;
+    int32_t speed;
+    int32_t heading;
 
     int32_t latitude;
     int32_t longitude;
@@ -27,7 +27,6 @@ typedef struct _gps_data
 } gps_data_t;
 
 
-void gps_init();
 void gps_update_data(void);
 
 uint8_t gps_check_nav(void);
@@ -40,5 +39,6 @@ void gps_acquirefix(void);
 uint8_t gps_getstate(void);
 
 gps_data_t* gps_getdata(void);
+uint8_t gps_ison(void);
 
 #endif /* GPS_H_ */
diff --git a/src/gps.c b/src/gps.c
--- a/src/gps.c
+++ b/src/gps.c
@@ -11,101 +11,13 @@
 
 
 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);
 
 
-// Initialize GPS module on startup
-void gps_init()
-{
-    // Initialize serial port
-   // done in poweron uart_init();
-
-	gps_poweron();
-
-//	// 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);
-
-	// 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);
-
-	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 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);
-
-
-
-
-}
-
-
 // Poll for fix data from the GPS and update the internal structure
 void gps_update_data(void)
 {
@@ -151,11 +63,21 @@ void gps_update_data(void)
 
     position.sats_in_solution = buf[6+23];
 
-    position.longitude = (buf[6+24] << 24) | (buf[6+25] << 16) | (buf[6+26] << 8) | (buf[6+27]);
-    position.latitude =  (buf[6+28] << 24) | (buf[6+29] << 16) | (buf[6+30] << 8) | (buf[6+31]);
-    position.altitude = (buf[6+36] << 24) | (buf[6+37] << 16) | (buf[6+38] << 8) | (buf[6+39]);
-    position.speed = (buf[6+60] << 24) | (buf[6+61] << 16) | (buf[6+62] << 8) | (buf[6+63]);
-    position.pdop = (buf[6+76] << 8) | (buf[6+77]);
+    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];
@@ -164,6 +86,8 @@ 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)
@@ -234,8 +158,68 @@ void gps_poweron(void)
     HAL_GPIO_WritePin(GPS_NOTEN, 0);
     uart_init();
 
+
+
+	// 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);
+
+	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 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);
+
+
+
+
+
     // Begin DMA reception
     //HAL_UART_Receive_DMA(uart_gethandle(), nmeaBuffer, NMEABUFFER_SIZE);
+
+    gpson = 1;
 }
 
 
@@ -245,6 +229,7 @@ void gps_poweroff(void)
     // NOTE: pchannel
     uart_deinit();
     HAL_GPIO_WritePin(GPS_NOTEN, 1);
+    gpson = 0;
 }
 
 gps_data_t* gps_getdata(void)
@@ -252,4 +237,9 @@ gps_data_t* gps_getdata(void)
     return &position;
 }
 
+uint8_t gps_ison(void)
+{
+    return gpson;
+}
+
 // vim:softtabstop=4 shiftwidth=4 expandtab 
diff --git a/src/main.c b/src/main.c
--- a/src/main.c
+++ b/src/main.c
@@ -33,7 +33,6 @@ int main(void)
     gpio_init();
     adc_init();
     i2c_init();
-    gps_init();
     wspr_init();
 
     uint32_t led_timer = HAL_GetTick();
@@ -53,7 +52,8 @@ int main(void)
         // Update fix status every 2 seconds
         if(HAL_GetTick() - fixinfo_timer > 2000)
         {
-        	gps_update_data();
+            if(gps_ison())
+                gps_update_data();
         	fixinfo_timer = HAL_GetTick();
         }
 
@@ -80,16 +80,14 @@ int main(void)
             {
                 blink_rate = BLINK_FAST;
 
-                // TODO: probably don't power on all the time, just on state transition
-//                gps_poweron();
-//                HAL_Delay(100);
-//                gps_update_position();
+                if(!gps_ison())
+                    gps_poweron(); // power on and initialize GPS module
                 
 
-                if(gps_getdata()->fixtype > 0)
+                if(gps_getdata()->fixtype > 0 && gps_getdata()->pdop < 5)
                 {
                     // Disable GPS module
-                    //gps_poweroff();
+                    gps_poweroff();
 
                     // TODO: Set RTC from GPS time