diff --git a/hardware/GPS.c b/hardware/GPS.c
index 62b547c..49f7d51 100644
--- a/hardware/GPS.c
+++ b/hardware/GPS.c
@@ -43,22 +43,28 @@ void gps_init(Serial *ser) {
 
 	if (gps_detect()) {
 		gps_installed = true;
-
-		serial_setbaudrate_9600(1);
-		delay_ms(100);
-
-		gps_send_command(PMTK_SET_BAUD_57600);
-		delay_ms(100);
-		
-		serial_setbaudrate_57600(1);
-		delay_ms(100);
-		
-		gps_send_command(PMTK_API_SET_FIX_CTL_1HZ);
-		gps_send_command(PMTK_SET_NMEA_OUTPUT_RMCGGA);
-
 		GPS_DDR |= _BV(GPS_EN_PIN);
-		if (gps_enabled()) { gps_powerup(); } else { gps_powerdown(); }
-		
+
+		if (config_gps_mode == CONFIG_GPS_AUTODETECT || config_gps_mode == CONFIG_GPS_REQUIRED) {
+			serial_setbaudrate_9600(1);
+			delay_ms(100);
+
+			gps_send_command(PMTK_SET_BAUD_57600);
+			delay_ms(100);
+			
+			serial_setbaudrate_57600(1);
+			delay_ms(100);
+			
+			gps_send_command(PMTK_API_SET_FIX_CTL_1HZ);
+			gps_send_command(PMTK_SET_NMEA_OUTPUT_RMCGGA);
+
+			gps_power = true;
+			gps_powerup();
+		} else {
+			gps_power = false;
+			gps_powerdown();
+		}
+
 	} else {
 		gps_installed = false;
 		gps_power = false;
diff --git a/hardware/Serial.c b/hardware/Serial.c
index 9d45250..c2240f3 100755
--- a/hardware/Serial.c
+++ b/hardware/Serial.c
@@ -83,6 +83,8 @@ ISR(USART0_RX_vect) {
         if (!fifo_isfull(&uart0FIFO)) {
             char c = uart0_getchar_nowait();
             fifo_push(&uart0FIFO, c);
+        } else {
+            uart1_getchar_nowait();
         }
     }
 }
@@ -92,6 +94,8 @@ ISR(USART1_RX_vect) {
         if (!fifo_isfull(&uart1FIFO)) {
             char c = uart1_getchar_nowait();
             fifo_push(&uart1FIFO, c);
+        } else {
+            uart1_getchar_nowait();
         }
     }
 }
diff --git a/main.c b/main.c
index 69010ce..08147b4 100755
--- a/main.c
+++ b/main.c
@@ -65,6 +65,9 @@ void system_check(void) {
         }
     }
 
+    // TODO: Check GPS_REQUIRED and BLUETOOTH_REQUIRED
+    // here before giving green light.
+
     // Give the green light if everything checks out
     LED_STATUS_ON();
 }